diff --git a/Project.toml b/Project.toml index 7b4a641d2..8a146db26 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.13.2" +version = "2.14.0" authors = ["Francis Gagnon"] [deps] diff --git a/README.md b/README.md index 0b5a96b4d..706043774 100644 --- a/README.md +++ b/README.md @@ -82,7 +82,7 @@ for more detailed examples. ### 🎯 Model Predictive Control Features -- 🏭️ **Plant Model**: Linear or nonlinear models exploiting multiple dispatch. +- 🏭️ **Plant Model**: Linear, nonlinear ODE and nonlinear DAE models exploiting multiple dispatch. - ⛳️ **Objectives**: Tracking for inputs/outputs, move suppression, terminal costs, and economic costs. - ⏳️ **Horizons**: Distinct prediction/control horizons with custom move blocking. - 📸 **Linearization**: Auto-differentiation for exact Jacobians. diff --git a/docs/src/internals/sim_model.md b/docs/src/internals/sim_model.md index 820d9d870..31bf5549b 100644 --- a/docs/src/internals/sim_model.md +++ b/docs/src/internals/sim_model.md @@ -22,6 +22,8 @@ ModelPredictiveControl.init_defectmat_dae ```@docs ModelPredictiveControl.f! ModelPredictiveControl.h! +ModelPredictiveControl.fq_dae! +ModelPredictiveControl.h_dae! ``` ## Init State diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 9313eb8c0..359299bfe 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -67,8 +67,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! -ModelPredictiveControl.fq_dae! -ModelPredictiveControl.ĥ_dae! ``` ## Remove Operating Points diff --git a/docs/src/manual/nonlinmpc2.md b/docs/src/manual/nonlinmpc2.md index 068ec43f3..63a724903 100644 --- a/docs/src/manual/nonlinmpc2.md +++ b/docs/src/manual/nonlinmpc2.md @@ -4,6 +4,10 @@ Pages = ["nonlinmpc2.md"] ``` +!!! todo "Under Construction" + This tutorial is currently under construction. Only the modeling part is written for + now. + ## Nonlinear Model (DAE) In this example, the goal is to control the pH of a solution in a continuously stirred diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index ebb3c8102..135041760 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -1,5 +1,3 @@ -const DEFAULT_LINMPC_TRANSCRIPTION = SingleShooting() - struct LinMPC{ NT<:Real, SE<:StateEstimator, @@ -241,7 +239,7 @@ function LinMPC( Wd = nothing, Wr = nothing, Cwt = DEFAULT_CWT, - transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, + transcription::ShootingMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_QP_OPTIMIZER, add_bridges=true), kwargs... ) @@ -300,7 +298,7 @@ function LinMPC( Wd = nothing, Wr = nothing, Cwt = DEFAULT_CWT, - transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, + transcription::ShootingMethod = SingleShooting(), optim::JM = JuMP.Model(DEFAULT_QP_OPTIMIZER, add_bridges=true) ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 18294ac07..f7f2749c2 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,4 +1,3 @@ -const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() const DEFAULT_NONLINMPC_HESSIAN = AutoSparse( AutoForwardDiff(); sparsity_detector=TracerSparsityDetector(), @@ -365,7 +364,7 @@ function NonLinMPC( gc ::Function = gc!, nc::Int = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, + transcription::TranscriptionMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -381,9 +380,6 @@ function NonLinMPC( ) end -default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) -default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) - """ NonLinMPC(estim::StateEstimator; ) @@ -441,7 +437,7 @@ function NonLinMPC( gc ::Function = gc!, nc = 0, p = estim.model.p, - transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, + transcription::TranscriptionMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -467,6 +463,9 @@ function NonLinMPC( ) end +default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) +default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) + """ validate_JE(NT, JE) -> nothing diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 400383ac1..4541155d0 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -3,13 +3,14 @@ struct StateEstimatorBuffer{NT<:Real} û ::Vector{NT} k̄ ::Vector{NT} x̂ ::Vector{NT} - a ::Vector{NT} + â ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} Ŵ ::Vector{NT} X̂ ::Vector{NT} - A ::Vector{NT} +  ::Vector{NT} Ŷ ::Vector{NT} + U ::Vector{NT} D ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} @@ -38,18 +39,19 @@ function StateEstimatorBuffer{NT}( transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real 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) + nV̂, nŴ, nX̂, nÂ, nŶ, nU, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nu*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) + â = 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Â) Ŷ = Vector{NT}(undef, nŶ) + U = Vector{NT}(undef, nU) D = Vector{NT}(undef, nD) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -60,7 +62,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, â, Z̃, V̂, Ŵ, X̂, Â, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index ce21c5f21..4b59ce8ea 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -98,12 +98,12 @@ Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument """ 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 + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + @views x̂dnext, x̂snext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] + mul!(û0, Cs_u, x̂s) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - f!(xdnext, k̄, model, xd, û0, d0, model.p) - mul!(xsnext, As, xs) + f!(x̂dnext, k̄, model, x̂d, û0, d0, model.p) + mul!(x̂snext, As, x̂s) x̂0next .+= f̂op .- x̂op return nothing end @@ -135,42 +135,25 @@ Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument """ 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) - mul!(ŷ0, Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + h!(ŷ0, model, x̂d, d0, model.p) # y0 = h(xd, d0) + mul!(ŷ0, Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end """ - fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) + ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) -Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). +Same than [`ĥ!`](@ref) for [`SimModel`](@ref), but call [`h_dae!`](@ref) internally. """ -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) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â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] - model.h!(ŷ0, xd, a0, d0, model.p) - mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + h_dae!(ŷ0, model, x̂d, â0, d0) + mul!(ŷ0, estim.Cs_y, x̂s, 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.jl b/src/estimator/mhe.jl index 5bfbe7b68..be1843d28 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -3,7 +3,7 @@ include("mhe/execute.jl") include("mhe/transcription.jl") "Return estimation horizon He and slack variables length nε for `MovingHorizonEstimator`." -get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε) +get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε, get_na(estim.model)) "Print optimizer and other information for `MovingHorizonEstimator`." function print_details(io::IO, estim::MovingHorizonEstimator) @@ -27,11 +27,13 @@ print_backends(::IO, ::MovingHorizonEstimator, ::LinModel) = nothing function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n; firstchars=nothing) nu, nd = estim.model.nu, estim.model.nd nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu + na = get_na(estim.model) He, nε = estim.He, estim.nε niu, niym = sum(estim.nint_u), sum(estim.nint_ym) println(io, " │ ├$(lpad(He, n)) estimation steps He") println(io, " │ ├$(lpad(nu, n)) manipulated inputs u ($niu integrating states)") println(io, " │ ├$(lpad(nx̂, n)) estimated states x̂") + println(io, " │ ├$(lpad(na, n)) algebraic variables a") println(io, " │ ├$(lpad(nym, n)) measured outputs ym ($niym integrating states)") println(io, " │ ├$(lpad(nyu, n)) unmeasured outputs yu") print(io, " │ └$(lpad(nd, n)) measured disturbances d") diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 914ed89f9..b0ade0423 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1,4 +1,3 @@ -const DEFAULT_MHE_TRANSCRIPTION = SingleShooting() const DEFAULT_NONLINMHE_HESSIAN = AutoSparse( AutoForwardDiff(); sparsity_detector=TracerSparsityDetector(), @@ -110,6 +109,7 @@ struct MovingHorizonEstimator{ nym::Int nyu::Int nxs::Int + nx̃::Int p::PT Mo::SparseMatrixCSC{NT, Int} Co::SparseMatrixCSC{NT, Int} @@ -173,15 +173,15 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator{NT} } nu, ny, nd = model.nu, model.ny, model.nd + validate_transcription(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) - validate_transcription(model, transcription) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs - nŵ = nx̂ + nŵ = nx̂ Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) @@ -204,6 +204,7 @@ struct MovingHorizonEstimator{ ES, GS, JS, BS, gc!, nc ) + nx̃ = nx̂ + 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) @@ -232,7 +233,7 @@ struct MovingHorizonEstimator{ covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nε, - i_ym, nx̂, nym, nyu, nxs, + i_ym, nx̂, nym, nyu, nxs, nx̃, p, Mo, Co, λo, As, Cs_u, Cs_y, nint_u, nint_ym, @@ -258,7 +259,7 @@ end Construct a moving horizon estimator (MHE) based on `model`. -It supports ([`LinModel`](@ref), [`NonLinModel`](@ref)), [`NonLinModelDAE`](@ref)) and +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, @@ -346,7 +347,8 @@ at each time step for the optimization. (details in Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). -- `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. +- `transcription=default_transcription(model)` : a [`TranscriptionMethod`](@ref) object, + default to [`SingleShooting`](@ref) or `model.transcription` for [`NonLinModelDAE`](@ref). - `optim=default_optim_mhe(model,nc)` : a [`JuMP.Model`](@extref) object with a quadratic or nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). @@ -380,6 +382,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 5.0 s: │ ├ 5 estimation steps He │ ├ 1 manipulated inputs u (0 integrating states) │ ├ 2 estimated states x̂ + │ ├ 0 algebraic variables a │ ├ 1 measured outputs ym (1 integrating states) │ ├ 0 unmeasured outputs yu │ └ 0 measured disturbances d @@ -557,7 +560,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc ::Int = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_MHE_TRANSCRIPTION, + transcription::TranscriptionMethod = default_transcription(model), optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -590,7 +593,7 @@ end gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing, gc=gc!, nc=0, - transcription=SingleShooting(), + transcription=default_transcription(model), optim=default_optim_mhe(model, nc), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), @@ -611,7 +614,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_MHE_TRANSCRIPTION, + transcription::TranscriptionMethod = default_transcription(model), optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -843,6 +846,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 1.0 s: │ ├ 3 estimation steps He │ ├ 1 manipulated inputs u (0 integrating states) │ ├ 2 estimated states x̂ + │ ├ 0 algebraic variables a │ ├ 1 measured outputs ym (1 integrating states) │ ├ 0 unmeasured outputs yu │ └ 0 measured disturbances d @@ -1445,13 +1449,13 @@ function get_nonlinobj_op( nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) - nŴ, nV̂, nX̂, nA, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) + nŴ, nV̂, nX̂, nÂ, 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) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nÂ) Ŵ::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) @@ -1460,16 +1464,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, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵ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(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1565,13 +1569,13 @@ function get_nonlincon_oracle( 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̂, nA, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) + nŴ, nV̂, nX̂, nÂ, 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) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nÂ) Ŵ::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) @@ -1583,26 +1587,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, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵ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, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵ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(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1615,8 +1619,8 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), @@ -1665,24 +1669,24 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end function ℓ_geq( - Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵ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(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) @@ -1694,8 +1698,8 @@ function get_nonlincon_oracle( ∇geq_structure = init_diffstructure(∇geq) if !isnothing(hess) ∇²geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), 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 d57d03eaa..7d5d43075 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -36,75 +36,6 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) 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 - """ correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) @@ -168,12 +99,14 @@ following fields: - `:ε` or *`:epsilon`* : optimal slack variable, ``ε`` - `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k``, ``\mathbf{X̂}`` - `:x̂` or *`:xhat`* : optimal estimated state, ``\mathbf{x̂}_k(k+p)`` +- `:Â` or *`:Ahat`* : optimal estimated algebraic variables over ``N_k``, ``\mathbf{Â}`` - `:V̂` or *`:Vhat`* : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}`` - `:P̄` or *`:Pbar`* : estimation error covariance at arrival, ``\mathbf{P̄}`` - `:x̄` or *`:xbar`* : optimal estimation error at arrival, ``\mathbf{x̄}`` - `:Ŷ` or *`:Yhat`* : optimal estimated outputs over ``N_k``, ``\mathbf{Ŷ}`` - `:Ŷm` or *`:Yhatm`* : optimal estimated measured outputs over ``N_k``, ``\mathbf{Ŷ^m}`` - `:x̂arr` or *`:xhatarr`* : optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+p)`` +- `:âarr` or *`:ahatarr`* : optimal estimated algebraic variable at arrival, ``\mathbf{â}(k-N_k+p)`` - `:J` : objective value optimum, ``J`` - `:Ym` : measured outputs over ``N_k``, ``\mathbf{Y^m}`` - `:U` : manipulated inputs over ``N_k``, ``\mathbf{U}`` @@ -215,20 +148,20 @@ 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, get_nk̄(model, estim.transcription) - nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ + nx̂, nym, nŵ, na = estim.nx̂, estim.nym, estim.nx̂, get_na(model) Z̃ = estim.Z̃ info = Dict{Symbol, Any}() - 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) + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + x̂0arr, â0arr = buffer.x̂, buffer.â + x̄, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nk̄*Nk) x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + â0arr = getâ0arr!(â0arr, 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̃ + V̂, X̂0, Â0 = predict_mhe!( + V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) - Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, A0, x̂0arr, a0arr) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, Â0, x̂0arr, â0arr) 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)] @@ -245,6 +178,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŵ] = Ŵ[1:nŵ*Nk] info[:ε] = getslack(estim, Z̃) info[:X̂] = X̂[1:nx̂*Nk] + info[:Â] = Â0[1:na*Nk] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂[1:nym*Nk] info[:P̄] = estim.P̂arr_old @@ -252,6 +186,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŷ] = Ŷ[1:ny*Nk] info[:Ŷm] = Ŷ[vec(estim.i_ym .+ ny.*(0:Nk-1)')] info[:x̂arr] = x̂arr + info[:âarr] = â0arr info[:J] = J info[:Ym] = Ym info[:U] = U @@ -260,9 +195,10 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real # --- non-Unicode fields --- info[:What] = info[:Ŵ] info[:xhatarr] = info[:x̂arr] + info[:ahatarr] = info[:âarr] info[:epsilon] = info[:ε] info[:Xhat] = info[:X̂] - info[:xhat] = info[:x̂] + info[:Ahat] = info[:Â] info[:Vhat] = info[:V̂] info[:Pbar] = info[:P̄] info[:xbar] = info[:x̄] @@ -291,11 +227,11 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher 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̂, nA, nŴ = He*nym, He*nx̂, He*na, He*nx̂ + nV̂, nX̂, nÂ, 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) + â0arr, Â0 = zeros(NT, na), zeros(NT, nÂ) Ŵ = zeros(NT, nŴ) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) Ŵe = zeros(NT, nŴe) @@ -306,15 +242,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher geq = zeros(NT, neq) gi = zeros(NT, ngi) J_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -329,15 +265,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- inequality constraint derivatives --- ∇g_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing @@ -361,18 +297,18 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²g_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(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 + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -385,15 +321,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- equality constraint derivatives --- ∇geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end @@ -416,17 +352,17 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) function ℓ_geq( - Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -547,15 +483,15 @@ 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̃ +"Get algebraic variable at arrival `â0arr` from the decision vector `Z̃`." +function getâ0arr!( + â0arr, 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)] + return â0arr .= @views Z̃[(nx̃_nX̂ + 1):(nx̃_nX̂ + na)] end -geta0arr!(a0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = a0arr +getâ0arr!(â0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = â0arr "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) @@ -729,15 +665,15 @@ 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, nx̂, Nk = model.nu, estim.nx̂, estim.Nk[] + nx̂, Nk = 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 + x̂0arr, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getx̂0arr!(x̂0arr, estim, Z̃) predict_mhe!( - V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing @@ -936,40 +872,40 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ end @doc raw""" - predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) -> Ŷ0 + predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, Â0, x̂0arr, â0arr) -> Ŷ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 arguments `X̂0` and `A0` are computed from [`predict_mhe!`](@ref) and +`Ŷ0` vector. The arguments `X̂0` and `Â0` 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). +argument `x̂0arr` and `â0arr` are computed from [`getx̂0arr!`](@ref) and [`getâ0arr!`](@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, A0, x̂0arr, a0arr) +function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, Â0, x̂0arr, â0arr) 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] + â0 = @views estim.direct ? Â0[1:na] : â0arr[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) + ĥ!(ŷ0, estim, estim.model, x̂0, â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))] + â0 = @views Â0[(1 + na*(j-p)):(na*(j-p+1))] end return Ŷ0 end """ update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -978,23 +914,23 @@ 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, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + â0arr = getâ0arr!(â0arr, 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̃ + V̂, X̂0, Â0 = predict_mhe!( + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, â0arr, Ŵ, 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, A0, Û0, K̄, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + geq, X̂0, Â0, Û0, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 2a7969a11..3416f0fbc 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -11,28 +11,26 @@ 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[] + nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.Nk[] na = get_na(estim.model) - nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk - nx̃ = estim.nε + nx̂ + nŴ, nX̂, n = nŵ*Nk, nx̂*Nk, na*Nk 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 + na + nA) - (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nA) + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nÂ) + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nÂ) (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[] + nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.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̂ + nŴ, nX̂, nÂ, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk 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 @@ -40,7 +38,7 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo 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 + na + nA); + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nÂ); (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Ŵ); @@ -48,8 +46,7 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo return i_Z̃_NK end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::SingleShooting) - nŵ, Nk = estim.nx̂, estim.Nk[] - nx̃ = estim.nε + estim.nx̂ + nx̃, nŵ, Nk = estim.nx̃, estim.nx̂, estim.Nk[] return (1):(nx̃ + nŵ*Nk) end @@ -1052,6 +1049,68 @@ end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing +""" + 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̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He + 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̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) + 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, ::OrthogonalCollocation) + model = estim.model + nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) + 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*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 + @doc raw""" set_warmstart_mhe!( estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var @@ -1090,7 +1149,6 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu 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 @@ -1103,14 +1161,14 @@ 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̄, 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 = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + K = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation 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, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1149,14 +1207,14 @@ The warm-starting value is provided in Extended Help. \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) \\ + \mathbf{â_0}(k-N_k+p|k-1) \\ + \mathbf{â_0}(k-N_k+p+1|k-1) \\ + \mathbf{â_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{â_0}(k-p-2|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{a_0}(k-p-1|k-1) \\ - \mathbf{0_a} \\ + \mathbf{0_â} \\ \mathbf{k̄}(k-N_k+p+0|k-1) \\ \mathbf{k̄}(k-N_k+p+1|k-1) \\ \vdots \\ @@ -1182,19 +1240,20 @@ The warm-starting value is provided in Extended Help. ``` 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_ŵ}``. + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` and ``\mathbf{ā}(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_â}``, ``\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, na = model.nu, get_na(model) + na = 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̂, nA, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He + nx̃, nŴ, nX̂, nÂ, 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]) @@ -1205,32 +1264,31 @@ function set_warmstart_mhe!( 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 --- + # --- algebraic variables â0arr and Â0 --- 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)] + Z̃s[(i_base+1):(i_base+nÂ)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nÂ)] + Z̃s[(i_base+nÂ+1):(i_base+na+nÂ)] .= @views estim.Z̃[(i_base+nÂ+1):(i_base+na+nÂ)] # --- deterministic states at collocation points K̄ --- - i_base = nx̃ + nX̂ + na + nA + i_base = nx̃ + nX̂ + na + n 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̄ + i_base = nx̃ + nX̂ + na + n + 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 Ŵ --- - i_base = nx̃ + nX̂ + na + nA + nK̄ + nĀ + i_base = nx̃ + nX̂ + na + n + 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 - A0, Ā = Vector{NT}(undef, na*Nk), Vector{NT}(undef, nā*Nk) # TODO: remove the 2 allocations + x̄, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) + â0arr = getâ0arr!(â0arr, 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, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) + predict_mhe!(V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 @@ -1268,14 +1326,14 @@ The warm-starting value is provided in Extended Help. \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) \\ + \mathbf{â_0}(k-N_k+p|k-1) \\ + \mathbf{â_0}(k-N_k+p+1|k-1) \\ + \mathbf{â_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{â_0}(k-p-2|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ + \mathbf{0_â} \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ \vdots \\ @@ -1290,31 +1348,42 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu - nk̄ = get_nk̄(estim.model, transcription) + na = 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̂ = nε + nx̂, nŵ*He, nx̂*He + nx̃, nŴ, nX̂, nÂ, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*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̂)] + 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 â0arr and Â0 --- + i_base = nx̃ + nX̂ + Z̃s[(i_base+1):(i_base+nÂ)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nÂ)] + Z̃s[(i_base+nÂ+1):(i_base+na+nÂ)] .= @views estim.Z̃[(i_base+nÂ+1):(i_base+na+nÂ)] + # --- algebraic variables Ā --- + i_base = nx̃ + nX̂ + na + 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):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- process noise estimates Ŵ --- - 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 + i_base = nx̃ + nX̂ + na + n + 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, 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 = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old - a0arr = geta0arr!(a0arr, estim, transcription, Z̃s) + â0arr = getâ0arr!(â0arr, 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, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1331,37 +1400,45 @@ end "Fill the unused decision variables in `Z̃` with `0`s (only when `Nk < He`)." function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::SingleShooting) - nŵ, nx̂, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nx̃ = estim.nε + nx̂ - Z̃[(nx̃ + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nŵ, nx̃, Nk = estim.nx̂, estim.nx̃, estim.Nk[] + Z̃[(1 + nx̃ + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::TranscriptionMethod) - nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*He - Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector - Z̃[(nx̃_nX̂_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nŵ, nx̂, nx̃, He, Nk = estim.nx̂, estim.nx̂, estim.nx̃, estim.He, estim.Nk[] + na = get_na(estim.model) + nx̃_nX̂_He = nx̃ + nx̂*He + nx̃_nX̂_na_nA_He = nx̃ + nx̂*He + na + na*He + nx̃_nX̂_na_nA_nĀ_He = nx̃ + nx̂*He + na + na*He + na*He + Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after Â0 vector + Z̃[(1 + nx̃_nX̂_na_nA_He + na*Nk):(nx̃_nX̂_na_nA_nĀ_He)] .= 0 # unused vars after Ā vector + Z̃[(1 + nx̃_nX̂_na_nA_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end function fill0unused!(Z̃, estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) - nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃ = estim.nε + nx̂ + nŵ, nx̂, nx̃, He, Nk = estim.nx̂, estim.nx̂, estim.nx̃, estim.He, estim.Nk[] + na = get_na(estim.model) nk̄ = get_nk̄(estim.model, transcription) - nx̃_nX̂_He = nx̃ + nx̂*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̂_nK_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nā = transcription.no*na + nx̃_nX̂_He = nx̃ + nx̂*He + nx̃_nX̂_na_nA_He = nx̃ + nx̂*He + na + na*He + nx̃_nX̂_na_nA_nK̄_He = nx̃ + nx̂*He + na + na*He + nk̄*He + nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃ + nx̂*He + na + na*He + nk̄*He + nā*He + Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after Â0 vector + Z̃[(1 + nx̃_nX̂_na_nA_He + nk̄*Nk):(nx̃_nX̂_na_nA_nK̄_He)] .= 0 # unused vars after K̄ vector + Z̃[(1 + nx̃_nX̂_na_nA_nK̄_He + nā*Nk):(nx̃_nX̂_na_nA_nK̄_nĀ_He)] .= 0 # unused vars after Ā vector + Z̃[(1 + nx̃_nX̂_na_nA_nK̄_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end @doc raw""" predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, - x̂0arr, a0arr, Ŵ, Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, Ŵ, Z̃ + ) -> V̂, X̂0, Â0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -1374,10 +1451,10 @@ 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. +The vector `Â0` is ignored and returned unchanged. """ function predict_mhe!( - V̂, X̂0, A0, _ , _ , _ , + V̂, X̂0, Â0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, _ , _ , _ , Z̃ ) @@ -1397,25 +1474,25 @@ 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, A0 + return V̂, X̂0, Â0 end @doc raw""" predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, a0arr, Ŵ, Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, Ŵ, Z̃ + ) -> V̂, X̂0, Â0 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 vector `A0` is +The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The vector `Â0` 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, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, _ , Ŵ, _ ) @@ -1460,15 +1537,15 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0, A0 + return V̂, X̂0, Â0 end @doc raw""" predict_mhe!( - V̂, X̂0, A0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, ::TranscriptionMethod, - x̂0arr, a0arr, _ , Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, _ , Z̃ + ) -> V̂, X̂0, Â0 Compute the vectors for all other cases. @@ -1476,30 +1553,29 @@ The function mutates `V̂`, `X̂0`, and `Ŷ0` vector arguments. The augmented o [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( - V̂, X̂0, A0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, transcription::TranscriptionMethod, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) 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̃, nym, Nk = estim.nx̂, estim.nx̃, estim.nym, estim.Nk[] nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads - 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)] + X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] # skip x0arr + Â0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + na*Nk)] # skip â0arr @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)] + â0 = @views Â0[(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))] + â0 = @views j < 2 ? â0arr[1:na] : Â0[(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)] - ĥ_dae!(ŷ0, estim, model, x̂0, a0, d0) + ĥ!(ŷ0, estim, model, x̂0, â0, 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)] @@ -1509,8 +1585,9 @@ function predict_mhe!( if Nk < estim.He # fill unused values with 0s for tracer sparsity detection: V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 + Â0[na*Nk+1:end] .= 0 end - return V̂, X̂0, A0 + return V̂, X̂0, Â0 end """ @@ -1630,11 +1707,10 @@ function con_nonlinprogeq_mhe!( 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 + nx̂, nx̃, nxs, nŵ, He = estim.nx̂, estim.nx̃, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads 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) @@ -1663,7 +1739,7 @@ end con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, - x̂0arr, a0arr, Ŵ, Z̃ + x̂0arr, â0arr, Ŵ, Z̃ ) -> geq Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalCollocation`](@ref). @@ -1671,7 +1747,7 @@ Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalColloc By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the deterministic state defects are computed with: ```math -\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇_1}(ℓ+j) + \mathbf{k̇_2}(ℓ+j)] +\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇_0}(ℓ+j) + \mathbf{k̇_1}(ℓ+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 @@ -1680,8 +1756,8 @@ variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continu function [`fq_dae!`](@ref) and: ```math \begin{aligned} -\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) +\mathbf{k̇_0}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \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}`` @@ -1689,90 +1765,90 @@ is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`] 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) +\mathbf{q_0}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` -and also one more residual at the arrival: +and also one final residual at `k+p`: ```math -\mathbf{q_0}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{a_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) +\mathbf{q_0}(k+p) = \mathbf{q}\Big(\mathbf{x̂_d}(k+p), \mathbf{â_0}(k+p), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) ``` """ function con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, - x̂0arr, a0arr, Ŵ, Z̃ + x̂0arr, â0arr, Ŵ, 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 + nx̂, nx̃, nxs, nŵ, He = estim.nx̂, 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̂ - nx̃_nX̂ = nx̃ + nx̂*estim.He - nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He - nX, nA = nx*He, na*He + nw = nŵ - nxs + nx̃_nX̂ = nx̃ + nx̂*estim.He + nŜk, nÂ, nĀ = nx*He, na*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 + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + (Nk < He) && (geq .= 0) + Ŝk = @views geq[1:nŜk] + Q0 = @views geq[(1 + nŜk):(nŜk + n + na)] + Q̄ = @views geq[(1 + nŜk + n + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0 = @views a0arr[1:na] + â0_Z̃ = @views â0arr[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)] + â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end 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̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + a1_Z̃ = @views A1_Z̃[(1 + na*(j-1)):(na*j)] + q0 = @views Q0[(1 + na*(j-1)):(na*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 = @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] + x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] + k̇0, k̇1 = @views k̄dot[1:nx], k̄dot[nx+1:2nx] if estim.direct || j < Nk - d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] - else - d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + else # special case, d0(k+1)≈d0(k), since d0(k+1) is not available at time k: + d1 = d0 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) - fq_dae!(k̇1, q1, model, x̂d_Z̃, ā, û0, d0) - else - 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 + # we need to recompute k̇0 & q0 with multi-threading, even with h>0, since + # the previous iteration (j-1) may not be executed (iterations are re-orderable) + computeDynamicsAtBegin = f_threads || h < 1 || j < 2 + if computeDynamicsAtBegin + fq_dae!(k̇0, q0, model, x̂d_Z̃, â0_Z̃, û0, d0) + else # piecewise linear inputs u and disturbances d: + k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) + q0 .= @views â0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] end - if h < 1 - 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))] - fq_dae!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) + if h > 0 && j < Nk + û1 = @views Û0[(1 + nu*j):(nu*(j+1))] + else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k: + û1 = û0 end - ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd + fq_dae!(k̇1, q1, model, x̂dnext_Z̃, a1_Z̃, û1, d1) + ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end - if Nk < He - Ŝk̄[(nx*Nk + 1):end] .= 0 - Q0[(na*Nk + 1):end] .= 0 - Q̄[(na*Nk + 1):end] .= 0 + # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 + q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + a0end = @views Â0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + if na > 0 && h < 1 # compute the final residual at k+p: + x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) + d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1) ≈ d0(k) + k̇0end = @views K̄[(end-nx+1):end] + fq_dae!(k̇0end, q0end, model, x̂dend, a0end, û0end, d0end) + else # piecewise linear inputs u and disturvances d: + q0end .= a0end .- @views A1_Z̃[(1 + na*(Nk-1)):(na*Nk)] end return geq end @@ -1781,7 +1857,7 @@ end con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) -> geq Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`OrthogonalCollocation`](@ref). @@ -1804,46 +1880,75 @@ are computed by: ``` for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{x̂_d}(ℓ+j)`` vectors are extracted from the decision variables in `Z̃`. The -``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as -described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the +``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function [`fq_dae!`](@ref), +as described in [`init_orthocolloc`](@ref). The nonlinear equality constraints also include +the residuals at the collocation points ``\mathf{q}_i(ℓ+j)``, see [`init_orthocolloc`](@ref). +The residuals at the sampling instants ``\mathbf{q_0}(ℓ+j)`` are other constraints: +```math +\mathbf{q_0}(ℓ+j) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) +``` +for ``j = 0, 1, ... , N_k``. The defects for the continuity constraints and the stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). The estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h - nx̂, He = estim.nx̂, estim.He + nx̂, nx̃, He = estim.nx̂, estim.nx̃, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads - Mo, no, τ = estim.Mo, transcription.no, transcription.τ - nk̄ = get_nk̄(model, transcription) - nx̃ = estim.nε + nx̂ - p = estim.direct ? 0 : 1 + no, τ, τendIsNotOne = transcription.no, transcription.τ, transcription.τendIsNotOne + Mo = estim.Mo + na = get_na(model) + nā, nk̄ = na*no, get_nk̄(model, transcription) + nx̃_nX̂ = nx̃ + nx̂*estim.He + nŜk̄, nÂ, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=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̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nK̄)] + Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + n + nK̄):(nx̃_nX̂ + na + n + nK̄ + nĀ)] Dtemp = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + (Nk < He) && (geq .= 0) + Ŝk̄ = @views geq[1:nŜk̄] + Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + n + na)] + Q̄ = @views geq[(1 + nŜk̄ + n + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] + â0_Z̃ = @views â0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] 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)] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K̄_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] + ā_Z̃ = @views Ā_Z̃[(1 + nā*(j-1)):(nā*j)] + q0 = @views Q0[(1 + na*(j-1)):(na*j)] + q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] + ŝk̄ = @views Ŝk̄[(1 + nk̄*(j-1)):(nk̄*j)] if estim.direct || j < Nk - d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else - d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + d1 = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available at time k: + end + # ----------------- residual at sampling instants (τ=0) ----------------------- + # we need to recompute q0 with multi-threading, even with h>0 and τ[end]≈1, since + # the previous iteration (j-1) may not be executed (iterations are re-orderable) + computeDynamicsAtBegin = τendIsNotOne || f_threads || h < 1 || j < 2 + if na > 0 && computeDynamicsAtBegin + @views fq_dae!(k̄dot[1:nx] ,q0 , model, x̂d_Z̃, â0_Z̃, û0, d0) + else + q0 .= @views â0_Z̃ .- Ā_Z̃[(1 + nā*(j-1) - na):(nā*(j-1))] # a0[j] = a_no[j-1] end # ----------------- 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̃ end @@ -1853,21 +1958,31 @@ 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)] - di .= (1-τ[i]).*d0 .+ τ[i].*d0next - if h < 1 - model.f!(k̇i, ki_Z̃, û0, di, model.p) - 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))] - ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next - model.f!(k̇i, ki_Z̃, ûi, di, model.p) + k̇i, ki_Z̃ = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄_Z̃[(1 + (i-1)*nx):(i*nx)] + qi, ai_Z̃ = @views q̄[(1 + (i-1)*na):(i*na)], ā_Z̃[(1 + (i-1)*na):(i*na)] + di .= (1-τ[i]).*d0 .+ τ[i].*d1 + if h > 0 && j < Nk + û1 = @views Û0[(1 + nu*j):(nu*(j+1))] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û1 + else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) is not available: + ûi = û0 end + fq_dae!(k̇i, qi, model, ki_Z̃, ai_Z̃, ûi, di) end - ŝk̄ .-= k̄ + ŝk̄ .-= k̄dot + end + # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 + q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + a0end = @views Â0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + if na > 0 && h < 1 # compute the final residual at k+p: + x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) + d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1) ≈ d0(k) + k̇0end = @views K̄[(end-nx+1):end] + fq_dae!(k̇0end, q0end, model, x̂dend, a0end, û0end, d0end) + else # piecewise linear inputs u and disturvances d: + q0end .= @views a0end .- Ā_Z̃[(1 + nā*Nk - na):(nā*Nk)] # a0[j] = a_no[j-1] end - Nk < He && (geq[nk̄*Nk+1:end] .= 0) return geq end diff --git a/src/general.jl b/src/general.jl index e81d7c6f6..fe2829a9d 100644 --- a/src/general.jl +++ b/src/general.jl @@ -28,7 +28,8 @@ const DEFAULT_JACSPARSE = AutoSparse( ) const HIDDEN_GETINFO_KEYS_MHE = ( - :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ, + :What, :xhatarr, :ahatarr, :epsilon, :Xhat, :Ahat, :xhat, :Vhat, + :Pbar, :xbar, :Yhat, :Yhatm, :ϵ, :nablaJ, :nabla2J, :nabla2J_ncolors, :nablag, :nablag_ncolors, :nabla2lg, :nabla2lg_ncolors, :nablageq, :nablag_ncolors, :nabla2lgeq, :nabla2lgeq_ncolors diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 04e091481..317387e7b 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -140,7 +140,7 @@ end Construct a nonlinear DAE model from state-space functions `fq`/`fq!` and `h`/`h!`. -It supports continuous differential and algebraic equations (DAE). The functions are +It supports continuous-time differential and algebraic equations (DAE). The functions are provided in the semi-explicit form: ```math \begin{aligned} @@ -355,6 +355,11 @@ function validate_h_dae(NT, h) return ismutating end +function validate_transcription(::NonLinModelDAE, ::ShootingMethod) + throw(ArgumentError("Transcription with shooting methods is not supported with NonLinModelDAE")) + return nothing +end + """ validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) -> iszero_Ha @@ -400,6 +405,10 @@ function validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) return iszero_Ha end +"Default to `SingleShooting()`, or `model.transcription` if `model` is a `NonLinModelDAE`." +default_transcription(::SimModel) = SingleShooting() +default_transcription(model::NonLinModelDAE) = model.transcription + "Get the number of algebraic variable `na` in `model`." get_na(model::NonLinModelDAE) = model.na get_na(::SimModel) = 0 @@ -681,11 +690,11 @@ function con_nonlinprogeq!( ) 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, q0, q1 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] + x0next_Z, a0_Z, a1_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] + 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) + model.fq!(k̇1, q1, x0next_Z, a1_Z, u0, d0, model.p) sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end @@ -789,6 +798,27 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) return nothing end +""" + fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) + +Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). + +Both the algebraic variable `a0` and the residual `q0` arguments are ignored if `model` is a +[`NonLinModel`](@ref). +""" +fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) = model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0) = model.f!(ẋ0, x0, u0, d0, model.p) + +""" + h_dae!(y0, model, x0, a0, d0) + +Call `model.h!` with the `a0` argument if [`NonLinModelDAE`](@ref), else without. + +The `a0` argument is ignored for [`SimModelODE`](@ref)s. See also [`fq_dae!`](@ref). +""" +h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) = model.h!(y0, x0, a0, d0, model.p) +h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0, model.p) + function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) model.beq .= @. -model.Fs diff --git a/src/precompile.jl b/src/precompile.jl index d49bd98d9..d569cb199 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -22,7 +22,7 @@ end p = (sys2.A, sys2.B, sys2.C) function fq_dae!(ẋ, res, x, a, u, _ , _ ) - ẋ[1] = -0.5*(x[1] - 0.2*u[1]) + ẋ[1] = -0.005*(x[1] - 0.2*u[1]) res .= (x .- a) return nothing end @@ -104,8 +104,13 @@ R̂y = repeat([55; 30], 3) linearizemodel = linearize(nlmodel) setmodel!(mpc_kf, linearizemodel) - daemodel = NonLinModelDAE(fq_dae!, h_dae!, 1.0, 1, 1, 1, 1) + daemodel = NonLinModelDAE(fq_dae!, h_dae!, 100.0, 1, 1, 1, 1; transcription) sim!(daemodel, 2, [10]) + + mhe_dae = MovingHorizonEstimator(daemodel, He=2, direct=false) + preparestate!(mhe_dae, [5]) + updatestate!(mhe_dae, [2], [5]) + end end # @setup_workload \ No newline at end of file diff --git a/src/transcription.jl b/src/transcription.jl index 6875610af..1ab65fbcf 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -161,8 +161,7 @@ moderately stiff systems and is A-stable. See Extended Help for more details. Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a `TrapezoidalCollocation` transcription, the built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to - estimate the plant states, not the trapezoidal rule (see `supersample` option of - [`RungeKutta`](@ref) for stiff systems). + estimate the plant states, not the trapezoidal rule. Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this transcription method. @@ -175,27 +174,27 @@ transcription method. \mathbf{Z} = \begin{bmatrix} \mathbf{x_0}(k+1) \\ \mathbf{a_0}(k+0) \\ - \mathbf{a_0}(k+1) \end{bmatrix} + \mathbf{a_1}(k+0) \end{bmatrix} ``` For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ - \mathbf{A_0} \\ + \mathbf{Â_0} \\ \mathbf{Ā} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k+1) \\ - \mathbf{a_0}(k+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k+1) \\ + \mathbf{â_0}(k+2) \\ \vdots \\ - \mathbf{a_0}(k+H_p) \end{bmatrix} + \mathbf{â_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a}(k+0) \\ - \mathbf{a}(k+1) \\ + \mathbf{a_1}(k+0) \\ + \mathbf{a_1}(k+1) \\ \vdots \\ - \mathbf{a}(k+H_p-1) \end{bmatrix} + \mathbf{a_1}(k+H_p-1) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math @@ -203,31 +202,32 @@ 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} \\ - \mathbf{0_a} \\ + \mathbf{â_0}(k-N_k+p) \\ + \mathbf{Â_0} \\ + \mathbf{0_â} \\ \mathbf{Ā} \\ \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k-N_k+p+1) \\ + \mathbf{â_0}(k-N_k+p+2) \\ \vdots \\ - \mathbf{a_0}(k+p) \end{bmatrix} + \mathbf{â_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a}(k-N_k+p+0) \\ - \mathbf{a}(k-N_k+p+1) \\ + \mathbf{a_1}(k-N_k+p+0) \\ + \mathbf{a_1}(k-N_k+p+1) \\ \vdots \\ - \mathbf{a}(k+p-1) \end{bmatrix} + \mathbf{a_1}(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 < 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. + the algebraic variables in ``\mathbf{Â_0}``, while the values in ``\mathbf{Ā}`` are + strictly reserved for the the `fq!` function. The ``\mathbf{â_0}`` vector is at the left + endpoint of the trapezoid, while the ``\mathbf{a_1}`` is at the right endpoint. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -283,7 +283,7 @@ where ``\mathbf{K̄}`` encompasses all the intermediate stages of the determinis ``` 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 +on `f_threads` and `h_threads` keywords. This transcription computes the predictions 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 @@ -293,8 +293,7 @@ non-stiff systems. See Extended Help for details and the transcription of Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a `OrthogonalCollocation` transcription, the built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to estimate - the plant states, not orthogonal collocation (see `supersample` option of - [`RungeKutta`](@ref) for stiff systems). + the plant states, not orthogonal collocation. Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for this transcription method (sparser formulation than [`MultipleShooting`](@ref)). @@ -345,15 +344,15 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ - \mathbf{A_0} \\ + \mathbf{Â_0} \\ \mathbf{K̄} \\ \mathbf{Ā} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k+1) \\ - \mathbf{a_0}(k+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k+1) \\ + \mathbf{â_0}(k+2) \\ \vdots \\ - \mathbf{a_0}(k+H_p) \end{bmatrix} + \mathbf{â_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k+0) \\ @@ -367,9 +366,9 @@ 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{â_0}(k-N_k+p) \\ + \mathbf{Â_0} \\ + \mathbf{0_â} \\ \mathbf{K̄} \\ \mathbf{0_k̄} \\ \mathbf{Ā} \\ @@ -377,11 +376,11 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} , \; - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k-N_k+p+1) \\ + \mathbf{â_0}(k-N_k+p+2) \\ \vdots \\ - \mathbf{a_0}(k+p) \end{bmatrix} + \mathbf{â_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k-N_k+p+0) \\ @@ -391,15 +390,17 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). ``` 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. + outputs are computed from the algebraic variables in ``\mathbf{Â_0}``, while the values + in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. The ``\mathbf{Â_0}`` + vector must be explicitly included in the decision variables since the output function + `h!` is evaluated at different locations than the collocation points, in general. - 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 - is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) - for more details. + More precisely, the outputs are at the sampling instants, while the collocation points + are 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 is more accurate than + Gauss-Radau but only A-stable, while the latter being L-stable. See [`init_orthocolloc`](@ref), + [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic states are left out of the ``\mathbf{K̄}`` vector to reduce the dimensions, and also @@ -412,6 +413,7 @@ struct OrthogonalCollocation <: CollocationMethod f_threads::Bool h_threads::Bool τ::Vector{COLLOCATION_NODE_TYPE} + τendIsNotOne::Bool function OrthogonalCollocation( h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) @@ -430,7 +432,8 @@ struct OrthogonalCollocation <: CollocationMethod else throw(ArgumentError("roots argument must be :gaussradau or :gausslegendre.")) end - return new(h, no, f_threads, h_threads, τ) + τendIsNotOne = (τ[end] < 1) + return new(h, no, f_threads, h_threads, τ, τendIsNotOne) end end @@ -492,9 +495,17 @@ knowing that the ``\mathbf{k}_i(k)`` vectors are directly extracted from the dec variables in `Z̃`. The ``\mathbf{x̂_d}(k)`` vector is the estimated deterministic state at the beginning of the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivatives for the ``i``th collocation point are computed from the continuous-time function -`model.f!` and: +[`fq_dae!`](@ref) and: ```math -\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) +\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{a}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) +``` +The residuals of [`NonLinModelDAE`](@ref) at the collocation points are nonlinear equality +constraints: +```math +\begin{aligned} +\mathbf{q}_i(k) &= \mathbf{q}\Big(\mathbf{k}_i(k), \mathbf{a}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) \\ + &= \mathbf{0} +\end{aligned} ``` Based on the normalized time ``τ_i`` and the hold order `transcription.h`, the inputs and disturbances are either piecewise constant or linear: diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index f4100d87c..d17376c63 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1035,6 +1035,34 @@ end ) end +@testitem "MHE construction (NonLinModelDAE)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + + 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] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) + + transcription = TrapezoidalCollocation() + mhe = MovingHorizonEstimator(dae; He=3, transcription) + @test mhe.transcription isa TrapezoidalCollocation + + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + mhe2 = MovingHorizonEstimator(dae; He=3, transcription, direct=false) + @test mhe2.direct == false + + @test_throws ArgumentError MovingHorizonEstimator(dae, He=3, transcription=SingleShooting()) + @test_throws ArgumentError MovingHorizonEstimator(dae, He=3, transcription=MultipleShooting()) +end + @testitem "MHE estim. & getinfo (LinModel, SS)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, DAQP @@ -1322,7 +1350,7 @@ end h! = (y,x,_,_) -> y .= x nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) - transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) + transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) mhe6 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=false, transcription ) @@ -1333,7 +1361,8 @@ end preparestate!(mhe6, [13]) @test mhe6() ≈ [13] atol=5e-3 - transcription = TrapezoidalCollocation(1) + ##### # h=1 & f_threads=false options test the branch with the reuse of k̇1 from prev. iter: + transcription = TrapezoidalCollocation(1, f_threads=false, h_threads=true) mhe7 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=true, transcription ) @@ -1400,7 +1429,7 @@ end 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() + transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) preparestate!(mhe, [0.0], [0.0]) x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) @@ -1423,17 +1452,75 @@ end preparestate!(mhe, [7.0], [0]) @test mhe([0]) ≈ [7.0] atol=1e-3 - transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + # h=1 & f_threads=false options test the branch with reuse of k̇1 & q1 from prev. iter: + transcription = TrapezoidalCollocation(1, f_threads=false, 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 # xd in X̂0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 # a1 in Ā +end + +@testitem "MHE estim. & getinfo (NonLinModelDAE, OC)" 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 = [-0.01], [0.01] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + + no, roots = 3, :gausslegendre + transcription = OrthogonalCollocation(0, no; roots, f_threads=true, h_threads=false) + 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=5e-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=5e-3 + + no, roots = 2, :gaussradau + # h=1 & f_threads=false options test the branch with reuse of q̄ from prev. iter: + transcription = OrthogonalCollocation(1, no; roots, f_threads=false, 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 + @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ xs_0 # xd in K̄ + @test mhe2.Z̃[13:13] ≈ mhe2.Z̃[14:14] ≈ as_0 # a in Ā end @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin diff --git a/test/5_test_extensions.jl b/test/5_test_extensions.jl index ee4ec83be..1384c0bd5 100644 --- a/test/5_test_extensions.jl +++ b/test/5_test_extensions.jl @@ -1,4 +1,3 @@ -# TODO: add `skip=(Sys.ARCH==:x86)` because bug in DAQP.jl, and activate Linux x86 CI job in juliaci.yml @testitem "LinearMPCext general" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP import LinearMPC