diff --git a/src/controller/execute.jl b/src/controller/execute.jl index c14527385..490081b62 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -144,7 +144,7 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) """ function getinfo(mpc::PredictiveController{NT}) where NT<:Real model, buffer, transcription = mpc.estim.model, mpc.buffer, mpc.transcription - nΔŨ, nK = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nk + nΔŨ, nK = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nk̄ nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp Z̃ = mpc.Z̃ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1aba52099..3b3ae1594 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -584,12 +584,12 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real hess = mpc.hessian transcription = mpc.transcription nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(mpc.con.i_g), sum(mpc.con.i_g) nc, neq = con.nc, con.neq - nU, nŶ, nX̂, nK = mpc.Hp*nu, Hp*ny, Hp*nx̂, Hp*nk + nU, nŶ, nX̂, nK = mpc.Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny ΔŨ = zeros(NT, nΔŨ) x̂0end = zeros(NT, nx̂) @@ -823,11 +823,11 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J transcription = mpc.transcription grad, hess = mpc.gradient, mpc.hessian nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc ng = length(mpc.con.i_g) nc, neq = mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk + nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN = convert(JNT, NaN) @@ -949,12 +949,12 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN transcription = mpc.transcription jac, hess = mpc.jacobian, mpc.hessian nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(mpc.con.i_g), sum(mpc.con.i_g) nc, neq = mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk + nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0bba4380c..041de5ca5 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -516,7 +516,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ - \mathbf{E_{S}^{k}} &= \begin{bmatrix} + \mathbf{E_{S}^{k̄}} &= \begin{bmatrix} \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ @@ -524,7 +524,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H \vdots & \vdots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k}} \end{bmatrix} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k̄}} \end{bmatrix} \\ \mathbf{K_S} &= \begin{bmatrix} λ_o\mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{A_s} \\ @@ -558,7 +558,7 @@ function init_defectmat_orthocolloc( Hp, Hc, Co, λo, As, nxs ) where {NT<:Real} nu, nx, nd, nx̂ = model.nu, model.nx, model.nd, estim.nx̂ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) λo_I = λo*I(nx) # --- current state estimates x̂0 --- KS = zeros(NT, nx̂*Hp, nx̂) @@ -577,8 +577,8 @@ function init_defectmat_orthocolloc( ESx̂[iRow_λo, iCol_λo] = λo_I ESx̂[iRow_As, iCol_As] = As end - ESk = repeatdiag([Co; zeros(NT, nxs, nk)], Hp) - ES = [ESΔu ESx̂ ESk] + ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], Hp) + ES = [ESΔu ESx̂ ESk̄] # --- current measured disturbances d0 and predictions D̂0 --- GS = zeros(NT, nx̂*Hp, nd) JS = zeros(NT, nx̂*Hp, nd*Hp) @@ -1027,18 +1027,18 @@ It warm-starts the solver at: \mathbf{x̂_0}(k+H_p-2|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{k}(k+0|k-1) \\ - \mathbf{k}(k+1|k-1) \\ + \mathbf{k̄}(k+0|k-1) \\ + \mathbf{k̄}(k+1|k-1) \\ \vdots \\ - \mathbf{k}(k+H_p-3|k-1) \\ - \mathbf{k}(k+H_p-2|k-1) \\ - \mathbf{k}(k+H_p-2|k-1) \\ + \mathbf{k̄}(k+H_p-3|k-1) \\ + \mathbf{k̄}(k+H_p-2|k-1) \\ + \mathbf{k̄}(k+H_p-2|k-1) \\ ϵ_{k-1} \end{bmatrix} ``` where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. The vector ``\mathbf{k}(k+j|k-1)`` include the ``n_o`` intermediate +``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` include the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at the last control period. """ function set_warmstart_mpc!( @@ -1046,8 +1046,8 @@ function set_warmstart_mpc!( ) nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ Hp, Hc, Z̃s = mpc.Hp, mpc.Hc, mpc.buffer.Z̃ - nk = get_nk(mpc.estim.model, transcription) - nΔU, nX̂, nK = nu*Hc, nx̂*Hp, nk*Hp + nk̄ = get_nk̄(mpc.estim.model, transcription) + nΔU, nX̂, nK = nu*Hc, nx̂*Hp, nk̄*Hp # --- input increments ΔU --- Z̃s[1:(nΔU-nu)] .= @views mpc.Z̃[(nu+1):(nΔU)] Z̃s[(nΔU-nu+1):(nΔU)] .= 0 @@ -1055,8 +1055,8 @@ function set_warmstart_mpc!( Z̃s[(nΔU+1):(nΔU+nX̂-nx̂)] .= @views mpc.Z̃[(nΔU+nx̂+1):(nΔU+nX̂)] Z̃s[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] .= @views mpc.Z̃[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] # --- collocation points K --- - Z̃s[(nΔU+nX̂+1):(nΔU+nX̂+nK-nk)] .= @views mpc.Z̃[(nΔU+nX̂+nk+1):(nΔU+nX̂+nK)] - Z̃s[(nΔU+nX̂+nK-nk+1):(nΔU+nX̂+nK)] .= @views mpc.Z̃[(nΔU+nX̂+nK-nk+1):(nΔU+nX̂+nK)] + Z̃s[(nΔU+nX̂+1):(nΔU+nX̂+nK-nk̄)] .= @views mpc.Z̃[(nΔU+nX̂+nk̄+1):(nΔU+nX̂+nK)] + Z̃s[(nΔU+nX̂+nK-nk̄+1):(nΔU+nX̂+nK)] .= @views mpc.Z̃[(nΔU+nX̂+nK-nk̄+1):(nΔU+nX̂+nK)] # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1169,16 +1169,16 @@ function predict!( mpc::PredictiveController, model::NonLinModel, ::SingleShooting, U0, _ ) - nu, nx̂, ny, nd, nk, Hp = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nk, mpc.Hp + nu, nx̂, ny, nd, nk̄, Hp = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nk̄, mpc.Hp D̂0 = mpc.D̂0 x̂0 = @views mpc.estim.x̂0[1:nx̂] d̂0 = @views mpc.d0[1:nd] for j=1:Hp u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k = @views K[(1 + nk*(j-1)):(nk*j)] + k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k, mpc.estim, model, x̂0, u0, d̂0) + f̂!(x̂0next, û0, k̄, mpc.estim, model, x̂0, u0, d̂0) x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d̂0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1338,7 +1338,7 @@ function con_nonlinprogeq!( mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) - nx̂, nx, nu, nd, nk = mpc.estim.nx̂, model.nx, model.nu, model.nd, model.nk + nx̂, nx, nu, nd, nk̄ = mpc.estim.nx̂, model.nx, model.nu, model.nd, model.nk̄ Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads @@ -1354,11 +1354,11 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k = @views K[(1 + nk*(j-1)):(nk*j)] + k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - f!(x̂dnext, k, model, x̂d_Z̃, û0, d̂0, model.p) + f!(x̂dnext, k̄, model, x̂d_Z̃, û0, d̂0, model.p) ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end return geq @@ -1400,7 +1400,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads Ts = model.Ts - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @@ -1413,7 +1413,7 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] + k̇ = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] @@ -1423,7 +1423,7 @@ function con_nonlinprogeq!( # last iteration (j-1) may not be executed (iterations are re-orderable) model.f!(k̇1, x̂d_Z̃, û0, d̂0, model.p) else - k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̇[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 end if h < 1 model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, model.p) @@ -1478,9 +1478,9 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads Mo, no, τ = mpc.Mo, transcription.no, transcription.τ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) D̂0 = mpc.D̂0 - X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] + X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk̄*Hp)] D̂temp = mpc.buffer.D̂ Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @threadsif f_threads for j=1:Hp @@ -1492,14 +1492,14 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] - k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] + k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - ŝk = @views geq[(1 + nk*(j-1)):(nk*j)] + ŝk = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] # ----------------- collocation constraint defects ----------------------------- - Δk = k̇ + Δk = k̄dot for i=1:no - Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ + Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end mul!(ŝk, Mo, Δk) d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)] @@ -1507,8 +1507,8 @@ function con_nonlinprogeq!( ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i = @views k̇[(1 + (i-1)*nx):(i*nx)] - ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] + k̇i = @views k̄dot[(1 + (i-1)*nx):(i*nx)] + ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next if h < 1 model.f!(k̇i, ki_Z̃, û0, d̂i, model.p) @@ -1519,7 +1519,7 @@ function con_nonlinprogeq!( model.f!(k̇i, ki_Z̃, ûi, d̂i, model.p) end end - ŝk .-= k̇ + ŝk .-= k̄dot end return geq end diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index c7f187787..7b258fe9c 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -1,7 +1,7 @@ struct StateEstimatorBuffer{NT<:Real} u ::Vector{NT} û ::Vector{NT} - k ::Vector{NT} + k̄ ::Vector{NT} x̂ ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} @@ -21,7 +21,7 @@ end @doc raw""" StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0 + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0 He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) @@ -31,15 +31,15 @@ Create a buffer for `StateEstimator` objects for estimated states and measured o The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0, + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0, He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) nV̂, nŴ, nX̂, nŶ, nD = nym*He, nŵ*He, nx̂*He, ny*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) - k = Vector{NT}(undef, nk) + k̄ = Vector{NT}(undef, nk̄) x̂ = Vector{NT}(undef, nx̂) Z̃ = Vector{NT}(undef, nZ̃) V̂ = Vector{NT}(undef, nV̂) @@ -56,7 +56,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 098dff1b0..ac65017de 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -31,7 +31,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm ) where {NT<:Real, SM<:SimModel} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) validate_internalmodel(model, nym, Csm, Dsm) As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm) @@ -46,7 +46,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} ŷs = zeros(NT, ny) direct = true # InternalModel always uses direct transmission from ym prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM}( model, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext, @@ -302,8 +302,8 @@ function update_estimate!(estim::InternalModel, u0, _ , d0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k - f̂!(x̂dnext, û0, k, estim, estim.model, x̂d, u0, d0) + x̂dnext, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ + f̂!(x̂dnext, û0, k̄, estim, estim.model, x̂d, u0, d0) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index bd9c872f0..52d573d77 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -35,7 +35,7 @@ struct SteadyKalmanFilter{ function SteadyKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -47,7 +47,7 @@ struct SteadyKalmanFilter{ cov.P̂ .= P̂ x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM, KC}( model, cov, @@ -342,7 +342,7 @@ struct KalmanFilter{ function KalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -352,7 +352,7 @@ struct KalmanFilter{ x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM, KC}( model, cov, @@ -568,7 +568,7 @@ struct UnscentedKalmanFilter{ function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true ) where {NT<:Real, SM<:SimModel{NT}, KC<:KalmanCovariances} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -582,7 +582,7 @@ struct UnscentedKalmanFilter{ X̂0, X̄0 = zeros(NT, nx̂, nσ), zeros(NT, nx̂, nσ) Ŷ0m, Ȳ0m = zeros(NT, nym, nσ), zeros(NT, nym, nσ) prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM, KC}( model, cov, @@ -877,7 +877,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - x̂0next, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k + x̂0next, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ # in-place operations to reduce allocations: P̂corr_temp = Hermitian(estim.buffer.P̂, :L) P̂corr_temp .= P̂corr @@ -890,7 +890,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) X̂0next = X̂0corr for j in axes(X̂0next, 2) @views x̂0corr .= X̂0corr[:, j] - @views f̂!(X̂0next[:, j], û0, k, estim, estim.model, x̂0corr, u0, d0) + @views f̂!(X̂0next[:, j], û0, k̄, estim, estim.model, x̂0corr, u0, d0) end x̂0next .= mul!(x̂0corr, X̂0next, m̂) X̄0next = estim.X̄0 @@ -958,7 +958,7 @@ struct ExtendedKalmanFilter{ FF<:Function, HF<:Function } - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -970,7 +970,7 @@ struct ExtendedKalmanFilter{ F̂_û, F̂ = zeros(NT, nx̂+nu, nx̂), zeros(NT, nx̂, nx̂) Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM, KC, JB, FF, HF}( model, cov, @@ -1114,7 +1114,7 @@ objects with the linearization points. """ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nx̂ = model.nx + size(As, 1) x̂op = f̂op = zeros(nx̂) # not important for Jacobian computations function f̂_ekf!(x̂0next, x̂0, û0, k, u0, d0) @@ -1126,15 +1126,15 @@ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) û0 = Cache(zeros(NT, nu)) - k = Cache(zeros(NT, nk)) + k̄ = Cache(zeros(NT, nk̄)) cst_u0 = Constant(rand(NT, nu)) cst_d0 = Constant(rand(NT, nd)) F̂prep = prepare_jacobian( - f̂_ekf!, x̂0next, jacobian, x̂0, û0, k, cst_u0, cst_d0; strict + f̂_ekf!, x̂0next, jacobian, x̂0, û0, k̄, cst_u0, cst_d0; strict ) Ĥprep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) function linfuncF̂!(F̂, x̂0next, backend, x̂0, cst_u0, cst_d0) - return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k, cst_u0, cst_d0) + return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k̄, cst_u0, cst_d0) end function linfuncĤ!(Ĥ, ŷ0, backend, x̂0, cst_d0) return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥprep, backend, x̂0, cst_d0) @@ -1275,9 +1275,9 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ Q̂ = estim.cov.Q̂ - x̂0next, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k + x̂0next, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ # in-place operations to reduce allocations: - f̂!(x̂0next, û0, k, estim, estim.model, x̂0corr, u0, d0) + f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ mul!(P̂corr_Âᵀ, P̂corr, Â') Â_P̂corr_Âᵀ = estim.buffer.Q̂ diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 256f0bd0f..56a4a4155 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -27,7 +27,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function Luenberger{NT, SM}( model, i_ym, nint_u, nint_ym, poles; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) validate_luenberger(model, nint_u, nint_ym, poles) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -42,7 +42,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} end x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] prepared = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM}( model, x̂op, f̂op, x̂0, diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index e5b0e2d79..f60f4d043 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -26,7 +26,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function ManualEstimator{NT}( model::SM, i_ym, nint_u, nint_ym ) where {NT<:Real, SM<:SimModel{NT}} - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -36,7 +36,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] direct = false prepared = [true] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄) return new{NT, SM}( model, x̂op, f̂op, x̂0, diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 492144229..2e22e49eb 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -170,7 +170,7 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator{NT} } nu, ny, nd = model.nu, model.ny, model.nd - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) @@ -201,7 +201,7 @@ struct MovingHorizonEstimator{ ES, GS, JS, BS, gc!, nc ) - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) @@ -216,7 +216,7 @@ struct MovingHorizonEstimator{ Nk = [0] prepared = [false] test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nŵ, nε, transcription) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄, He, nŵ, nε, transcription) estim = new{NT, SM, KC, TM, JM, GB, JB, HB, PT, GCfunc, CE}( model, transcription, optim, con, gradient, jacobian, hessian, @@ -1078,8 +1078,8 @@ and ``\mathbf{0}`` is properly sized for the `transcription` instance. function init_ZtoŴ( model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} - nk = get_nk(model, transcription) - nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, transcription) + nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) Tŵ = [spzeros(NT, nŴ, nZ-nŴ) I] return Tŵ end @@ -1305,8 +1305,8 @@ function init_boxconstraint_mhe( x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, A_x̂min, A_x̂max, C_x̂min, C_x̂max, A_Ŵmin, A_Ŵmax ) where {NT<:Real} - nk = get_nk(model, transcription) - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, transcription) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) Z̃min, Z̃max = fill(convert(NT,-Inf), nZ̃), fill(convert(NT,+Inf), nZ̃) nε > 0 && (Z̃min[begin] = 0) nŴ = nŵ*He @@ -1426,11 +1426,11 @@ function get_nonlinobj_op( model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu - nk = get_nk(model, estim.transcription) + nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - nK, nU, nŶ = He*nk, He*nu, He*nŷ + nK, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN = convert(JNT, NaN) @@ -1542,13 +1542,13 @@ function get_nonlincon_oracle( model, con = estim.model, estim.con jac, hess = estim.jacobian, estim.hessian nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu - nk = get_nk(model, estim.transcription) + nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) - nK, nU, nŶ = He*nk, He*nu, He*nŷ + nK, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d8a052f48..5593586b7 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -145,13 +145,13 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3) """ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ Z̃ = estim.Z̃ info = Dict{Symbol, Any}() V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ x̂0arr = buffer.x̂ - x̄, Û0, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk*Nk) + x̄, Û0, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) x̂0arr = getarrival!(x̂0arr, estim, Z̃) Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) @@ -214,13 +214,13 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher optim, con = estim.optim, estim.con hess = estim.hessian nx̂, nym, nŷ, nu, nc = estim.nx̂, estim.nym, model.ny, model.nu, con.nc - nk = get_nk(model, estim.transcription) + nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) nV̂, nX̂, nŴ = He*nym, He*nx̂, He*nx̂ - nK, nU, nŶ = He*nk, He*nu, He*nŷ + nK, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) Ŵ = zeros(NT, nŴ) @@ -422,8 +422,8 @@ function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT< fx̄, r = estim.fx̄, estim.r nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] nYm = estim.nym*Nk - nk = get_nk(model, estim.transcription) - nZ = get_nZ_mhe(estim.transcription, Nk, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, estim.transcription) + nZ = get_nZ_mhe(estim.transcription, Nk, nx̂, nk̄, nŵ) # --- truncate vectors and matrices if Nk < He --- U0, D0, Y0m = trunc_windows(estim) Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim) @@ -477,8 +477,8 @@ getx̄!(x̄, estim::MovingHorizonEstimator, x̂0arr) = (x̄ .= estim.x̂0arr_old "Get the estimated process noise from the decision vector `Z̃`." function getŴ!(Ŵ, estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃) He, nx̂, nŵ = estim.He, estim.nx̂, estim.nx̂ - nk = get_nk(estim.model, transcription) - nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(estim.model, transcription) + nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) Ŵ .= @views Z̃[(nZ̃ - nŵ*He + 1):end] return Ŵ end @@ -634,10 +634,10 @@ otherwise the state is for the next time step. """ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real model, buffer = estim.model, estim.buffer - nu, nk, nx̂, Nk = model.nu, model.nk, estim.nx̂, estim.Nk[] + nu, nk̄, nx̂, Nk = model.nu, model.nk̄, estim.nx̂, estim.Nk[] x̂0arr = buffer.x̂ V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk*Nk) # TODO: remove the 2 allocations + Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getarrival!(x̂0arr, estim, Z̃) predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index d2fbfe46a..ff28ead4f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,7 +1,7 @@ "Get the number of elements in the optimization decision vector `Z`" get_nZ_mhe(::SingleShooting, He, nx̂, _ , nŵ) = nx̂ + nŵ*He get_nZ_mhe(::TranscriptionMethod, He, nx̂, _ , nŵ) = nx̂ + nx̂*He + nŵ*He -get_nZ_mhe(::OrthogonalCollocation, He, nx̂, nk, nŵ) = nx̂ + nx̂*He + nk*He + nŵ*He +get_nZ_mhe(::OrthogonalCollocation, He, nx̂, nk̄, nŵ) = nx̂ + nx̂*He + nk̄*He + nŵ*He "Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) @@ -17,11 +17,11 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nk = get_nk(estim.model, transcription) - nŴ, nX̂, nK = nŵ*Nk, nx̂*Nk, nk*Nk + nk̄ = get_nk̄(estim.model, transcription) + nŴ, nX̂, nK = nŵ*Nk, nx̂*Nk, nk̄*Nk nx̃ = estim.nε + nx̂ nx̃_nX̂_He = nx̃ + nx̂*estim.He - nx̃_nX̂_nK_He = nx̃_nX̂_He + nk*estim.He + nx̃_nX̂_nK_He = nx̃_nX̂_He + nk̄*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); (1 + nx̃_nX̂_He):(nx̃_nX̂_He + nK); @@ -381,8 +381,8 @@ function init_predmat_mhe( ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ - nk = get_nk(model, transcription) - nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) E = zeros(NT, 0, nZ) ex̄ = [-I zeros(NT, nx̂, nZ - nx̂)] EX̂ = zeros(NT, 0, nZ) @@ -409,8 +409,8 @@ function init_predmat_mhe( ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ - nk = get_nk(model, transcription) - nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) E = zeros(NT, 0, nZ) ex̄ = [-I zeros(NT, nx̂, nZ - nx̂)] EX̂ = [zeros(NT, nx̂*He, nx̂) I zeros(NT, nx̂*He, nZ - nx̂ - nx̂*He)] @@ -598,7 +598,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ - \mathbf{E_S^k} &= \begin{bmatrix} + \mathbf{E_S^k̄} &= \begin{bmatrix} \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ @@ -607,7 +607,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{E_S^ŵ} &= \mathbf{I} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^k} & \mathbf{E_S^ŵ} \end{bmatrix} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^k̄} & \mathbf{E_S^ŵ} \end{bmatrix} \\ \end{aligned} ``` """ @@ -617,7 +617,7 @@ function init_defectmat_mhe( ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) λo_I = λo*I(nx) ESx̂ = [zeros(NT, nx̂*He, nx̂) -I] for j=1:He @@ -628,9 +628,9 @@ function init_defectmat_mhe( iCol = (nx+1:nx̂) .+ (j-1)*nx̂ ESx̂[iRow, iCol] = As end - ESk = repeatdiag([Co; zeros(NT, nxs, nk)], He) + ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], He) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented - ES = [ESx̂ ESk ESŵ] + ES = [ESx̂ ESk̄ ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -651,8 +651,8 @@ function init_defectmat_mhe_empty( model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} nu, nd = model.nu, model.nd - nk = get_nk(model, transcription) - nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) + nk̄ = get_nk̄(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) ES = zeros(NT, 0, nZ) GS = zeros(NT, 0, nu*He) JS = zeros(NT, 0, nd*(He+1)) @@ -1060,7 +1060,7 @@ function set_warmstart_mhe!( ) where NT<:Real model, buffer = estim.model, estim.buffer nu = model.nu - nk = get_nk(estim.model, transcription) + nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ = nε + nx̂, nŵ*He Z̃s = estim.buffer.Z̃ @@ -1074,7 +1074,7 @@ function set_warmstart_mhe!( # --- verify definiteness of objective function --- x̄ = buffer.x̂ V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk*Nk) # TODO: remove the 2 allocations + Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) @@ -1112,12 +1112,12 @@ It warm-starts the solver at: \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{0_x̂} \\ - \mathbf{k}(k-N_k+p+0|k-1) \\ - \mathbf{k}(k-N_k+p+1|k-1) \\ + \mathbf{k̄}(k-N_k+p+0|k-1) \\ + \mathbf{k̄}(k-N_k+p+1|k-1) \\ \vdots \\ - \mathbf{k}(k+p-3|k-1) \\ - \mathbf{k}(k+p-2|k-1) \\ - \mathbf{k}(k+p-2|k-1) \\ + \mathbf{k̄}(k+p-3|k-1) \\ + \mathbf{k̄}(k+p-2|k-1) \\ + \mathbf{k̄}(k+p-2|k-1) \\ \mathbf{0_k} \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ @@ -1130,7 +1130,7 @@ It warm-starts the solver at: ``` where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. The vector ``\mathbf{k}(k-j|k-1)`` include the ``n_o`` intermediate +``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` include the ``n_o`` intermediate stage predictions for the interval ``k-j``, and is also computed at the last control period. See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for the defintion of vectors ``\mathbf{0_x̂}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. @@ -1140,9 +1140,9 @@ function set_warmstart_mhe!( ) where NT<:Real model, buffer = estim.model, estim.buffer nu = model.nu - nk = get_nk(estim.model, transcription) + nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nK = nε + nx̂, nŵ*He, nx̂*He, nk*He + nx̃, nŴ, nX̂, nK = nε + nx̂, nŵ*He, nx̂*He, nk̄*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) @@ -1152,15 +1152,15 @@ function set_warmstart_mhe!( Z̃s[(nx̃+1):(nx̃+nX̂-nx̂)] .= @views estim.Z̃[(nx̃+nx̂+1):(nx̃+nX̂)] Z̃s[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] .= @views estim.Z̃[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] # --- collocation points K --- - Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nK-nk)] .= @views estim.Z̃[(nx̃+nX̂+nk+1):(nx̃+nX̂+nK)] - Z̃s[(nx̃+nX̂+nK-nk+1):(nx̃+nX̂+nK)] .= @views estim.Z̃[(nx̃+nX̂+nK-nk+1):(nx̃+nX̂+nK)] + Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nK-nk̄)] .= @views estim.Z̃[(nx̃+nX̂+nk̄+1):(nx̃+nX̂+nK)] + Z̃s[(nx̃+nX̂+nK-nk̄+1):(nx̃+nX̂+nK)] .= @views estim.Z̃[(nx̃+nX̂+nK-nk̄+1):(nx̃+nX̂+nK)] # --- process noise estimates Ŵ --- Z̃s[(nx̃+nX̂+nK+1):(nx̃+nX̂+nK+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nK+nŵ+1):(nx̃+nX̂+nK+nŴ)] Z̃s[(nx̃+nX̂+nK+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- x̄ = buffer.x̂ V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk*Nk) # TODO: remove the 2 allocations + Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) @@ -1213,7 +1213,7 @@ function set_warmstart_mhe!( ) where NT<:Real model, buffer = estim.model, estim.buffer nu = model.nu - nk = get_nk(estim.model, transcription) + nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He Z̃s = estim.buffer.Z̃ @@ -1230,7 +1230,7 @@ function set_warmstart_mhe!( # --- verify definiteness of objective function --- x̄ = buffer.x̂ V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk*Nk) # TODO: remove the 2 allocations + Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) @@ -1266,11 +1266,11 @@ end function fill0unused!(Z̃, estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃ = estim.nε + nx̂ - nk = get_nk(estim.model, transcription) + nk̄ = get_nk̄(estim.model, transcription) nx̃_nX̂_He = nx̃ + nx̂*He - nx̃_nX̂_nK_He = nx̃_nX̂_He + nk*He + nx̃_nX̂_nK_He = nx̃_nX̂_He + nk̄*He Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector - Z̃[(nx̃_nX̂_He + nk*Nk + 1):(nx̃_nX̂_nK_He)] .= 0 # unused decision variables after K vector + Z̃[(nx̃_nX̂_He + nk̄*Nk + 1):(nx̃_nX̂_nK_He)] .= 0 # unused decision variables after K vector Z̃[(nx̃_nX̂_nK_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector return nothing end @@ -1336,7 +1336,7 @@ function predict_mhe!( estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, Ŵ, _ ) - nu, nd, ny, nk = model.nu, model.nd, model.ny, model.nk + nu, nd, ny, nk̄ = model.nu, model.nd, model.ny, model.nk̄ nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] p = estim.direct ? 0 : 1 x̂0 = @views x̂0arr[1:nx̂] @@ -1344,10 +1344,10 @@ function predict_mhe!( u0 = @views estim.U0[(1+nu*(j-1)):(nu*j)] d0 = @views estim.D0[(1+nd*(j+p-1)):(nd*(j+p))] ŵ = @views Ŵ[(1+nŵ*(j-1)):(nŵ*j)] - k = @views K[(1+nk*(j-1)):(nk*j)] + k̄ = @views K[(1+nk̄*(j-1)):(nk̄*j)] û0 = @views Û0[(1+nu*(j-1)):(nu*j)] x̂0next = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, k̄, estim, model, x̂0, u0, d0) x̂0next .+= ŵ if estim.direct ŷ0next = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1543,7 +1543,7 @@ function con_nonlinprogeq_mhe!( estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, Ŵ, Z̃ ) - nu, nx, nd, nk = model.nu, model.nx, model.nd, model.nk + nu, nx, nd, nk̄ = model.nu, model.nx, model.nd, model.nk̄ nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads @@ -1559,13 +1559,13 @@ function con_nonlinprogeq_mhe!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] - k = @views K[(1 + nk*(j-1)):(nk*j)] + k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - f!(x̂dnext, k, model, x̂d_Z̃, û0, d0, model.p) + f!(x̂dnext, k̄, model, x̂d_Z̃, û0, d0, model.p) x̂dnext .+= ŵd ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end @@ -1611,7 +1611,7 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads Ts = model.Ts - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) nw = nŵ - nxs nx̃ = estim.nε + nx̂ p = estim.direct ? 0 : 1 @@ -1625,18 +1625,18 @@ function con_nonlinprogeq_mhe!( end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] + k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] + k̇1, k̇2 = @views k̄dot[1:nx], k̄dot[nx+1:2*nx] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) model.f!(k̇1, x̂d_Z̃, û0, d0, model.p) else - k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̇[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 end if h < 1 model.f!(k̇2, x̂dnext_Z̃, û0, d0next, model.p) @@ -1694,10 +1694,10 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads Mo, no, τ = estim.Mo, transcription.no, transcription.τ - nk = get_nk(model, transcription) + nk̄ = get_nk̄(model, transcription) nx̃ = estim.nε + nx̂ p = estim.direct ? 0 : 1 - X̂0_Z̃, K_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)], Z̃[(nx̃+nx̂*He+1):(nx̃+nx̂*He+nk*He)] + X̂0_Z̃, K_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)], Z̃[(nx̃+nx̂*He+1):(nx̃+nx̂*He+nk̄*He)] Dtemp = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) @threadsif f_threads for j=1:Nk @@ -1708,14 +1708,14 @@ function con_nonlinprogeq_mhe!( end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] - k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] - ŝk = @views geq[(1 + nk*(j-1)):(nk*j)] + k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] + ŝk = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] # ----------------- collocation constraint defects ----------------------------- - Δk = k̇ + Δk = k̄dot for i=1:no - Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ + Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end mul!(ŝk, Mo, Δk) di = @views Dtemp[(1 + nd*(j-1)):(nd*j)] @@ -1723,8 +1723,8 @@ function con_nonlinprogeq_mhe!( ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i = @views k̇[(1 + (i-1)*nx):(i*nx)] - ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] + k̇i = @views k̄dot[(1 + (i-1)*nx):(i*nx)] + ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] di .= (1-τ[i]).*d0 .+ τ[i].*d0next if h < 1 model.f!(k̇i, ki_Z̃, û0, di, model.p) @@ -1735,9 +1735,9 @@ function con_nonlinprogeq_mhe!( model.f!(k̇i, ki_Z̃, ûi, di, model.p) end end - ŝk .-= k̇ + ŝk .-= k̄dot end - Nk < He && (geq[nk*Nk+1:end] .= 0) + Nk < He && (geq[nk̄*Nk+1:end] .= 0) return geq end diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 7d0aad91b..eb916dca3 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -163,7 +163,7 @@ function linearize!( nonlinmodel = model buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- - x0, u0, d0, k = buffer.x, buffer.u, buffer.d, buffer.k + x0, u0, d0, k̄ = buffer.x, buffer.u, buffer.d, buffer.k̄ x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop @@ -175,7 +175,7 @@ function linearize!( y0 .+= nonlinmodel.yop y = y0 # --- compute the nonlinear model next state at operating points --- - f!(x0next, k, nonlinmodel, x0, u0, d0, model.p) + f!(x0next, k̄, nonlinmodel, x0, u0, d0, model.p) x0next .+= nonlinmodel.fop xnext = x0next # xnext = f(x0,u0,d0) + fop - xop + xop = f(x0,u0,d0) + fop # --- recompute x since it was modified in buffer.x --- diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index e4d1c28b9..b16dc3351 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -12,7 +12,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} nx::Int ny::Int nd::Int - nk::Int + nk̄::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -50,14 +50,14 @@ struct LinModel{NT<:Real} <: SimModel{NT} xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - nk = 0 # not used for LinModel + nk̄ = 0 # not used for LinModel buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, p, Ts, t, - nu, nx, ny, nd, nk, + nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, buffer diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 282166199..ebc774ff5 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -34,7 +34,7 @@ struct NonLinModel{ nx::Int ny::Int nd::Int - nk::Int + nk̄::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -72,7 +72,7 @@ struct NonLinModel{ x0 = zeros(NT, nx) t = zeros(NT, 1) ni = solver.ni - nk = nx*(ni+1) + nk̄ = nx*(ni+1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd, ni) return new{NT, DS, F, H, PT, JB, LF}( x0, @@ -80,7 +80,7 @@ struct NonLinModel{ f!, h!, p, Ts, t, - nu, nx, ny, nd, nk, + nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, jacobian, linfunc!, @@ -306,15 +306,15 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, k, model::NonLinModel, x0, u0, d0, p) + f!(x0next, k̄, model::NonLinModel, x0, u0, d0, p) Compute `x0next` using the [`DiffSolver`](@ref) in `model.solver` and `model.f!`. -The method mutates `x0next` and `k` arguments in-place. The latter is used to store the +The method mutates `x0next` and `k̄` arguments in-place. The latter is used to store the intermediate stage values of the solver. """ -function f!(x0next, k, model::NonLinModel, x0, u0, d0, p) - return solver_f!(x0next, k, model.f!, model.Ts, model.solver, x0, u0, d0, p) +function f!(x0next, k̄, model::NonLinModel, x0, u0, d0, p) + return solver_f!(x0next, k̄, model.f!, model.Ts, model.solver, x0, u0, d0, p) end """ diff --git a/src/model/solver.jl b/src/model/solver.jl index eb81ad7a3..c42577c2f 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -28,15 +28,15 @@ This solver is allocation-free if the `f!` and `h!` functions do not allocate. RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) "Solve the differential equation with the 4th order Runge-Kutta method." -function solver_f!(xnext, k, f!::F, Ts, solver::RungeKutta{4}, x, u, d, p) where F +function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{4}, x, u, d, p) where F supersample = solver.supersample Ts_inner = Ts/supersample nx = length(x) - xcurr = @views k[1:nx] - k1 = @views k[(1nx + 1):(2nx)] - k2 = @views k[(2nx + 1):(3nx)] - k3 = @views k[(3nx + 1):(4nx)] - k4 = @views k[(4nx + 1):(5nx)] + xcurr = @views k̄[1:nx] + k1 = @views k̄[(1nx + 1):(2nx)] + k2 = @views k̄[(2nx + 1):(3nx)] + k3 = @views k̄[(3nx + 1):(4nx)] + k4 = @views k̄[(4nx + 1):(5nx)] @. xcurr = x for i=1:supersample f!(k1, xcurr, u, d, p) @@ -63,12 +63,12 @@ const ForwardEuler(;supersample=1) = RungeKutta(1; supersample) "Solve the differential equation with the forward Euler method." -function solver_f!(xnext, k, f!::F, Ts, solver::RungeKutta{1}, x, u, d, p) where F +function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{1}, x, u, d, p) where F supersample = solver.supersample Ts_inner = Ts/supersample nx = length(x) - xcurr = @views k[1:nx] - k1 = @views k[(1nx + 1):(2nx)] + xcurr = @views k̄[1:nx] + k1 = @views k̄[(1nx + 1):(2nx)] @. xcurr = x for i=1:supersample f!(k1, xcurr, u, d, p) diff --git a/src/sim_model.jl b/src/sim_model.jl index ef70d4f9a..4c819bc50 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -25,7 +25,7 @@ struct SimModelBuffer{NT<:Real} x::Vector{NT} y::Vector{NT} d::Vector{NT} - k::Vector{NT} + k̄::Vector{NT} empty::Vector{NT} end @@ -43,9 +43,9 @@ function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) where x = Vector{NT}(undef, nx) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) - k = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling + k̄ = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, y, d, k, empty) + return SimModelBuffer{NT}(u, x, y, d, k̄, empty) end @@ -240,10 +240,10 @@ julia> x = updatestate!(model, [1]) """ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0, x0next, k = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k + u0, d0, x0next, k̄ = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k̄ u0 .= u .- model.uop d0 .= d .- model.dop - f!(x0next, k, model, model.x0, u0, d0, model.p) + f!(x0next, k̄, model, model.x0, u0, d0, model.p) x0next .+= model.fop .- model.xop model.x0 .= x0next xnext = x0next diff --git a/src/transcription.jl b/src/transcription.jl index f80078830..9501dbb9b 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -189,25 +189,26 @@ includes the collocation points: where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic states (the first `nx` elements of ``\mathbf{x̂}``): ```math -\mathbf{K} = \begin{bmatrix} - \mathbf{k}_{1}(k+0) \\ - \mathbf{k}_{2}(k+0) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+0) \\ - \mathbf{k}_{1}(k+1) \\ - \mathbf{k}_{2}(k+1) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} +\mathbf{K} = \begin{bmatrix} + \mathbf{k̄}(k+0) \\ + \mathbf{k̄}(k+1) \\ + \vdots \\ + \mathbf{k̄}(k+H_p-1) +\end{bmatrix} \quad \text{and} \quad +\mathbf{k̄}(k+j) = \begin{bmatrix} + \mathbf{k}_1(k+j) \\ + \mathbf{k}_2(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) +\end{bmatrix} ``` -and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation -point at the ``j``th stage/interval (details in Extended Help). The `roots` keyword argument -is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or Gauss-Legendre quadrature, -respectively. See [`MultipleShooting`](@ref) docstring for descriptions of `f_threads` and -`h_threads` keywords. This transcription computes the predictions by enforcing the -collocation and continuity constraints at the collocation points. It is efficient for highly -stiff systems, but generally more expensive than the other methods for non-stiff systems. -See Extended Help for details and the transcription of [`MovingHorizonEstimator`](@ref) -objects. +The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or +Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for info +on `f_threads` and `h_threads` keywords. This transcription computes thecpredictions by +enforcing the collocation and continuity constraints at the collocationc points. It is +efficient for highly stiff systems, but generally more expensive than the other methods for +non-stiff systems. See Extended Help for details and the transcription of +[`MovingHorizonEstimator`](@ref) objects. !!! warning Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a @@ -233,7 +234,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ \mathbf{K} \\ - \mathbf{0_k} \\ + \mathbf{0_k̄} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} ``` @@ -241,17 +242,12 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). all these variables, except for the vector with the intermediate stages of the deterministic states at the collation points: ```math - \mathbf{K} = \begin{bmatrix} - \mathbf{k}_{1}(k-N_k+p+0) \\ - \mathbf{k}_{2}(k-N_k+p+0) \\ - \vdots \\ - \mathbf{k}_{n_o}(k-N_k+p+0) \\ - \mathbf{k}_{1}(k-N_k+p+1) \\ - \mathbf{k}_{2}(k-N_k+p+1) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+p-1) \end{bmatrix} + \mathbf{K} = \begin{bmatrix} + \mathbf{k̄}(k-N_k+p+0) \\ + \mathbf{k̄}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{k̄}(k+p-1) \end{bmatrix} ``` - The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme @@ -329,8 +325,8 @@ dynamics at the discrete time ``k`` are given by: \begin{aligned} \mathbf{ŝ_k}(k) &= \mathbf{M_o} \begin{bmatrix} - \mathbf{k}_1(k) - \mathbf{x̂_d}(k) \\ - \mathbf{k}_2(k) - \mathbf{x̂_d}(k) \\ + \mathbf{k}_1(k) - \mathbf{x̂_d}(k) \\ + \mathbf{k}_2(k) - \mathbf{x̂_d}(k) \\ \vdots \\ \mathbf{k}_{n_o}(k) - \mathbf{x̂_d}(k) \end{bmatrix} - \begin{bmatrix} @@ -442,9 +438,9 @@ function validate_transcription(::NonLinModel{<:Real, <:EmptySolver}, ::Collocat end validate_transcription(::SimModel, ::TranscriptionMethod) = nothing -"Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." -get_nk(model::SimModel, ::ShootingMethod) = model.nk -get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no +"Get length of the `k̄` vector with all the solver intermediate steps or all the collocation pts." +get_nk̄(model::SimModel, ::ShootingMethod) = model.nk̄ +get_nk̄(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no transcription_str(transription::TranscriptionMethod) = string(nameof(typeof(transription))) function transcription_str(transription::OrthogonalCollocation) diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 8a618b864..e6c23dbf2 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -221,8 +221,8 @@ end @test string(solver) == "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver, p=p) - xnext, k, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k, nonlinmodel5.buffer.y - ModelPredictiveControl.f!(xnext, k, nonlinmodel5, [0; 0], [0], [0], nonlinmodel5.p) + xnext, k̄, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k̄, nonlinmodel5.buffer.y + ModelPredictiveControl.f!(xnext, k̄, nonlinmodel5, [0; 0], [0], [0], nonlinmodel5.p) @test xnext ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel5, [0; 0], [0], nonlinmodel5.p) @test y ≈ zeros(1) @@ -239,14 +239,14 @@ end return nothing end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta(), p=p) - xnext, k, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k, nonlinmodel6.buffer.y - ModelPredictiveControl.f!(xnext, k, nonlinmodel6, [0; 0], [0], [0], nonlinmodel6.p) + xnext, k̄, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k̄, nonlinmodel6.buffer.y + ModelPredictiveControl.f!(xnext, k̄, nonlinmodel6, [0; 0], [0], [0], nonlinmodel6.p) @test xnext ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel6, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) nonlinmodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) - xnext, k, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k, nonlinmodel7.buffer.y - ModelPredictiveControl.f!(xnext, k, nonlinmodel7, [0; 0], [0], [0], nonlinmodel7.p) + xnext, k̄, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k̄, nonlinmodel7.buffer.y + ModelPredictiveControl.f!(xnext, k̄, nonlinmodel7, [0; 0], [0], [0], nonlinmodel7.p) @test xnext ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel7, [0; 0], [0], nonlinmodel7.p) @test y ≈ zeros(1) @@ -330,7 +330,7 @@ end nonlinmodel3 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=RungeKutta()) linmodel3 = linearize(nonlinmodel3; x, u, d) x0, u0, d0 = x - nonlinmodel3.xop, u - nonlinmodel3.uop, d - nonlinmodel3.dop - x0next, k, y0 = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k, nonlinmodel3.buffer.y + x0next, k, y0 = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k̄, nonlinmodel3.buffer.y backend = AutoForwardDiff() f_A(x0next, x0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) f_Bu(x0next, u0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p)