diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 359299bfe..9313eb8c0 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -67,6 +67,8 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! +ModelPredictiveControl.fq_dae! +ModelPredictiveControl.ĥ_dae! ``` ## Remove Operating Points diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index e9e418920..400383ac1 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -3,10 +3,12 @@ struct StateEstimatorBuffer{NT<:Real} û ::Vector{NT} k̄ ::Vector{NT} x̂ ::Vector{NT} + a ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} Ŵ ::Vector{NT} X̂ ::Vector{NT} + A ::Vector{NT} Ŷ ::Vector{NT} D ::Vector{NT} P̂ ::Matrix{NT} @@ -21,7 +23,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, na::Int=0, He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) @@ -31,20 +33,22 @@ 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, na::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ŵ) - nV̂, nŴ, nX̂, nŶ, nD = nym*He, nŵ*He, nx̂*He, ny*He, nd*(He+1) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) + nV̂, nŴ, nX̂, nA, nŶ, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) x̂ = Vector{NT}(undef, nx̂) + a = Vector{NT}(undef, na) Z̃ = Vector{NT}(undef, nZ̃) V̂ = Vector{NT}(undef, nV̂) Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) + A = Vector{NT}(undef, nA) Ŷ = Vector{NT}(undef, nŶ) D = Vector{NT}(undef, nD) P̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -56,7 +60,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̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end @@ -174,7 +178,7 @@ where ``\mathbf{e}(k)`` is an unknown zero mean white noise and ``\mathbf{A_s} = it is thus ignored. The function [`init_integrators`](@ref) builds the state-space matrices. """ function init_estimstoch( - model::SimModelODE{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt + model::SimModel{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt ) where {NT<:Real} nu, ny, nym = model.nu, model.ny, length(i_ym) As_u , Cs_u , nint_u = init_integrators(nint_u , nu , "u") @@ -198,7 +202,7 @@ function validate_ym(model::SimModel, i_ym) end "Convert the measured outputs stochastic model `stoch_ym` to all outputs `stoch_y`." -function stoch_ym2y(model::SimModelODE{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} +function stoch_ym2y(model::SimModel{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} As = Asm Bs = Bsm Cs = zeros(NT, model.ny, size(Csm,2)) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index c2c4e5a53..ce21c5f21 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state update function ``\mathbf{f̂}`` of the augmented model. @@ -28,7 +28,7 @@ the function returns the next state of the augmented model, as deviation vectors ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, `û0` and `k` in place. The argument `û0` stores the disturbed input of the augmented model -``\mathbf{û_0}``, and `k`, the intermediate stage values of `model.solver`, when applicable. +``\mathbf{û_0}``, and `k̄`, the intermediate stage values of `model.solver`, when applicable. The model parameter `model.p` is not included in the function signature for conciseness. The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. @@ -61,8 +61,8 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for are computed by [`augment_model`](@ref) (almost always zeros in practice for [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) - return f̂!(x̂0next, û0, k, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, k̄, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end @doc raw""" @@ -92,28 +92,28 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) -Same than [`f̂!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. +Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k̄, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - f!(xdnext, k, model, xd, û0, d0, model.p) + f!(xdnext, k̄, model, xd, û0, d0, model.p) mul!(xsnext, As, xs) x̂0next .+= f̂op .- x̂op return nothing end @doc raw""" - ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) -> nothing + ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref). """ -function ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) return ĥ!(ŷ0, model, estim.Cs_y, x̂0, d0) end @@ -129,11 +129,11 @@ function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0) end """ - ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) + ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) -Same than [`ĥ!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. +Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) +function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] h!(ŷ0, model, xd, d0, model.p) # y0 = h(xd, d0) @@ -141,6 +141,36 @@ function ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) return nothing end +""" + fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) + +Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). +""" +function fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) + return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +end +fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0, model.p) + +""" + ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) + +Similar than [`ĥ!`](@ref) but with a algebraic variable `a0` for [`NonLinModelDAE`](@ref) . +""" +function ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) + # `@views` macro avoid copies with matrix slice operator e.g. [a:b] + @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] + model.h!(ŷ0, xd, a0, d0, model.p) + mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + return nothing +end + +""" + ĥ_dae!(ŷ0, estim::StateEstimator, model::SimModelODE, x0, _ , d0) + +Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. +""" +ĥ_dae!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _, d0) = ĥ!(ŷ0, estim, model, x̂0, d0) + """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 185054343..914ed89f9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -173,7 +173,7 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator{NT} } nu, ny, nd = model.nu, model.ny, model.nd - nk̄ = get_nk̄(model, transcription) + nk̄, na = get_nk̄(model, transcription), get_na(model) 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) @@ -204,7 +204,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ŵ, na) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) @@ -221,7 +221,9 @@ struct MovingHorizonEstimator{ test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct) # force computation of derivatives for the first NLP iteration: force∇J, force∇g, force∇geq = [true], [true], [true] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄, He, nŵ, nε, transcription) + buffer = StateEstimatorBuffer{NT}( + nu, nx̂, nym, ny, nd, nk̄, na, He, nŵ, nε, transcription + ) estim = new{NT, SM, KC, TM, JM, GB, JB, HB, PT, GCfunc, CE}( model, transcription, optim, con, gradient, jacobian, hessian, @@ -245,6 +247,7 @@ struct MovingHorizonEstimator{ direct, prepared, buffer ) + reset_warmstart!(estim, transcription) init_optimization!(estim, model, optim) return estim end @@ -253,9 +256,10 @@ end @doc raw""" MovingHorizonEstimator(model::SimModel; ) -Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). +Construct a moving horizon estimator (MHE) based on `model`. -It can handle constraints on the estimates. Additionally, `model` is not linearized like the +It supports ([`LinModel`](@ref), [`NonLinModel`](@ref)), [`NonLinModelDAE`](@ref)) and +constraints on the estimates. Additionally, `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the probability distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, however, since it minimizes the following objective function at each discrete time ``k``: @@ -498,7 +502,8 @@ MovingHorizonEstimator estimator with a sample time Ts = 5.0 s: the `f` and `h` functions must be compatible with this feature. See the [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref) - estimates the arrival covariance by default. + estimates the arrival covariance by default for [`NonLinModel`](@ref). The default is + a [`SteadyKalmanFilter`](@ref) for [`NonLinModelDAE`](@ref). Two exceptions about AD: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument and the `hessian=true` option default to this sparse backend: @@ -648,11 +653,16 @@ function default_optim_mhe(model::SimModel, nc) end "Default arrival covariance estimator for MHE, depending on the model type only." -function default_covestim_mhe(model::SimModelODE, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) +function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) if model isa LinModel return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) - else + elseif model isa NonLinModel return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + else + nx, nu, ny = model.nx, model.nu, model.ny + A, Bu, C = 0.1*I(nx), ones(nx, nu), ones(ny, nx) + dummy_model = LinModel(A, Bu, C, 0, 0, model.Ts) + return SteadyKalmanFilter(dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) end end @@ -1084,8 +1094,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̄, na = get_nk̄(model, transcription), get_na(model) + nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Tŵ = [spzeros(NT, nŴ, nZ-nŴ) I] return Tŵ end @@ -1311,8 +1321,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̄, na = get_nk̄(model, transcription), get_na(model) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Z̃min, Z̃max = fill(convert(NT,-Inf), nZ̃), fill(convert(NT,+Inf), nZ̃) nε > 0 && (Z̃min[begin] = 0) nŴ = nŵ*He @@ -1431,16 +1441,17 @@ function get_nonlinobj_op( ) where JNT<:Real model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian - nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu + nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) 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̃) + nŴ, nV̂, nX̂, nA, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) 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) J::Vector{JNT} = zeros(JNT, 1) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) @@ -1449,16 +1460,16 @@ function get_nonlinobj_op( Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = zeros(JNT, nZ̃) J_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1548,18 +1559,19 @@ function get_nonlincon_oracle( # ----------- common cache for all functions ---------------------------------------- model, con = estim.model, estim.con jac, hess = estim.jacobian, estim.hessian - nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu + nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) 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̃) + nŴ, nV̂, nX̂, nA, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) 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) myInf = convert(JNT, Inf) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) @@ -1571,24 +1583,26 @@ function get_nonlincon_oracle( gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi) + function ℓ_gi( + Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = zeros(JNT, nZ̃) ∇gi_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1601,9 +1615,9 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), - Cache(Ŵe), Cache(V̂e), Cache(X̂e), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) @@ -1651,22 +1665,24 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end - function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function ℓ_geq( + Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end Z̃_∇geq = zeros(JNT, nZ̃) ∇geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) @@ -1678,8 +1694,8 @@ function get_nonlincon_oracle( ∇geq_structure = init_diffstructure(∇geq) if !isnothing(hess) ∇²geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 08674d10c..d57d03eaa 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -3,7 +3,6 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model nu, ny, nd = model.nu, model.ny, model.nd uop, yop, dop = model.uop, model.yop, model.dop - estim.Z̃ .= 0 estim.Y0m .= NaN estim.Yem .= NaN estim.U0 .= NaN @@ -33,6 +32,76 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.P̂arr_old .= estim.cov.P̂_0 invert_cov!(estim, estim.covestim) estim.x̂0arr_old .= 0 + reset_warmstart!(estim, estim.transcription) + return nothing +end + +""" + reset_warmstart!(estim::MovingHorizonEstimator, ::TranscriptionMethod) + +Reset warm-starting values `estim.Z̃` at values stored in `estim.model` +""" +function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) + model = estim.model + nx, nx̂, nε, He = model.nx, estim.nx̂, estim.nε, estim.He + nx̃ = nε + nx̂ + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + end + return nothing +end +function reset_warmstart!( + estim::MovingHorizonEstimator, ::TrapezoidalCollocation +) + model = estim.model + nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) + nx̃ = nε + nx̂ + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+j*na)] .= a0s + end + return nothing +end +function reset_warmstart!( + estim::MovingHorizonEstimator, transcription::OrthogonalCollocation +) + model = estim.model + nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) + nx̃ = nε + nx̂ + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*nx̂+1):(nx̃+nx̂*He+na+na*He+(j-1)*nx̂+nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+nx*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+nx*He+j*na)] .= a0s + end + return nothing +end +function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) + model = estim.model + nx, nε = model.nx, estim.nε + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s return nothing end @@ -145,20 +214,23 @@ 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, get_nk̄(model, estim.transcription) 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̂ + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ + x̂0arr, a0arr = buffer.x̂, buffer.a 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) - V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) - Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr) - J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) - yopm = model.yop[estim.i_ym] + x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) + x̄ = getx̄!(x̄, estim, x̂0arr) + V̂, X̂0, A0 = predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, A0, x̂0arr, a0arr) + J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) + yopm = model.yop[estim.i_ym] Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)] Ym, U, D, Ŷ, X̂, x̂arr = Ym0, U0, D0, Ŷ0, X̂0, x̂0arr for i=1:Nk @@ -213,35 +285,36 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher # --- objective derivatives --- optim, con = estim.optim, estim.con hess = estim.hessian - nx̂, nym, nŷ, nu, nc = estim.nx̂, estim.nym, model.ny, model.nu, con.nc + nx̂, nym, nŷ, nu, nc, na = estim.nx̂, estim.nym, model.ny, model.nu, con.nc, get_na(model) 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ŷ + nV̂, nX̂, nA, nŴ = He*nym, He*nx̂, He*na, He*nx̂ + 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̂) + a0arr, A0 = zeros(NT, na), zeros(NT, nA) Ŵ = zeros(NT, nŴ) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) Ŵe = zeros(NT, nŴe) V̂e, X̂e = zeros(NT, nV̂e), zeros(NT, nX̂e) - K = zeros(NT, nK) + K̄ = zeros(NT, nK̄) Û0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) gc, g = zeros(NT, nc), zeros(NT, ng) geq = zeros(NT, neq) gi = zeros(NT, ngi) J_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -256,15 +329,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- inequality constraint derivatives --- ∇g_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing @@ -288,16 +361,18 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²g_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), - Cache(gc), Cache(g), Cache(geq), + Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(gc), Cache(g), Cache(geq), Cache(gi) ) - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi) + function ℓ_gi( + Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -310,15 +385,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- equality constraint derivatives --- ∇geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end @@ -341,15 +416,17 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function ℓ_geq( + Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -465,20 +542,29 @@ Does nothing if `model` is not a [`LinModel`](@ref). """ initpred!(::MovingHorizonEstimator, ::SimModel) = nothing -"Get the estimated state at arrival from the decision vector `Z̃`." -function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) - nx̃ = estim.nε + estim.nx̂ - return x̂0arr .= @views Z̃[nx̃-estim.nx̂+1:nx̃] +"Get the estimated state at arrival `x̂0arr` from the decision vector `Z̃`." +function getx̂0arr!(x̂0arr, estim::MovingHorizonEstimator, Z̃) + return x̂0arr .= @views Z̃[(estim.nε + 1):(estim.nε + estim.nx̂)] end +"Get algebraic variable at arrival `a0arr` from the decision vector `Z̃`." +function geta0arr!( + a0arr, estim::MovingHorizonEstimator, ::CollocationMethod, Z̃ +) + na = get_na(estim.model) + nx̃_nX̂ = estim.nε + estim.nx̂ + estim.nx̂*estim.He + return a0arr .= @views Z̃[(nx̃_nX̂ + 1):(nx̃_nX̂ + na)] +end +geta0arr!(a0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = a0arr + "Get the estimation error at arrival from the estimated state at arrival `x̂0arr`." getx̄!(x̄, estim::MovingHorizonEstimator, x̂0arr) = (x̄ .= estim.x̂0arr_old .- x̂0arr) "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̄, na = get_nk̄(estim.model, transcription), get_na(estim.model) + nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Ŵ .= @views Z̃[(nZ̃ - nŵ*He + 1):end] return Ŵ end @@ -643,13 +729,16 @@ 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[] - x̂0arr = buffer.x̂ - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ + nu, nx̂, Nk = model.nu, estim.nx̂, estim.Nk[] + nk̄ = get_nk̄(model, estim.transcription) + x̂0arr, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ Û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̃) + getx̂0arr!(x̂0arr, estim, Z̃) + predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing end @@ -834,7 +923,7 @@ Objective function of the MHE when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ, Z̃) - Nk = estim.Nk[] + Nk = estim.Nk[] invP̄ = estim.cov.invP̄ invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) @@ -847,35 +936,40 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ end @doc raw""" - predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) -> Ŷ0 + predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) -> Ŷ0 Predict in-place the outputs of `estim` [`MovingHorizonEstimator`](@ref). This function is not used for the optimization, but it can be useful to predict the estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the -`Ŷ0` vector. The argument `X̂0` is computed from [`predict_mhe!`](@ref) and contains the -estimated states from ``k-N_k+1+p`` to ``k+p``. The argument `x̂0arr` is computed from -[`getarrival!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. +`Ŷ0` vector. The arguments `X̂0` and `A0` are computed from [`predict_mhe!`](@ref) and +contains the estimated states and algebraic variable from ``k-N_k+1+p`` to ``k+p``. The +argument `x̂0arr` and `a0arr` are computed from [`getx̂0arr!`](@ref) and [`geta0arr!`](@ref). +They respectively contains the arrival state and algebraic estimates for the time step +``k-N_k+p``. """ -function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) +function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) model = estim.model nd, ny, nx̂, Nk = model.nd, model.ny, estim.nx̂, estim.Nk[] + na = get_na(model) D0 = estim.D0 p = estim.direct ? 0 : 1 x̂0 = @views estim.direct ? X̂0[1:nx̂] : x̂0arr[1:nx̂] + a0 = @views estim.direct ? A0[1:na] : a0arr[1:na] for j=1:Nk d0 = @views D0[(1 + nd*j):(nd*(j+1))] # 1st data in D0 is d0(k-Nk), not used here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] ĥ!(ŷ0, estim, estim.model, x̂0, d0) j < Nk || break x̂0 = @views X̂0[(1 + nx̂*(j-p)):(nx̂*(j-p+1))] + a0 = @views A0[(1 + na*(j-p)):(na*(j-p+1))] end return Ŷ0 end """ update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -884,18 +978,24 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription - x̂0arr = getarrival!(x̂0arr, estim, Z̃) - x̄ = getx̄!(x̄, estim, x̂0arr) - Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) - V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, Ŵ, Z̃) + x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + x̄ = getx̄!(x̄, estim, x̂0arr) + Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) + V̂, X̂0, A0 = predict_mhe!( + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) ε = getslack(estim, Z̃) gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) - geq = con_nonlinprogeq_mhe!(geq, X̂0, Û0, K̄, estim, model, transcription, x̂0arr, Ŵ, Z̃) + geq = con_nonlinprogeq_mhe!( + geq, X̂0, A0, Û0, K̄, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 3ca0b340e..8ccdad902 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,31 +1,49 @@ "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(::SingleShooting, He, nx̂ , ::Any, nŵ, ::Any=0) = nx̂ + nŵ*He +get_nZ_mhe(::MultipleShooting, He, nx̂ , ::Any, nŵ, ::Any=0) = nx̂ + nx̂*He + nŵ*He +function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, ::Any, nŵ, na=0) + return nx̂ + nx̂*He + na + na*He + na*He + nŵ*He +end +function get_nZ_mhe(transcription::OrthogonalCollocation, He, nx̂, nk̄, nŵ, na=0) + nā = na*transcription.no + return nx̂ + nx̂*He + na + na*He + nk̄*He + nā*He + nŵ*He +end "Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nŴ, nX̂ = nŵ*Nk, nx̂*Nk + na = get_na(estim.model) + nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*estim.He + nx̃_nX̂_He = nx̃ + nx̂*estim.He + na_nA_He = na + na*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He + nx̃_nX̂_na_nA_nĀ_He = nx̃_nX̂_na_nA_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + nŴ) + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA) + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nA) + (1 + nx̃_nX̂_na_nA_nĀ_He):(nx̃_nX̂_na_nA_nĀ_He + nŴ) ] return i_Z̃_NK 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 + na = get_na(estim.model) + nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) + nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*estim.He - nx̃_nX̂_nK_He = nx̃_nX̂_He + nk̄*estim.He + nx̃_nX̂_He = nx̃ + nx̂*estim.He + na_nA_He = na + na*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He + nx̃_nX̂_na_nA_nK̄_He = nx̃_nX̂_na_nA_He + nk̄*estim.He + nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃_nX̂_na_nA_nK̄_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + nK); - (1 + nx̃_nX̂_nK_He):(nx̃_nX̂_nK_He + nŴ); + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA); + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nK̄); + (1 + nx̃_nX̂_na_nA_nK̄_He):(nx̃_nX̂_na_nA_nK̄_He + nĀ); + (1 + nx̃_nX̂_na_nA_nK̄_nĀ_He):(nx̃_nX̂_na_nA_nK̄_nĀ_He + nŴ); ] return i_Z̃_NK end @@ -369,14 +387,14 @@ end """ init_predmat_mhe( - model::SimModelODE, transcription::SingleShooting, direct::Bool, + model::SimModel, transcription::SingleShooting, direct::Bool, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return empty matrices for [`SingleShooting`](@ref) and non-`LinModel`, except for `ex̄`. """ function init_predmat_mhe( - model::SimModelODE{NT}, transcription::SingleShooting, ::Bool, + model::SimModel{NT}, transcription::SingleShooting, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -397,20 +415,20 @@ end """ init_predmat_mhe( - model::SimModelODE, transcription::TranscriptionMethod, direct::Bool + model::SimModel, transcription::TranscriptionMethod, direct::Bool He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return `ex̄, EX̂, GX̂, JX̂, BX̂` and empty matrices non-`LinModel` and other [`TranscriptionMethod`](@ref). """ function init_predmat_mhe( - model::SimModelODE{NT}, transcription::TranscriptionMethod, ::Bool, + model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) 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̄, na = get_nk̄(model, transcription), get_na(model) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) 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)] @@ -510,7 +528,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModelODE, transcription::TranscriptionMethod, direct::Bool, + model::SimModel, transcription::TranscriptionMethod, direct::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) -> ES, GS, JS, BS @@ -541,15 +559,18 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{I} \end{bmatrix} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^ŵ} \end{bmatrix} + \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S^ā} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \end{aligned} ``` """ function init_defectmat_mhe( - model::SimModelODE{NT}, ::TranscriptionMethod, ::Bool, + model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) + na, nā = get_na(model), get_nā(model, transcription) nx = nx̂ - nxs nŵ = nx̂ nŵd = nŵ - nxs @@ -560,7 +581,9 @@ function init_defectmat_mhe( ESx̂[iRow, iCol] = As end ESŵ = repeatdiag([zeros(NT, nxs, nŵd) I], He) - ES = [ESx̂ ESŵ] + ESa = zeros(nxs*He, na + na*He) + ESā = zeros(nxs*He, nā*He) + ES = [ESx̂ ESa ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -569,7 +592,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModelODE, transcription::OrthogonalCollocation, direct::Bool + model::SimModel, transcription::OrthogonalCollocation, direct::Bool He, Â, _ , _ , _ , _ , As, Co, λo ) -> ES, GS, JS, BS @@ -606,17 +629,20 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \vdots & \vdots & \ddots & \vdots \\ \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^ŵ} &= \mathbf{I} \\ + \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S^ā} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^k̄} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \\ \end{aligned} ``` """ function init_defectmat_mhe( - model::SimModelODE{NT}, transcription::OrthogonalCollocation, ::Bool, + model::SimModel{NT}, transcription::OrthogonalCollocation, ::Bool, He, Â, _ , _ , _ , _ , As, Co, λo ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs + na, nā = get_na(model), get_nā(model, transcription) nk̄ = get_nk̄(model, transcription) λo_I = λo*I(nx) ESx̂ = [zeros(NT, nx̂*He, nx̂) -I] @@ -630,7 +656,9 @@ function init_defectmat_mhe( end ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], He) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented - ES = [ESx̂ ESk̄ ESŵ] + ESa = zeros(nx̂*He, na + na*He) + ESā = zeros(nx̂*He, nā*He) + ES = [ESx̂ ESa ESk̄ ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -648,11 +676,11 @@ function init_defectmat_mhe( end function init_defectmat_mhe_empty( - model::SimModelODE{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ + 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̄, na = get_nk̄(model, transcription), get_na(model) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) ES = zeros(NT, 0, nZ) GS = zeros(NT, 0, nu*He) JS = zeros(NT, 0, nd*(He+1)) @@ -662,7 +690,7 @@ end @doc raw""" init_matconstraint_mhe( - model::LinModel, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::LinModel, transcription::ShootingMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) -> i_b, i_g, A, Aeq, neq @@ -686,7 +714,7 @@ case, `args` needs to contain all the inequality and equality constraint matric the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mhe( - model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::LinModel{NT}, transcription::ShootingMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) where {NT<:Real} if isempty(args) @@ -734,9 +762,9 @@ function init_matconstraint_mhe( return i_b, i_g, A, Aeq, neq end -"Init `i_b, A` without sensor noise constraints if `NonLinModel` and other `TranscriptionMethod`." +"Init `i_b, A` without sensor noise constraints if not `LinModel` and other `TranscriptionMethod`." function init_matconstraint_mhe( - model::NonLinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::SimModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) where {NT<:Real} if isempty(args) @@ -784,7 +812,7 @@ boxconstraint_states!(Z̃min, Z̃max, ::SingleShooting, _, _, _, _, _, _) = Z̃m "Unset `i_x̂min` and `i_x̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deletex̂arr_lincon!( - i_x̂min, i_x̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε + i_x̂min, i_x̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε ) nx̂ = length(i_x̂min) x̂0min, x̂0max = @views Z̃min[(nε+1):(nε+nx̂)], @views Z̃max[(nε+1):(nε+nx̂)] @@ -795,7 +823,7 @@ end "Unset `i_X̂min` and `i_X̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deleteX̂_lincon!( - i_X̂min, i_X̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ + i_X̂min, i_X̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ ) nx̃ = nε + nx̂ nX̂ = length(i_X̂min) @@ -804,10 +832,10 @@ function deleteX̂_lincon!( foreach(i -> !isinf(X̂0max[i]) && (i_X̂max[i] = false), eachindex(i_X̂max)) return i_X̂min, i_X̂max end -deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModelODE, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max +deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max "Unset `i_Ŵmin` and `i_Ŵmax` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max) +function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max) nŴ = length(i_Ŵmin) Ŵmin, Ŵmax = @views Z̃min[end-nŴ+1:end], Z̃max[end-nŴ+1:end] foreach(i -> !isinf(Ŵmin[i]) && (i_Ŵmin[i] = false), eachindex(i_Ŵmin)) @@ -874,9 +902,9 @@ function linconstraint!( return nothing end -"Set `b` excluding sensor noise bounds if for `NonLinModel` and non-`SingleShooting`." +"Set `b` excluding sensor noise bounds if not a `LinModel` and non-`SingleShooting`." function linconstraint!( - estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod ) nx̂, nŵ = estim.nx̂, estim.nx̂ # --- truncate vector and matrices if necessary --- @@ -978,7 +1006,7 @@ end """ linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod ) By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@ref). @@ -988,7 +1016,7 @@ vector is only zeros for this specific case. See [`init_defectmat_mhe`](@ref) fo equations. """ function linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod ) optim, con, Nk = estim.optim, estim.con, estim.Nk[] nŝ = size(con.Aeq, 1) ÷ estim.He # number of state defects per time step @@ -1022,7 +1050,7 @@ function linconstrainteq!( return nothing end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." -linconstrainteq!(::MovingHorizonEstimator, ::SimModelODE, ::SingleShooting) = nothing +linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing @doc raw""" set_warmstart_mhe!( @@ -1032,28 +1060,31 @@ linconstrainteq!(::MovingHorizonEstimator, ::SimModelODE, ::SingleShooting) = no Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). If supported by `estim.optim` and based a [`SingleShooting`](@ref) transcription, it -warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{ŵ}(k-N_k+p+0|k-1) \\ - \mathbf{ŵ}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\end{bmatrix} -``` -where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the -process noise estimates computed at the last time step ``k-1``. The vector -``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated -at time ``k-N_k``. If the objective function is not finite at this point, all the process -noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended Help of -[`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates -all the arguments. +warm-starts the solver at the value provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \end{bmatrix} + ``` + where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the + process noise estimates computed at the last time step ``k-1``. The vector + ``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated + at time ``k-N_k``. If the objective function is not finite at this point, all the process + noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended Help of + [`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates + all the arguments. """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var @@ -1072,13 +1103,15 @@ function set_warmstart_mhe!( Z̃s[(nx̃+1):(nx̃+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nŵ+1):(nx̃+nŴ)] Z̃s[(nx̃+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄ = buffer.x̂ - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ + x̄, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ Û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) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] .= 0 # Ŵ = 0 @@ -1099,81 +1132,114 @@ end Do the same but for [`OrthogonalCollocation`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ - \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+p-2|k-1) \\ - \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) \\ - \vdots \\ - \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) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\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 -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_ŵ}``. +The warm-starting value is provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ + \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+p-2|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p|k-1) \\ + \mathbf{a_0}(k-N_k+p+1|k-1) \\ + \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \vdots + \mathbf{a_0}(k-p-2|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{0_a} \\ + \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{0_k̄} \\ + \mathbf{ā}(k-N_k+p+0|k-1) \\ + \mathbf{ā}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+p-3|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{0_ā} \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \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 + 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_ŵ}``. """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu - nk̄ = get_nk̄(estim.model, transcription) + nu, na = model.nu, get_na(model) + nk̄, nā = get_nk̄(estim.model, transcription), get_nā(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̂, nA, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- - Z̃s[nε+1:nx̃] = estim.x̂0arr_old - # --- state estimates X̂0 --- - 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)] + i_base = nε + Z̃s[i_base+1:nx̃] = estim.x̂0arr_old + # --- state estimates X̂0 --- + i_base = nx̃ + Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views estim.Z̃[(i_base+nx̂+1):(i_base+nX̂)] + Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] + # --- algebraic variables a0arr and A0 --- + i_base = nx̃ + nX̂ + Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] + Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] + # --- deterministic states at collocation points K̄ --- + i_base = nx̃ + nX̂ + na + nA + Z̃s[(i_base+1):(i_base+nK̄-nk̄)] .= @views estim.Z̃[(i_base+nk̄+1):(i_base+nK̄)] + Z̃s[(i_base+nK̄-nk̄+1):(i_base+nK̄)] .= @views estim.Z̃[(i_base+nK̄-nk̄+1):(i_base+nK̄)] + # --- algebraic variables at collocation points Ā --- + i_base = nx̃ + nX̂ + na + nA + nK̄ + Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] + Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- 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̂ + i_base = nx̃ + nX̂ + na + nA + nK̄ + nĀ + Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] + Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 + # --- verify definiteness of objective function --- + x̄, a0arr = buffer.x̂, buffer.a 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 + A0, Ā = Vector{NT}(undef, na*Nk), Vector{NT}(undef, nā*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+nX̂+nK+1:end] .= 0 # Ŵ = 0 + Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 end # --- unused variable in Z̃ (applied only when Nk < He) --- # We force the update of the NLP gradient and jacobian by warm-starting the unused # variable of Ŵ in Z̃ at 1. Since Ŵ is initialized with 0s, at least 1 variable in Z̃s # will be inevitably different at the following time step. - Z̃s[nx̃+nX̂+nK+nŵ*Nk+1:end] .= 1 + Z̃s[nx̃+nX̂+nK̄+nŵ*Nk+1:end] .= 1 JuMP.set_start_value.(Z̃var, Z̃s) return Z̃s end @@ -1185,28 +1251,40 @@ end Do the same but for other transcription [`TranscriptionMethod`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ - \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+p-2|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{0_x̂} \\ - \mathbf{ŵ}(k-N_k+p+0|k-1) \\ - \mathbf{ŵ}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\end{bmatrix} -``` +The warm-starting value is provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ + \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+p-2|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p|k-1) \\ + \mathbf{a_0}(k-N_k+p+1|k-1) \\ + \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \vdots + \mathbf{a_0}(k-p-2|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{0_a} \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \end{bmatrix} + ``` """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var @@ -1228,13 +1306,16 @@ function set_warmstart_mhe!( Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nŵ+1):(nx̃+nX̂+nŴ)] Z̃s[(nx̃+nX̂+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 + x̄, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ + Û0, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old + a0arr = geta0arr!(a0arr, estim, transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!( + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+1:end] .= 0 # Ŵ = 0 @@ -1277,10 +1358,10 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, _ , _ , _ , + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, - _ , _ , Z̃ - ) -> V̂, X̂0 + x̂0arr, a0arr, Ŵ, Z̃ + ) -> V̂, X̂0, A0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -1293,11 +1374,12 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k \mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂} \end{aligned} ``` +The vector `A0` is ignored and returned unchanged. """ function predict_mhe!( - V̂, X̂0, _ , _ , _ , + V̂, X̂0, A0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, - _ , _ , Z̃ + _ , _ , _ , Z̃ ) nε, Nk = estim.nε, estim.Nk[] if Nk < estim.He @@ -1315,26 +1397,27 @@ function predict_mhe!( end V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F X̂0_res .= mul!(X̂0_res, ẼX̂, Z̃) .+ FX̂ - return V̂, X̂0 + return V̂, X̂0, A0 end @doc raw""" predict_mhe!( - V̂, X̂0, Û0, K̄, Ŷ0, + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, Ŵ, _ - ) -> V̂, X̂0 + x̂0arr, a0arr, Ŵ, Z̃ + ) -> V̂, X̂0, A0 Compute the vectors when `model` is a [`NonLinModel`](@ref) with [`SingleShooting`](@ref). -The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The augmented model of -[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, -and by adding the estimated process noise ``\mathbf{ŵ}``. +The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The vector `A0` is +ignored and returned unchanged. The augmented model of [`f̂!`](@ref) and [`ĥ!`](@ref) is +called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated +process noise ``\mathbf{ŵ}``. """ function predict_mhe!( - V̂, X̂0, Û0, K̄, Ŷ0, + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, Ŵ, _ + x̂0arr, _ , Ŵ, _ ) nu, nd, ny, nk̄ = model.nu, model.nd, model.ny, model.nk̄ nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] @@ -1377,42 +1460,46 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0 + return V̂, X̂0, A0 end @doc raw""" predict_mhe!( - V̂, X̂0, _ , _ , Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, ::TranscriptionMethod, - x̂0arr , _ , Z̃ - ) -> V̂, X̂0 + V̂, X̂0, A0, _ , _ , Ŷ0, + estim::MovingHorizonEstimator, model::SimModel, ::TranscriptionMethod, + x̂0arr, a0arr, _ , Z̃ + ) -> V̂, X̂0, A0 -Compute the vectors when `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). +Compute the vectors for all other cases. The function mutates `V̂`, `X̂0`, and `Ŷ0` vector arguments. The augmented output function [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( - V̂, X̂0, _ , _ , Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::TranscriptionMethod, - x̂0arr, _ , Z̃ + V̂, X̂0, A0, _ , _ , Ŷ0, + estim::MovingHorizonEstimator, model::SimModel, transcription::TranscriptionMethod, + x̂0arr, a0arr, _ , Z̃ ) - nd, ny = model.nd, model.ny + nd, ny, na = model.nd, model.ny, get_na(model) nx̂, nε, nym, Nk = estim.nx̂, estim.nε, estim.nym, estim.Nk[] nx̃ = nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads - X̂0[1:nx̂*Nk] .= @views Z̃[(nx̃+1):(nx̃+nx̂*Nk)] + X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] + A0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] @threadsif h_threads for j=1:Nk if estim.direct x̂0 = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] + a0 = @views A0[(1+na*(j-1)):(na*j)] else x̂0 = @views j < 2 ? x̂0arr[1:nx̂] : X̂0[(1+nx̂*(j-2)):(nx̂*(j-1))] + a0 = @views j < 2 ? a0arr[1:na] : A0[(1+na*(j-2)):(na*(j-1))] end d0 = @views estim.D0[(1+nd*j):(nd*(j+1))] # the 1st nd elements are not needed here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] v̂ = @views V̂[(1 + nym*(j-1)):(nym*j)] y0m = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] - ĥ!(ŷ0, estim, model, x̂0, d0) + ĥ_dae!(ŷ0, estim, model, x̂0, a0, d0) ŷ0m = @views ŷ0[estim.i_ym] if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] @@ -1423,40 +1510,7 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0 -end - - -""" - con_nonlinprog_mhe!( - g, estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod, _ , V̂, gc, ε - ) -> g - -Nonlinear MHE constraint when `model` is [`NonLinModel`](@ref) with non-[`SingleShooting`](@ref). - -The method mutates the `g` vectors in argument and returns it. The estimated sensor noises -and custom constraints are included in the `g` vector. -""" -function con_nonlinprog_mhe!( - g, estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod, _ , V̂, gc, ε -) - nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] - for i in eachindex(g) - estim.con.i_g[i] || continue - if i ≤ nV̂con - j = i - jcon = nV̂con-nV̂+j - g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] - elseif i ≤ 2nV̂con - j = i - nV̂con - jcon = nV̂con-nV̂+j - g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] - else - j = i - 2nV̂con - g[i] = gc[j] - end - end - return g + return V̂, X̂0, A0 end """ @@ -1519,11 +1573,43 @@ function con_nonlinprog_mhe!( return g end +""" + con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod, _ , V̂, gc, ε + ) -> g + +Nonlinear MHE constraint for all other cases. + +The method mutates the `g` vectors in argument and returns it. The estimated sensor noises +and custom constraints are included in the `g` vector. +""" +function con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod, _ , V̂, gc, ε +) + nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] + for i in eachindex(g) + estim.con.i_g[i] || continue + if i ≤ nV̂con + j = i + jcon = nV̂con-nV̂+j + g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] + elseif i ≤ 2nV̂con + j = i - nV̂con + jcon = nV̂con-nV̂+j + g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] + else + j = i - 2nV̂con + g[i] = gc[j] + end + end + return g +end + @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, - x̂0arr, Ŵ, Z̃ + x̂0arr, _ , Ŵ, Z̃ ) -> geq Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). @@ -1539,9 +1625,9 @@ for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). """ function con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, - x̂0arr, Ŵ, Z̃ + x̂0arr, _ , Ŵ, Z̃ ) nu, nx, nd, nk̄ = model.nu, model.nx, model.nd, model.nk̄ nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He @@ -1575,97 +1661,136 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, ::TrapezoidalCollocation, - x̂0arr, Ŵ, Z̃ + geq, _ , _ , Û0, K̄, + estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, + x̂0arr, a0arr, Ŵ, Z̃ ) -> geq -Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). +Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalCollocation`](@ref). By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the deterministic state defects are computed with: ```math -\mathbf{ŝ_d}(ℓ+j+1) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇}_1(ℓ+j) + \mathbf{k̇}_2(ℓ+j)] - + \mathbf{ŵ_d}(ℓ+j) - \mathbf{x̂_d}(ℓ+j+1) +\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇_1}(ℓ+j) + \mathbf{k̇_2}(ℓ+j)] + - \mathbf{x̂_d}(ℓ+j+1) + \mathbf{ŵ_d}(ℓ+j) ``` for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the deterministic state and process noise estimates, respectively, extracted from the decision -variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continuous-time -function `model.f!` and: +variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continuous-time +function [`fq_dae!`](@ref) and: ```math \begin{aligned} -\mathbf{k̇}_1(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d̂_0}(ℓ+j), \mathbf{p}\Big) \\ -\mathbf{k̇}_2(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d̂_0}(ℓ+j+1), \mathbf{p}\Big) +\mathbf{k̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇_2}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂!`](@ref) documentation. +is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`](@ref) are +also computed from ``j = 0, 1, ... , N_k-1`` and: +```math +\begin{aligned} +\mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q_2}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) +\end{aligned} +``` +and also one more residual at the arrival: +```math +\mathbf{q_0}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{a_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) +``` """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::TrapezoidalCollocation, - x̂0arr, Ŵ, Z̃ + geq, _ , _ , Û0, K̄, + estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, + x̂0arr, a0arr, Ŵ, Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads Ts = model.Ts + na = get_na(model) nk̄ = get_nk̄(model, transcription) - nw = nŵ - nxs - nx̃ = estim.nε + nx̂ - p = estim.direct ? 0 : 1 - X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] - Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + nw = nŵ - nxs + nx̃ = estim.nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He + nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He + nX, nA = nx*He, na*He + i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 + X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)] + A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] + Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nA)] + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + Ŝk̄ = @views geq[1:nX] + q0arr = @views geq[(nX + 1):(nX + na)] + Q0 = @views geq[(nX + na + 1):(nX + na + nA)] + Q̄ = @views geq[(nX + na + nA + 1):end] + if na > 0 + k̇0, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] + û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + i_d0arr):(nd + i_d0arr)] + fq_dae!(k̇0, q0arr, model, x̂darr, a0arr, û0arr, d0arr) + end @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] + a0 = @views a0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end - d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û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)] + q1 = @views Q̄[(1 + na*(j-1)):(na*j)] + q2 = @views Q0[(1 + na*(j-1)):(na*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] - d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + x̂dnext = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + ā = @views Ā_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] + ŝk = @views Ŝk̄[(1 + nx*(j-1)):(nx*j)] + k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2nx] + if estim.direct || j ≥ Nk + d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + else + d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + end 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) + fq_dae!(k̇1, q1, model, x̂d_Z̃, ā, û0, d0) 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))] # k̇2 of the last iter. j-1 + q1 .= @views Q0[(1 + na*(j-1)-na):(na*(j-1))] # q2 of the last iter. j-1 end if h < 1 - model.f!(k̇2, x̂dnext_Z̃, û0, d0next, model.p) + fq_dae!(k̇2, q2, model, x̂dnext, a0, û0, d0next) else # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - model.f!(k̇2, x̂dnext_Z̃, û0next, d0next, model.p) + fq_dae!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) end - ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) - ŝdnext .+= ŵd + ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd + end + if Nk < He + Ŝk̄[(nx*Nk + 1):end] .= 0 + Q0[(na*Nk + 1):end] .= 0 + Q̄[(na*Nk + 1):end] .= 0 end - Nk < He && (geq[nx*Nk+1:end] .= 0) return geq end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, ::OrthogonalCollocation, - x̂0arr, _ , Z̃ + geq, _ , _ , Û0, K̄, + estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, + x̂0arr, a0arr, _ , Z̃ ) -> geq -Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). +Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`OrthogonalCollocation`](@ref). By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the defects between the deterministic state derivative at the ``n_o`` collocation points and the model dynamics are computed by: ```math -\mathbf{ŝ_k}(ℓ+j) +\mathbf{ŝ_k̄}(ℓ+j) = \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ \mathbf{k}_2(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ @@ -1685,9 +1810,9 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::OrthogonalCollocation, - x̂0arr, _ , Z̃ + geq, _ , _ , Û0, K̄, + estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, + x̂0arr, a0arr, _ , Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, He = estim.nx̂, estim.He @@ -1711,7 +1836,11 @@ function con_nonlinprogeq_mhe!( 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)] - d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + if estim.direct || j ≥ Nk + d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + else + d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + end # ----------------- collocation constraint defects ----------------------------- Δk = k̄ for i=1:no @@ -1742,4 +1871,4 @@ function con_nonlinprogeq_mhe!( end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModelODE, ::TranscriptionMethod, _,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 087d71d37..d0d5c1f66 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -22,6 +22,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} yname::Vector{String} dname::Vector{String} xname::Vector{String} + xs_0::Vector{NT} buffer::SimModelBuffer{NT} function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real} A, Bu = to_mat(A, 1, 1), to_mat(Bu, 1, 1) @@ -44,6 +45,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + xs_0 = zeros(NT, nx) uname = ["\$u_{$i}\$" for i in 1:nu] yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] @@ -60,6 +62,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, + xs_0, buffer ) end diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 9e7072980..e9f6b2ab7 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -44,6 +44,7 @@ struct NonLinModel{ yname::Vector{String} dname::Vector{String} xname::Vector{String} + xs_0::Vector{NT} jacobian::JB linfunc!::LF buffer::SimModelBuffer{NT} @@ -65,6 +66,7 @@ struct NonLinModel{ dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + xs_0 = zeros(NT, nx) uname = ["\$u_{$i}\$" for i in 1:nu] yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] @@ -83,6 +85,7 @@ struct NonLinModel{ nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, + xs_0, jacobian, linfunc!, buffer ) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 94715ffb6..04e091481 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -400,6 +400,19 @@ function validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) return iszero_Ha end +"Get the number of algebraic variable `na` in `model`." +get_na(model::NonLinModelDAE) = model.na +get_na(::SimModel) = 0 + +"Get length of the `ā` vector with all the algebraic variable collocation pts (excl. τ=1)." +get_nā(model::NonLinModelDAE, transcription::OrthogonalCollocation) = model.na*transcription.no +get_nā(model::NonLinModelDAE, ::TrapezoidalCollocation) = model.na +get_nā(::SimModel, ::TranscriptionMethod) = 0 + +"Get the warm-starting value for the algebraic variable `as_0`." +get_as_0(model::NonLinModelDAE) = model.as_0 +get_as_0(model::SimModel) = model.buffer.a + "Get the number of elements in the optimization decision vector `Z` for DAE solving." function get_nZ_dae(transcription::OrthogonalCollocation, nx, na) return nx + transcription.no*nx + na + transcription.no*na @@ -656,26 +669,40 @@ function update_predictions!(k̄, geq, model, Z) return nothing end +""" + con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z + ) -> geq + +TBW +""" function con_nonlinprogeq!( geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z ) nx, na = model.nx, model.na Ts = model.Ts x0next_Z, a0_Z, a0next_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] - sknext, q1, q2 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] - k̇1, k̇2 = @views k̄[1:nx], k̄[(nx+1):(2nx)] - model.fq!(k̇1, q1, x0, a0_Z, u0, d0, model.p) - model.fq!(k̇2, q2, x0next_Z, a0next_Z, u0, d0, model.p) - sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇1 + k̇2) + sknext, q0, q1 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] + k̇0, k̇1 = @views k̄[1:nx], k̄[(nx+1):(2nx)] + model.fq!(k̇0, q0, x0, a0_Z, u0, d0, model.p) + model.fq!(k̇1, q1, x0next_Z, a0next_Z, u0, d0, model.p) + sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end +""" + con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z + ) -> geq + +TBW +""" function con_nonlinprogeq!( geq, k̄, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z ) nx, na = model.nx, model.na Mo, no = model.Mo, transcription.no - nk̄, nā = get_nk̄(model, transcription), no*na + nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) a0_Z, k̄_Z, ā_Z = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] q0, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] @views model.fq!(k̄[1:nx], q0, x0, a0_Z, u0, d0, model.p) diff --git a/src/sim_model.jl b/src/sim_model.jl index b0f4a6f32..33dbc27d6 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -377,7 +377,7 @@ function validate_args(model::SimModel, d, u=nothing) end end -"Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." +"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 diff --git a/src/transcription.jl b/src/transcription.jl index 375609c50..6875610af 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -182,14 +182,20 @@ transcription method. \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ - \mathbf{a_0}(k+0) \\ - \mathbf{A_0} \end{bmatrix} - \quad \text{and} \quad + \mathbf{A_0} \\ + \mathbf{Ā} \end{bmatrix} + , \: \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k+1) \\ \mathbf{a_0}(k+2) \\ \vdots \\ \mathbf{a_0}(k+H_p) \end{bmatrix} + \: \text{and} \: + \mathbf{Ā} = \begin{bmatrix} + \mathbf{a}(k+0) \\ + \mathbf{a}(k+1) \\ + \vdots \\ + \mathbf{a}(k+H_p-1) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math @@ -197,21 +203,31 @@ transcription method. \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ - \mathbf{a_0}(k-N_k+p) \\ + \mathbf{a_0}(k-N_k+p) \\ \mathbf{A_0} \\ \mathbf{0_a} \\ + \mathbf{Ā} \\ + \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} - \quad \text{and} \quad + , \: \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+2) \\ \vdots \\ \mathbf{a_0}(k+p) \end{bmatrix} + \: \text{and} \: + \mathbf{Ā} = \begin{bmatrix} + \mathbf{a}(k-N_k+p+0) \\ + \mathbf{a}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{a}(k+p-1) \end{bmatrix} ``` See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision - variables at the beginning (``N_k < He``). + variables at the beginning (``N_k < H_e``). The predicted outputs are computed from + the algebraic variables in ``\mathbf{A_0}``, while the values in ``\mathbf{Ā}`` are + strictly reserved for the the `fq!` function. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -310,10 +326,10 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). algebraic variables at the collocation points: ```math \mathbf{ā}(k+j) = \begin{bmatrix} - \mathbf{ā}_1(k+j) \\ - \mathbf{ā}_2(k+j) \\ + \mathbf{a}_1(k+j) \\ + \mathbf{a}_2(k+j) \\ \vdots \\ - \mathbf{ā}_{n_o}(k+j) \end{bmatrix} + \mathbf{a}_{n_o}(k+j) \end{bmatrix} ``` Although not strictly needed, the current algebraic variable ``\mathbf{a_0}(k+0)`` is still included in the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): @@ -332,7 +348,13 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{A_0} \\ \mathbf{K̄} \\ \mathbf{Ā} \end{bmatrix} - \quad \text{and} \quad + , \: + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k+1) \\ + \mathbf{a_0}(k+2) \\ + \vdots \\ + \mathbf{a_0}(k+H_p) \end{bmatrix} + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k+0) \\ \mathbf{ā}(k+1) \\ @@ -345,6 +367,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p) \\ \mathbf{A_0} \\ \mathbf{0_a} \\ \mathbf{K̄} \\ @@ -353,16 +376,23 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} - \quad \text{and} \quad + , \; + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+2) \\ + \vdots \\ + \mathbf{a_0}(k+p) \end{bmatrix} + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k-N_k+p+0) \\ \mathbf{ā}(k-N_k+p+1) \\ \vdots \\ \mathbf{ā}(k+p-1) \end{bmatrix} ``` - See the Extended Help of [`TrapezoidalCollocation`](@ref) for the exact definition of - ``\mathbf{A_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with - zeros for the unused decision variables at the beginning (``N_k < H_e``). + All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at + the beginning in the [`MovingHorizonEstimator`](@ref) (``N_k < H_e``). The predicted + outputs are computed from the algebraic variables in ``\mathbf{A_0}``, while the values + in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. 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``. diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 26dae5bda..559792804 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -566,7 +566,7 @@ end @test evaloutput(dae) ≈ zeros(1) atol=1e-6 transcription = TrapezoidalCollocation() - dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, as_0, xs_0) + dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, xs_0, as_0) @test updatestate!(dae2, u) ≈ zeros(1) atol=1e-6 @test updatestate!(dae2, u, d) ≈ zeros(1) atol=1e-6 @test dae2.x0 ≈ zeros(1) atol=1e-6 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b46cc981a..f4100d87c 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1383,6 +1383,59 @@ end @test mhe11() ≈ [13] atol=5e-3 end +@testitem "MHE estim. & getinfo (NonLinModelDAE, TC)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer + import ForwardDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = 2 * x[] + a[] + 0.1 * d[] + end + Ts, p = 100.0, [0.01] + as_0, xs_0 = [-1.0], [1.0] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + + transcription = TrapezoidalCollocation() + mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) + preparestate!(mhe, [0.0], [0.0]) + x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) + @test x̂ ≈ zeros(mhe.nx̂) atol=1e-6 + @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-6 + preparestate!(mhe, [0], [0]) + info = getinfo(mhe) + @test info[:x̂] ≈ x̂ atol=1e-6 + @test info[:Ŷ][end] ≈ 0 atol=1e-6 + for i in 1:40 + preparestate!(mhe, [0], [0]) + updatestate!(mhe, [3.0], [0], [0]) + end + preparestate!(mhe, [0], [0]) + @test mhe([0]) ≈ [0] atol=1e-3 + for i in 1:40 + preparestate!(mhe, [7.0], [0]) + updatestate!(mhe, [0], [7.0], [0]) + end + preparestate!(mhe, [7.0], [0]) + @test mhe([0]) ≈ [7.0] atol=1e-3 + + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) + preparestate!(mhe2, [0.0], [0.0]) + x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 + initstate!(mhe2, [0], [0], [0]) + @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 +end + @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin f(x,u,_,_) = 0.5x + u h(x,_,_) = x