From 97911071310373d21abb8701b3816a6a2aa46ef6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 16 Sep 2026 10:31:10 -0400 Subject: [PATCH 01/35] changed: `a0_arr` in `Z` for MHE with DAE and `OrthogonalCollocation` --- src/estimator/mhe/transcription.jl | 12 +++++++++--- src/transcription.jl | 18 +++++++++++++++--- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 3ca0b340e..a16f6af1f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,7 +1,13 @@ "Get the number of elements in the optimization decision vector `Z`" -get_nZ_mhe(::SingleShooting, He, nx̂, _ , nŵ) = nx̂ + nŵ*He -get_nZ_mhe(::TranscriptionMethod, He, nx̂, _ , nŵ) = nx̂ + nx̂*He + nŵ*He -get_nZ_mhe(::OrthogonalCollocation, He, nx̂, nk̄, nŵ) = nx̂ + nx̂*He + nk̄*He + nŵ*He +get_nZ_mhe(::SingleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nŵ*He +get_nZ_mhe(::MultipleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nx̂*He + nŵ*He +function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, _ , nŵ, na=0) + return nx̂ + nx̂*He + na + na*He + nŵ*He +end +function get_nZ_mhe(transcription::OrthogonalCollocation, He, nx̂, nk̄, nŵ, na=0) + nā = na*transcription.no + return nx̂ + nx̂*He + na + na*He + nk̄*He + nā*He + nŵ*He +end "Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) diff --git a/src/transcription.jl b/src/transcription.jl index 375609c50..20965b2d2 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -332,6 +332,12 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{A_0} \\ \mathbf{K̄} \\ \mathbf{Ā} \end{bmatrix} + \: , \quad + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k+1) \\ + \mathbf{a_0}(k+2) \\ + \vdots \\ + \mathbf{a_0}(k+H_p) \end{bmatrix} \quad \text{and} \quad \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k+0) \\ @@ -345,6 +351,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p) \\ \mathbf{A_0} \\ \mathbf{0_a} \\ \mathbf{K̄} \\ @@ -353,6 +360,12 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} + \: , \quad + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{a_0}(k+p) \end{bmatrix} \quad \text{and} \quad \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k-N_k+p+0) \\ @@ -360,9 +373,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \vdots \\ \mathbf{ā}(k+p-1) \end{bmatrix} ``` - See the Extended Help of [`TrapezoidalCollocation`](@ref) for the exact definition of - ``\mathbf{A_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with - zeros for the unused decision variables at the beginning (``N_k < H_e``). + All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at + the beginning in the [`MovingHorizonEstimator`](@ref) (``N_k < H_e``). The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. From 98f5a8c152c1bd0026fd73db964f69456c0232bf Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 16 Sep 2026 13:25:26 -0400 Subject: [PATCH 02/35] wip: support `NonLinModelDAE` in MHE --- src/estimator/construct.jl | 13 +- src/estimator/execute.jl | 24 +- src/estimator/mhe/construct.jl | 25 +- src/estimator/mhe/execute.jl | 47 ++-- src/estimator/mhe/transcription.jl | 383 +++++++++++++++++------------ src/model/nonlinmodeldae.jl | 4 + 6 files changed, 306 insertions(+), 190 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index e9e418920..3f9776ffd 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -21,7 +21,7 @@ end @doc raw""" StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0 + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0, na::Int=0, He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) @@ -31,16 +31,17 @@ Create a buffer for `StateEstimator` objects for estimated states and measured o The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0, + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk̄::Int=0, na::Int=0, He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) nV̂, nŴ, nX̂, nŶ, nD = nym*He, nŵ*He, nx̂*He, ny*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) x̂ = Vector{NT}(undef, nx̂) + a = Vector{NT}(undef, na) Z̃ = Vector{NT}(undef, nZ̃) V̂ = Vector{NT}(undef, nV̂) Ŵ = Vector{NT}(undef, nŴ) @@ -56,7 +57,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end @@ -174,7 +175,7 @@ where ``\mathbf{e}(k)`` is an unknown zero mean white noise and ``\mathbf{A_s} = it is thus ignored. The function [`init_integrators`](@ref) builds the state-space matrices. """ function init_estimstoch( - model::SimModelODE{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt + model::SimModel{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt ) where {NT<:Real} nu, ny, nym = model.nu, model.ny, length(i_ym) As_u , Cs_u , nint_u = init_integrators(nint_u , nu , "u") @@ -198,7 +199,7 @@ function validate_ym(model::SimModel, i_ym) end "Convert the measured outputs stochastic model `stoch_ym` to all outputs `stoch_y`." -function stoch_ym2y(model::SimModelODE{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} +function stoch_ym2y(model::SimModel{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} As = Asm Bs = Bsm Cs = zeros(NT, model.ny, size(Csm,2)) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index c2c4e5a53..815bf772d 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -129,11 +129,11 @@ function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0) end """ - ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) + ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) Same than [`ĥ!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. """ -function ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) +function ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] h!(ŷ0, model, xd, d0, model.p) # y0 = h(xd, d0) @@ -141,6 +141,26 @@ function ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) return nothing end +""" + ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, a0, x̂0, d0) + +Same than [`ĥ!`](@ref) for [`NonLinModelDAE`](@ref) but with the algebraic variable `a0`. +""" +function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, a0, 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] + model.h!(ŷ0, xd, a0, d0, model.p) + mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + return nothing +end + +""" + ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, _ , x̂0, d0) + +Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. +""" +ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, _ , x̂0, d0) = ĥ!(ŷ0, model, estim, x̂0, d0) + """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 185054343..8db383cea 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -173,7 +173,7 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator{NT} } nu, ny, nd = model.nu, model.ny, model.nd - nk̄ = get_nk̄(model, transcription) + nk̄, na = get_nk̄(model, transcription), get_na(model) He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) @@ -204,7 +204,7 @@ struct MovingHorizonEstimator{ ES, GS, JS, BS, gc!, nc ) - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) @@ -221,7 +221,9 @@ struct MovingHorizonEstimator{ test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct) # force computation of derivatives for the first NLP iteration: force∇J, force∇g, force∇geq = [true], [true], [true] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄, He, nŵ, nε, transcription) + buffer = StateEstimatorBuffer{NT}( + nu, nx̂, nym, ny, nd, nk̄, na, He, nŵ, nε, transcription + ) estim = new{NT, SM, KC, TM, JM, GB, JB, HB, PT, GCfunc, CE}( model, transcription, optim, con, gradient, jacobian, hessian, @@ -648,11 +650,16 @@ function default_optim_mhe(model::SimModel, nc) end "Default arrival covariance estimator for MHE, depending on the model type only." -function default_covestim_mhe(model::SimModelODE, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) +function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) if model isa LinModel return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) - else + elseif model isa NonLinModel return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + else + nx, nu, ny = model.nx, model.nu, model.ny + A, Bu, C = 0.1*I(nx), ones(nx, nu), ones(ny, nx) + dummy_model = LinModel(A, Bu, C, 0, 0, model.Ts) + return SteadyKalmanFilter(dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) end end @@ -1084,8 +1091,8 @@ and ``\mathbf{0}`` is properly sized for the `transcription` instance. function init_ZtoŴ( model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} - nk̄ = get_nk̄(model, transcription) - nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nk̄, na = get_nk̄(model, transcription), get_na(model) + nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Tŵ = [spzeros(NT, nŴ, nZ-nŴ) I] return Tŵ end @@ -1311,8 +1318,8 @@ function init_boxconstraint_mhe( x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, A_x̂min, A_x̂max, C_x̂min, C_x̂max, A_Ŵmin, A_Ŵmax ) where {NT<:Real} - nk̄ = get_nk̄(model, transcription) - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nk̄, na = get_nk̄(model, transcription), get_na(model) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Z̃min, Z̃max = fill(convert(NT,-Inf), nZ̃), fill(convert(NT,+Inf), nZ̃) nε > 0 && (Z̃min[begin] = 0) nŴ = nŵ*He diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 08674d10c..ba1da9a3d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -152,7 +152,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ x̂0arr = buffer.x̂ x̄, Û0, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) - x̂0arr = getarrival!(x̂0arr, estim, Z̃) + x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) @@ -465,20 +465,29 @@ Does nothing if `model` is not a [`LinModel`](@ref). """ initpred!(::MovingHorizonEstimator, ::SimModel) = nothing -"Get the estimated state at arrival from the decision vector `Z̃`." -function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) - nx̃ = estim.nε + estim.nx̂ - return x̂0arr .= @views Z̃[nx̃-estim.nx̂+1:nx̃] +"Get the estimated state at arrival `x̂0arr` from the decision vector `Z̃`." +function getx̂0arr!(x̂0arr, estim::MovingHorizonEstimator, Z̃) + return x̂0arr .= @views Z̃[(estim.nε + 1):(estim.nε + estim.nx̂)] end +"Get algebraic variable at arrival `a0arr` from the decision vector `Z̃`." +function geta0arr!( + a0arr, estim::MovingHorizonEstimator, ::CollocationMethod, Z̃ +) + na = get_na(estim.model) + nx̃_nX̂ = estim.nε + estim.nx̂ + estim.nx̂*estim.He + return a0arr .= @views Z̃[(nx̃_nX̂ + 1):(nx̃_nX̂ + na)] +end +geta0arr!(a0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = a0arr + "Get the estimation error at arrival from the estimated state at arrival `x̂0arr`." getx̄!(x̄, estim::MovingHorizonEstimator, x̂0arr) = (x̄ .= estim.x̂0arr_old .- x̂0arr) "Get the estimated process noise from the decision vector `Z̃`." function getŴ!(Ŵ, estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃) He, nx̂, nŵ = estim.He, estim.nx̂, estim.nx̂ - nk̄ = get_nk̄(estim.model, transcription) - nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nk̄, na = get_nk̄(estim.model, transcription), get_na(estim.model) + nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) Ŵ .= @views Z̃[(nZ̃ - nŵ*He + 1):end] return Ŵ end @@ -648,7 +657,7 @@ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real 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 getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) - getarrival!(x̂0arr, estim, Z̃) + getx̂0arr!(x̂0arr, estim, Z̃) predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing @@ -855,7 +864,7 @@ This function is not used for the optimization, but it can be useful to predict estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the `Ŷ0` vector. The argument `X̂0` is computed from [`predict_mhe!`](@ref) and contains the estimated states from ``k-N_k+1+p`` to ``k+p``. The argument `x̂0arr` is computed from -[`getarrival!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. +[`getx̂0arr!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. """ function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) model = estim.model @@ -875,7 +884,7 @@ end """ update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ā, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -884,18 +893,24 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ā, Ŷ0, gc, g, geq, + estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription - x̂0arr = getarrival!(x̂0arr, estim, Z̃) - x̄ = getx̄!(x̄, estim, x̂0arr) - Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) - V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, Ŵ, Z̃) + x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + x̄ = getx̄!(x̄, estim, x̂0arr) + Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) + V̂, X̂0, A0 = predict_mhe!( + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) ε = getslack(estim, Z̃) gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) - geq = con_nonlinprogeq_mhe!(geq, X̂0, Û0, K̄, estim, model, transcription, x̂0arr, Ŵ, Z̃) + geq = con_nonlinprogeq_mhe!( + geq, X̂0, A0, Û0, K̄, Ā, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index a16f6af1f..f4a9154f0 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -12,26 +12,34 @@ end "Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nŴ, nX̂ = nŵ*Nk, nx̂*Nk + na = get_na(estim.model) + nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*estim.He + nx̃_nX̂_He = nx̃ + nx̂*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + nŴ) + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA) + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nŴ) ] return i_Z̃_NK end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nk̄ = get_nk̄(estim.model, transcription) - nŴ, nX̂, nK = nŵ*Nk, nx̂*Nk, nk̄*Nk + na = get_na(estim.model) + nk̄, nā = get_nk̄(estim.model, transcription), transcription.no*na + nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*estim.He - nx̃_nX̂_nK_He = nx̃_nX̂_He + nk̄*estim.He + nx̃_nX̂_He = nx̃ + nx̂*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na + na*estim.He + nx̃_nX̂_na_nA_nK̄_He = nx̃_nX̂_na_nA_He + nk̄*estim.He + nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃_nX̂_na_nA_nK̄_He + nā*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + nK); - (1 + nx̃_nX̂_nK_He):(nx̃_nX̂_nK_He + nŴ); + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA); + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nK̄); + (1 + nx̃_nX̂_na_nA_nK̄_He):(nx̃_nX̂_na_nA_nK̄_He + nĀ); + (1 + nx̃_nX̂_na_nA_nK̄_nĀ_He):(nx̃_nX̂_na_nA_nK̄_nĀ_He + nŴ); ] return i_Z̃_NK end @@ -375,14 +383,14 @@ end """ init_predmat_mhe( - model::SimModelODE, transcription::SingleShooting, direct::Bool, + model::SimModel, transcription::SingleShooting, direct::Bool, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return empty matrices for [`SingleShooting`](@ref) and non-`LinModel`, except for `ex̄`. """ function init_predmat_mhe( - model::SimModelODE{NT}, transcription::SingleShooting, ::Bool, + model::SimModel{NT}, transcription::SingleShooting, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -403,20 +411,20 @@ end """ init_predmat_mhe( - model::SimModelODE, transcription::TranscriptionMethod, direct::Bool + model::SimModel, transcription::TranscriptionMethod, direct::Bool He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return `ex̄, EX̂, GX̂, JX̂, BX̂` and empty matrices non-`LinModel` and other [`TranscriptionMethod`](@ref). """ function init_predmat_mhe( - model::SimModelODE{NT}, transcription::TranscriptionMethod, ::Bool, + model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ - nk̄ = get_nk̄(model, transcription) - nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nk̄, na = get_nk̄(model, transcription), get_na(model) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) E = zeros(NT, 0, nZ) ex̄ = [-I zeros(NT, nx̂, nZ - nx̂)] EX̂ = [zeros(NT, nx̂*He, nx̂) I zeros(NT, nx̂*He, nZ - nx̂ - nx̂*He)] @@ -516,7 +524,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModelODE, transcription::TranscriptionMethod, direct::Bool, + model::SimModel, transcription::TranscriptionMethod, direct::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) -> ES, GS, JS, BS @@ -547,15 +555,17 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{I} \end{bmatrix} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^ŵ} \end{bmatrix} + \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^ŵ} \end{bmatrix} \end{aligned} ``` """ function init_defectmat_mhe( - model::SimModelODE{NT}, ::TranscriptionMethod, ::Bool, + model::SimModel{NT}, ::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) + na = get_na(model) nx = nx̂ - nxs nŵ = nx̂ nŵd = nŵ - nxs @@ -566,7 +576,8 @@ function init_defectmat_mhe( ESx̂[iRow, iCol] = As end ESŵ = repeatdiag([zeros(NT, nxs, nŵd) I], He) - ES = [ESx̂ ESŵ] + ESa = zeros(nxs*He, na + na*He) + ES = [ESx̂ ESa ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -575,7 +586,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModelODE, transcription::OrthogonalCollocation, direct::Bool + model::SimModel, transcription::OrthogonalCollocation, direct::Bool He, Â, _ , _ , _ , _ , As, Co, λo ) -> ES, GS, JS, BS @@ -612,18 +623,20 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \vdots & \vdots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ - \mathbf{E_S^ŵ} &= \mathbf{I} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^k̄} & \mathbf{E_S^ŵ} \end{bmatrix} \\ + \mathbf{E_S^ŵ} &= \mathbf{I} \\ + \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S^ā} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^k̄} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \\ \end{aligned} ``` """ function init_defectmat_mhe( - model::SimModelODE{NT}, transcription::OrthogonalCollocation, ::Bool, + model::SimModel{NT}, transcription::OrthogonalCollocation, ::Bool, He, Â, _ , _ , _ , _ , As, Co, λo ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs - nk̄ = get_nk̄(model, transcription) + nk̄, na = get_nk̄(model, transcription), get_na(model) λo_I = λo*I(nx) ESx̂ = [zeros(NT, nx̂*He, nx̂) -I] for j=1:He @@ -636,7 +649,9 @@ function init_defectmat_mhe( end ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], He) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented - ES = [ESx̂ ESk̄ ESŵ] + ESa = zeros(nx̂*He, na + na*He) + ESā = zeros(nx̂*He, na*transcription.no*He) + ES = [ESx̂ ESa ESk̄ ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -654,11 +669,11 @@ function init_defectmat_mhe( end function init_defectmat_mhe_empty( - model::SimModelODE{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ + model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} nu, nd = model.nu, model.nd - nk̄ = get_nk̄(model, transcription) - nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ) + nk̄, na = get_nk̄(model, transcription), get_na(model) + nZ = get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) ES = zeros(NT, 0, nZ) GS = zeros(NT, 0, nu*He) JS = zeros(NT, 0, nd*(He+1)) @@ -668,7 +683,7 @@ end @doc raw""" init_matconstraint_mhe( - model::LinModel, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::LinModel, transcription::ShootingMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) -> i_b, i_g, A, Aeq, neq @@ -692,7 +707,7 @@ case, `args` needs to contain all the inequality and equality constraint matric the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mhe( - model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::LinModel{NT}, transcription::ShootingMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) where {NT<:Real} if isempty(args) @@ -740,9 +755,9 @@ function init_matconstraint_mhe( return i_b, i_g, A, Aeq, neq end -"Init `i_b, A` without sensor noise constraints if `NonLinModel` and other `TranscriptionMethod`." +"Init `i_b, A` without sensor noise constraints if not `LinModel` and other `TranscriptionMethod`." function init_matconstraint_mhe( - model::NonLinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, + model::SimModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... ) where {NT<:Real} if isempty(args) @@ -790,7 +805,7 @@ boxconstraint_states!(Z̃min, Z̃max, ::SingleShooting, _, _, _, _, _, _) = Z̃m "Unset `i_x̂min` and `i_x̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deletex̂arr_lincon!( - i_x̂min, i_x̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε + i_x̂min, i_x̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε ) nx̂ = length(i_x̂min) x̂0min, x̂0max = @views Z̃min[(nε+1):(nε+nx̂)], @views Z̃max[(nε+1):(nε+nx̂)] @@ -801,7 +816,7 @@ end "Unset `i_X̂min` and `i_X̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deleteX̂_lincon!( - i_X̂min, i_X̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ + i_X̂min, i_X̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ ) nx̃ = nε + nx̂ nX̂ = length(i_X̂min) @@ -810,10 +825,10 @@ function deleteX̂_lincon!( foreach(i -> !isinf(X̂0max[i]) && (i_X̂max[i] = false), eachindex(i_X̂max)) return i_X̂min, i_X̂max end -deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModelODE, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max +deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max "Unset `i_Ŵmin` and `i_Ŵmax` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max) +function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max) nŴ = length(i_Ŵmin) Ŵmin, Ŵmax = @views Z̃min[end-nŴ+1:end], Z̃max[end-nŴ+1:end] foreach(i -> !isinf(Ŵmin[i]) && (i_Ŵmin[i] = false), eachindex(i_Ŵmin)) @@ -1038,28 +1053,31 @@ linconstrainteq!(::MovingHorizonEstimator, ::SimModelODE, ::SingleShooting) = no Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). If supported by `estim.optim` and based a [`SingleShooting`](@ref) transcription, it -warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{ŵ}(k-N_k+p+0|k-1) \\ - \mathbf{ŵ}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\end{bmatrix} -``` -where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the -process noise estimates computed at the last time step ``k-1``. The vector -``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated -at time ``k-N_k``. If the objective function is not finite at this point, all the process -noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended Help of -[`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates -all the arguments. +warm-starts the solver at the value provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \end{bmatrix} + ``` + where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the + process noise estimates computed at the last time step ``k-1``. The vector + ``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated + at time ``k-N_k``. If the objective function is not finite at this point, all the process + noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended Help of + [`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates + all the arguments. """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var @@ -1105,81 +1123,114 @@ end Do the same but for [`OrthogonalCollocation`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ - \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+p-2|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{0_x̂} \\ - \mathbf{k̄}(k-N_k+p+0|k-1) \\ - \mathbf{k̄}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{k̄}(k+p-3|k-1) \\ - \mathbf{k̄}(k+p-2|k-1) \\ - \mathbf{k̄}(k+p-2|k-1) \\ - \mathbf{0_k} \\ - \mathbf{ŵ}(k-N_k+p+0|k-1) \\ - \mathbf{ŵ}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\end{bmatrix} -``` -where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the -last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` include the ``n_o`` intermediate -stage predictions for the interval ``k-j``, and is also computed at the last control period. -See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for -the defintion of vectors ``\mathbf{0_x̂}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. +The warm-starting value is provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ + \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+p-2|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p|k-1) \\ + \mathbf{a_0}(k-N_k+p+1|k-1) \\ + \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \vdots + \mathbf{a_0}(k-p-2|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{0_a} \\ + \mathbf{k̄}(k-N_k+p+0|k-1) \\ + \mathbf{k̄}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{k̄}(k+p-3|k-1) \\ + \mathbf{k̄}(k+p-2|k-1) \\ + \mathbf{k̄}(k+p-2|k-1) \\ + \mathbf{0_k̄} \\ + \mathbf{ā}(k-N_k+p+0|k-1) \\ + \mathbf{ā}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+p-3|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{0_ā} \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \end{bmatrix} + ``` + where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the + last control period ``k-1``, expressed as a deviation from the operating point + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` include the ``n_o`` intermediate + stage predictions for the interval ``k-j``, and is also computed at the last control period. + See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for + the defintion of vectors ``\mathbf{0_x̂}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu - nk̄ = get_nk̄(estim.model, transcription) + nu, na = model.nu, get_na(model) + nk̄, nā = get_nk̄(estim.model, transcription), na*transcription.no nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nK = nε + nx̂, nŵ*He, nx̂*He, nk̄*He + nx̃, nŴ, nX̂, nA, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- - Z̃s[nε+1:nx̃] = estim.x̂0arr_old - # --- state estimates X̂0 --- - Z̃s[(nx̃+1):(nx̃+nX̂-nx̂)] .= @views estim.Z̃[(nx̃+nx̂+1):(nx̃+nX̂)] - Z̃s[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] .= @views estim.Z̃[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] - # --- collocation points K --- - Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nK-nk̄)] .= @views estim.Z̃[(nx̃+nX̂+nk̄+1):(nx̃+nX̂+nK)] - Z̃s[(nx̃+nX̂+nK-nk̄+1):(nx̃+nX̂+nK)] .= @views estim.Z̃[(nx̃+nX̂+nK-nk̄+1):(nx̃+nX̂+nK)] + i_base = nε + Z̃s[i_base+1:nx̃] = estim.x̂0arr_old + # --- state estimates X̂0 --- + i_base = nx̃ + Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views estim.Z̃[(i_base+nx̂+1):(i_base+nX̂)] + Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] + # --- algebraic variables a0arr and A0 --- + i_base = nx̃ + nX̂ + Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] + Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] + # --- deterministic states at collocation points K̄ --- + i_base = nx̃ + nX̂ + na + nA + Z̃s[(i_base+1):(i_base+nK̄-nk̄)] .= @views estim.Z̃[(i_base+nk̄+1):(i_base+nK̄)] + Z̃s[(i_base+nK̄-nk̄+1):(i_base+nK̄)] .= @views estim.Z̃[(i_base+nK̄-nk̄+1):(i_base+nK̄)] + # --- algebraic variables at collocation points Ā --- + i_base = nx̃ + nX̂ + na + nA + nK̄ + Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] + Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- process noise estimates Ŵ --- - Z̃s[(nx̃+nX̂+nK+1):(nx̃+nX̂+nK+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nK+nŵ+1):(nx̃+nX̂+nK+nŴ)] - Z̃s[(nx̃+nX̂+nK+nŴ-nŵ+1):end] .= 0 - # --- verify definiteness of objective function --- - x̄ = buffer.x̂ + i_base = nx̃ + nX̂ + na + nA + nK̄ + nĀ + Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] + Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 + # --- verify definiteness of objective function --- + x̄, a0rr = 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 - x̂0arr = estim.x̂0arr_old + Û0, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations + A0, Ā = Vector{NT}(undef, na*Nk), Vector{NT}(undef, nā*Nk) # TODO: remove the 2 allocations + x̂0arr = estim.x̂0arr_old + a0arr .= x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ā, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+nX̂+nK+1:end] .= 0 # Ŵ = 0 + Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 end # --- unused variable in Z̃ (applied only when Nk < He) --- # We force the update of the NLP gradient and jacobian by warm-starting the unused # variable of Ŵ in Z̃ at 1. Since Ŵ is initialized with 0s, at least 1 variable in Z̃s # will be inevitably different at the following time step. - Z̃s[nx̃+nX̂+nK+nŵ*Nk+1:end] .= 1 + Z̃s[nx̃+nX̂+nK̄+nŵ*Nk+1:end] .= 1 JuMP.set_start_value.(Z̃var, Z̃s) return Z̃s end @@ -1191,28 +1242,40 @@ end Do the same but for other transcription [`TranscriptionMethod`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂_0^†}(k-N_k+p) \\ - \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ - \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+p-2|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{x̂_0}(k+p-1|k-1) \\ - \mathbf{0_x̂} \\ - \mathbf{ŵ}(k-N_k+p+0|k-1) \\ - \mathbf{ŵ}(k-N_k+p+1|k-1) \\ - \vdots \\ - \mathbf{ŵ}(k+p-3|k-1) \\ - \mathbf{ŵ}(k+p-2|k-1) \\ - \mathbf{0} \\ - \mathbf{0_ŵ} -\end{bmatrix} -``` +The warm-starting value is provided in Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = + \begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ + \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+p-2|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p|k-1) \\ + \mathbf{a_0}(k-N_k+p+1|k-1) \\ + \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \vdots + \mathbf{a_0}(k-p-2|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{a_0}(k-p-1|k-1) \\ + \mathbf{0_a} \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ + \mathbf{0_ŵ} + \end{bmatrix} + ``` """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var @@ -1283,10 +1346,10 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, _ , _ , _ , + V̂, X̂0, A0, _ , _ , estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, - _ , _ , Z̃ - ) -> V̂, X̂0 + _ , _ , _ , Z̃ + ) -> V̂, X̂0, A0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -1299,11 +1362,12 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k \mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂} \end{aligned} ``` +The vector `A0` is ignored and returned unchanged. """ function predict_mhe!( - V̂, X̂0, _ , _ , _ , + V̂, X̂0, A0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, - _ , _ , Z̃ + _ , _ , _ , Z̃ ) nε, Nk = estim.nε, estim.Nk[] if Nk < estim.He @@ -1321,26 +1385,27 @@ function predict_mhe!( end V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F X̂0_res .= mul!(X̂0_res, ẼX̂, Z̃) .+ FX̂ - return V̂, X̂0 + return V̂, X̂0, A0 end @doc raw""" predict_mhe!( - V̂, X̂0, Û0, K̄, Ŷ0, + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, Ŵ, _ - ) -> V̂, X̂0 + x̂0arr, _ , Ŵ, _ + ) -> V̂, X̂0, A0 Compute the vectors when `model` is a [`NonLinModel`](@ref) with [`SingleShooting`](@ref). -The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The augmented model of -[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, -and by adding the estimated process noise ``\mathbf{ŵ}``. +The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The vector `A0` is +ignored and returned unchanged. The augmented model of [`f̂!`](@ref) and [`ĥ!`](@ref) is +called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated +process noise ``\mathbf{ŵ}``. """ function predict_mhe!( - V̂, X̂0, Û0, K̄, Ŷ0, + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, Ŵ, _ + x̂0arr, _ , Ŵ, _ ) nu, nd, ny, nk̄ = model.nu, model.nd, model.ny, model.nk̄ nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] @@ -1383,15 +1448,15 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0 + return V̂, X̂0, A0 end @doc raw""" predict_mhe!( - V̂, X̂0, _ , _ , Ŷ0, + V̂, X̂0, A0, _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::TranscriptionMethod, - x̂0arr , _ , Z̃ - ) -> V̂, X̂0 + x̂0arr, a0arr, _ , Z̃ + ) -> V̂, X̂0, A0 Compute the vectors when `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). @@ -1399,26 +1464,30 @@ 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, _ , _ , Ŷ0, + V̂, X̂0, A0, _ , Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, transcription::TranscriptionMethod, - x̂0arr, _ , Z̃ + x̂0arr, a0arr, _ , Z̃ ) - nd, ny = model.nd, model.ny + nd, ny, na = model.nd, model.ny, get_na(model) nx̂, nε, nym, Nk = estim.nx̂, estim.nε, estim.nym, estim.Nk[] nx̃ = nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads - X̂0[1:nx̂*Nk] .= @views Z̃[(nx̃+1):(nx̃+nx̂*Nk)] + X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] + A0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] @threadsif h_threads for j=1:Nk if estim.direct x̂0 = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] + a0 = @views A0[(1+na*(j-1)):(na*j)] else x̂0 = @views j < 2 ? x̂0arr[1:nx̂] : X̂0[(1+nx̂*(j-2)):(nx̂*(j-1))] + a0 = @views j < 2 ? a0arr[1:na] : A0[(1+na*(j-2)):(na*(j-1))] end d0 = @views estim.D0[(1+nd*j):(nd*(j+1))] # the 1st nd elements are not needed here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] v̂ = @views V̂[(1 + nym*(j-1)):(nym*j)] y0m = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] - ĥ!(ŷ0, estim, model, x̂0, d0) + ĥ!(ŷ0, estim, model, x̂0, a0, d0) ŷ0m = @views ŷ0[estim.i_ym] if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 94715ffb6..eeebc9cc3 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -400,6 +400,10 @@ function validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) return iszero_Ha end +"Get the number of algebraic variable `na` in `model`." +get_na(model::NonLinModelDAE) = model.na +get_na(model::SimModel) = 0 + "Get the number of elements in the optimization decision vector `Z` for DAE solving." function get_nZ_dae(transcription::OrthogonalCollocation, nx, na) return nx + transcription.no*nx + na + transcription.no*na From 4429923f47da451f6db366a868474d9f08ff0df7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 16 Sep 2026 16:32:00 -0400 Subject: [PATCH 03/35] wip: idem --- src/estimator/construct.jl | 1 + src/estimator/execute.jl | 8 +- src/estimator/mhe/construct.jl | 78 ++++++++------- src/estimator/mhe/transcription.jl | 155 +++++++++++++++-------------- src/model/nonlinmodeldae.jl | 12 ++- src/sim_model.jl | 2 +- 6 files changed, 141 insertions(+), 115 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 3f9776ffd..def47e2f8 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -3,6 +3,7 @@ struct StateEstimatorBuffer{NT<:Real} û ::Vector{NT} k̄ ::Vector{NT} x̂ ::Vector{NT} + a ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} Ŵ ::Vector{NT} diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 815bf772d..16dbe2679 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -142,11 +142,11 @@ function ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) end """ - ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, a0, x̂0, d0) + ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) Same than [`ĥ!`](@ref) for [`NonLinModelDAE`](@ref) but with the algebraic variable `a0`. """ -function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, a0, x̂0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] model.h!(ŷ0, xd, a0, d0, model.p) @@ -155,11 +155,11 @@ function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, a0, x̂0, d0) end """ - ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, _ , x̂0, d0) + ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x0, _ , d0) Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. """ -ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, _ , x̂0, d0) = ĥ!(ŷ0, model, estim, x̂0, d0) +ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _ , d0) = ĥ!(ŷ0, model, estim, x̂0, d0) """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 8db383cea..40cf38976 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1438,36 +1438,37 @@ function get_nonlinobj_op( ) where JNT<:Real model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian - nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu - nk̄ = get_nk̄(model, estim.transcription) + nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) + nk̄, nā = get_nk̄(model, estim.transcription), get_nā(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) - nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ + nŴ, nV̂, nX̂, nA, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) + nK̄, nQ̄, nU, nŶ = He*nk̄, He*nā, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) J::Vector{JNT} = zeros(JNT, 1) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K̄::Vector{JNT} = zeros(JNT, nK̄) + K̄::Vector{JNT}, Q̄::Vector{JNT} = zeros(JNT, nK̄), zeros(JNT, nQ̄) Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = zeros(JNT, nZ̃) J_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation window for the preparation of the gradient: @@ -1555,49 +1556,52 @@ function get_nonlincon_oracle( # ----------- common cache for all functions ---------------------------------------- model, con = estim.model, estim.con jac, hess = estim.jacobian, estim.hessian - nx̂, nym, nŷ, nu = estim.nx̂, estim.nym, model.ny, model.nu - nk̄ = get_nk̄(model, estim.transcription) + nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) + nk̄, nā = get_nk̄(model, estim.transcription), get_nā(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) - nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) - nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ + nŴ, nV̂, nX̂, nA, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) + nK̄, nQ̄, nU, nŶ = He*nk̄, He*nā, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myInf = convert(JNT, Inf) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K̄::Vector{JNT} = zeros(JNT, nK̄) + K̄::Vector{JNT}, Q̄::Vector{JNT} = zeros(JNT, nK̄), zeros(JNT, nQ̄) Û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) gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi) + function ℓ_gi( + Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, gi + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = zeros(JNT, nZ̃) ∇gi_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation windows for the preparation of the gradient: @@ -1608,10 +1612,10 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), - Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), + Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) @@ -1658,24 +1662,26 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end - function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function ℓ_geq( + Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end Z̃_∇geq = zeros(JNT, nZ̃) ∇geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), Cache(gc), Cache(g) ) estim.Nk[] = He # see comment above @@ -1685,10 +1691,10 @@ function get_nonlincon_oracle( ∇geq_structure = init_diffstructure(∇geq) if !isnothing(hess) ∇²geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) estim.Nk[] = He # see comment above diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f4a9154f0..92942477d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -27,7 +27,7 @@ end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] na = get_na(estim.model) - nk̄, nā = get_nk̄(estim.model, transcription), transcription.no*na + nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk nx̃ = estim.nε + nx̂ nx̃_nX̂_He = nx̃ + nx̂*estim.He @@ -1183,7 +1183,7 @@ function set_warmstart_mhe!( ) where NT<:Real model, buffer = estim.model, estim.buffer nu, na = model.nu, get_na(model) - nk̄, nā = get_nk̄(estim.model, transcription), na*transcription.no + 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 Z̃s = estim.buffer.Z̃ @@ -1454,18 +1454,18 @@ end @doc raw""" predict_mhe!( V̂, X̂0, A0, _ , _ , Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, ::TranscriptionMethod, + estim::MovingHorizonEstimator, model::SimModel, ::TranscriptionMethod, x̂0arr, a0arr, _ , Z̃ ) -> V̂, X̂0, A0 -Compute the vectors when `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). +Compute the vectors for all other cases. The function mutates `V̂`, `X̂0`, and `Ŷ0` vector arguments. The augmented output function [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( - V̂, X̂0, A0, _ , Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::TranscriptionMethod, + V̂, X̂0, A0, _ , _ , Ŷ0, + estim::MovingHorizonEstimator, model::SimModel, transcription::TranscriptionMethod, x̂0arr, a0arr, _ , Z̃ ) nd, ny, na = model.nd, model.ny, get_na(model) @@ -1498,40 +1498,7 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0 -end - - -""" - con_nonlinprog_mhe!( - g, estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod, _ , V̂, gc, ε - ) -> g - -Nonlinear MHE constraint when `model` is [`NonLinModel`](@ref) with non-[`SingleShooting`](@ref). - -The method mutates the `g` vectors in argument and returns it. The estimated sensor noises -and custom constraints are included in the `g` vector. -""" -function con_nonlinprog_mhe!( - g, estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod, _ , V̂, gc, ε -) - nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] - for i in eachindex(g) - estim.con.i_g[i] || continue - if i ≤ nV̂con - j = i - jcon = nV̂con-nV̂+j - g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] - elseif i ≤ 2nV̂con - j = i - nV̂con - jcon = nV̂con-nV̂+j - g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] - else - j = i - 2nV̂con - g[i] = gc[j] - end - end - return g + return V̂, X̂0, A0 end """ @@ -1594,11 +1561,43 @@ function con_nonlinprog_mhe!( return g end +""" + con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod, _ , V̂, gc, ε + ) -> g + +Nonlinear MHE constraint for all other cases. + +The method mutates the `g` vectors in argument and returns it. The estimated sensor noises +and custom constraints are included in the `g` vector. +""" +function con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod, _ , V̂, gc, ε +) + nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] + for i in eachindex(g) + estim.con.i_g[i] || continue + if i ≤ nV̂con + j = i + jcon = nV̂con-nV̂+j + g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] + elseif i ≤ 2nV̂con + j = i - nV̂con + jcon = nV̂con-nV̂+j + g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] + else + j = i - 2nV̂con + g[i] = gc[j] + end + end + return g +end + @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, _ , Û0, K̄, _ , estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, - x̂0arr, Ŵ, Z̃ + x̂0arr, _ , Ŵ, Z̃ ) -> geq Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). @@ -1614,9 +1613,9 @@ for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). """ function con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, _ , Û0, K̄, _ , estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, - x̂0arr, Ŵ, Z̃ + x̂0arr, _ , Ŵ, Z̃ ) nu, nx, nd, nk̄ = model.nu, model.nx, model.nd, model.nk̄ nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He @@ -1650,17 +1649,17 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, ::TrapezoidalCollocation, - x̂0arr, Ŵ, Z̃ + geq, _ , _ , Û0, K̄, Q̄, + estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, + x̂0arr, a0arr, Ŵ, Z̃ ) -> geq -Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). +Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalCollocation`](@ref). By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the deterministic state defects are computed with: ```math -\mathbf{ŝ_d}(ℓ+j+1) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇}_1(ℓ+j) + \mathbf{k̇}_2(ℓ+j)] +\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇}_1(ℓ+j) + \mathbf{k̇}_2(ℓ+j)] + \mathbf{ŵ_d}(ℓ+j) - \mathbf{x̂_d}(ℓ+j+1) ``` for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the @@ -1669,59 +1668,69 @@ variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the contin function `model.f!` and: ```math \begin{aligned} -\mathbf{k̇}_1(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d̂_0}(ℓ+j), \mathbf{p}\Big) \\ -\mathbf{k̇}_2(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d̂_0}(ℓ+j+1), \mathbf{p}\Big) +\mathbf{k̇}_1(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d̂_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇}_2(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d̂_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref) documentation. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::TrapezoidalCollocation, - x̂0arr, Ŵ, Z̃ + geq, _ , _ , Û0, K̄, Q̄, + estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, + x̂0arr, a0arr, Ŵ, Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads Ts = model.Ts - nk̄ = get_nk̄(model, transcription) + nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) nw = nŵ - nxs nx̃ = estim.nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He p = estim.direct ? 0 : 1 - X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] + X̂0_Z̃, A0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)], Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + + # TODO: continue here, chose how to store ŝk, q1 and q2 in geq and choose if + # TODO: precall fq! before the loop for the first one. + #@views fq!(K̄[1:nx], q0, model, x̂0arr[1:nx], a0arr, Û0[1:nu], estim.D0[]) @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] + a0 = @views a0arr[1:nx] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2*nx] + a0next_Z̃ = @views A0_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] + ŝk = @views geq[(1 + nx*(j-1)):(nx*j)] + k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2nx] + q1, q2 = @views q̄[1:na], q̄[na+1:2na] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) - model.f!(k̇1, x̂d_Z̃, û0, d0, model.p) + fq!(k̇1, q1, model, x̂d_Z̃, a0, û0, d0) else k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 end if h < 1 - model.f!(k̇2, x̂dnext_Z̃, û0, d0next, model.p) + fq!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0, d0next) else # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - model.f!(k̇2, x̂dnext_Z̃, û0next, d0next, model.p) + fq!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0next, d0next) end - ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) - ŝdnext .+= ŵd + ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + ŝk .+= ŵd end Nk < He && (geq[nx*Nk+1:end] .= 0) return geq @@ -1729,18 +1738,18 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, ::OrthogonalCollocation, - x̂0arr, _ , Z̃ + geq, _ , _ , Û0, K̄, Q̄, + estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, + x̂0arr, a0arr, _ , Z̃ ) -> geq -Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). +Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`OrthogonalCollocation`](@ref). By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the defects between the deterministic state derivative at the ``n_o`` collocation points and the model dynamics are computed by: ```math -\mathbf{ŝ_k}(ℓ+j) +\mathbf{ŝ_k̄}(ℓ+j) = \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ \mathbf{k}_2(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ @@ -1760,9 +1769,9 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄, - estim::MovingHorizonEstimator, model::NonLinModel, transcription::OrthogonalCollocation, - x̂0arr, _ , Z̃ + geq, _ , _ , Û0, K̄, Q̄, + estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, + x̂0arr, a0arr, _ , Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, He = estim.nx̂, estim.He @@ -1817,4 +1826,4 @@ function con_nonlinprogeq_mhe!( end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModelODE, ::TranscriptionMethod, _,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index eeebc9cc3..15d9d8b72 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -404,6 +404,10 @@ end get_na(model::NonLinModelDAE) = model.na get_na(model::SimModel) = 0 +"Get length of the `ā` vector with all the algebraic variable collocation pts." +get_nā(model::NonLinModelDAE, transcription::CollocationMethod) = model.nx*transcription.no +get_nā(::SimModel, ::TranscriptionMethod) = 0 + "Get the number of elements in the optimization decision vector `Z` for DAE solving." function get_nZ_dae(transcription::OrthogonalCollocation, nx, na) return nx + transcription.no*nx + na + transcription.no*na @@ -679,7 +683,7 @@ function con_nonlinprogeq!( ) nx, na = model.nx, model.na Mo, no = model.Mo, transcription.no - nk̄, nā = get_nk̄(model, transcription), no*na + nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) a0_Z, k̄_Z, ā_Z = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] q0, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] @views model.fq!(k̄[1:nx], q0, x0, a0_Z, u0, d0, model.p) @@ -766,6 +770,12 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) return nothing end +"Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref)." +function fq!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) + return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +end +fq!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0, model.p) + function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) model.beq .= @. -model.Fs diff --git a/src/sim_model.jl b/src/sim_model.jl index b0f4a6f32..33dbc27d6 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -377,7 +377,7 @@ function validate_args(model::SimModel, d, u=nothing) end end -"Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." +"Get length of the `k̄` vector with all the solver intermediate steps or all the collocation pts." get_nk̄(model::SimModel, ::ShootingMethod) = model.nk̄ get_nk̄(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no From 0ec88596555bc381bb0b00a9ba8d848f31fc43c1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 12:44:31 -0400 Subject: [PATCH 04/35] doc: minor correction --- src/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 20965b2d2..e13a0cccf 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -205,13 +205,13 @@ transcription method. \quad \text{and} \quad \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+2) \\ \vdots \\ \mathbf{a_0}(k+p) \end{bmatrix} ``` See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision - variables at the beginning (``N_k < He``). + variables at the beginning (``N_k < H_e``). Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). From a226d1fb58a7804534a4e75fc64331189b5bfebe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 12:47:01 -0400 Subject: [PATCH 05/35] wip: idem --- src/estimator/mhe/transcription.jl | 41 ++++++++++++++++++------------ 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 92942477d..5eeb7e44e 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1660,12 +1660,12 @@ By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the de 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{ŵ_d}(ℓ+j) - \mathbf{x̂_d}(ℓ+j+1) + - \mathbf{x̂_d}(ℓ+j+1) + \mathbf{ŵ_d}(ℓ+j) ``` for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the deterministic state and process noise estimates, respectively, extracted from the decision -variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continuous-time -function `model.f!` and: +variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continuous-time +function [`fq!`](@ref) and: ```math \begin{aligned} \mathbf{k̇}_1(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d̂_0}(ℓ+j), \mathbf{p}\Big) \\ @@ -1673,7 +1673,14 @@ function `model.f!` and: \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂!`](@ref) documentation. +is defined in [`f̂!`](@ref) documentation. The algebraic variable residuals for +[`NonLinModelDAE`](@ref) are also computed from ``j = 0, 1, ... , N_k-1`` and: +```math +\begin{aligned} +\mathbf{q}_1(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+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+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d̂_0}(ℓ+j+1), \mathbf{p}\Big) +\end{aligned} +``` """ function con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, Q̄, @@ -1685,17 +1692,16 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads Ts = model.Ts + na = get_na(model) nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) nw = nŵ - nxs nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He - p = estim.direct ? 0 : 1 + nŜk̄, nQ̄ = nx*He, 2*na*He + i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃, A0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)], Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - - # TODO: continue here, chose how to store ŝk, q1 and q2 in geq and choose if - # TODO: precall fq! before the loop for the first one. - #@views fq!(K̄[1:nx], q0, model, x̂0arr[1:nx], a0arr, Û0[1:nu], estim.D0[]) + Ŝk̄, Q̄ = @views geq[1:nŜk̄], geq[(nŜk̄+1):nQ̄] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1704,23 +1710,24 @@ function con_nonlinprogeq_mhe!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end - d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] a0next_Z̃ = @views A0_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] - ŝk = @views geq[(1 + nx*(j-1)):(nx*j)] + ŝk = @views Ŝk̄[(1 + nx*(j-1)):(nx*j)] k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2nx] q1, q2 = @views q̄[1:na], q̄[na+1:2na] - d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] 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!(k̇1, q1, model, x̂d_Z̃, a0, û0, d0) else - k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇2 of the last iter. j-1 + q1 .= @views Q̄[(1 + nā*(j-1)-na):(nā*(j-1))] # q2 of the last iter. j-1 end if h < 1 fq!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0, d0next) @@ -1729,10 +1736,12 @@ function con_nonlinprogeq_mhe!( û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] fq!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0next, d0next) end - ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) - ŝk .+= ŵd + ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + ŵd + end + if Nk < He + Ŝk[(nx*Nk + 1):end] .= 0 + Q̄[(nā*Nk + 1):end] .= 0 end - Nk < He && (geq[nx*Nk+1:end] .= 0) return geq end From 8eedf6232d68cf62f90bf6ebbc33efe8f3c2f244 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 13:06:16 -0400 Subject: [PATCH 06/35] changed: new notation for `Z` in `TrapezoidalCollocation` --- src/model/nonlinmodeldae.jl | 26 ++++++++++++++++++++------ src/transcription.jl | 22 +++++++++++++++++----- 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 15d9d8b72..d2af0de1c 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -664,20 +664,34 @@ function update_predictions!(k̄, geq, model, Z) return nothing end +""" + con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z + ) -> geq + +TBW +""" function con_nonlinprogeq!( geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z ) nx, na = model.nx, model.na Ts = model.Ts - x0next_Z, a0_Z, a0next_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] - sknext, q1, q2 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] - k̇1, k̇2 = @views k̄[1:nx], k̄[(nx+1):(2nx)] - model.fq!(k̇1, q1, x0, a0_Z, u0, d0, model.p) - model.fq!(k̇2, q2, x0next_Z, a0next_Z, u0, d0, model.p) - sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇1 + k̇2) + 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, a1_Z, u0, d0, model.p) + sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end +""" + con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z + ) -> geq + +TBW +""" function con_nonlinprogeq!( geq, k̄, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z ) diff --git a/src/transcription.jl b/src/transcription.jl index e13a0cccf..06b5ba427 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -175,21 +175,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}(k+0) \\ - \mathbf{A_0} \end{bmatrix} - \quad \text{and} \quad + \mathbf{A_0} \\ + \mathbf{Ā} \end{bmatrix} + \: , \quad \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k+1) \\ \mathbf{a_0}(k+2) \\ \vdots \\ \mathbf{a_0}(k+H_p) \end{bmatrix} + \quad \text{and} \quad + \mathbf{Ā} = \begin{bmatrix} + \mathbf{a_1}(k+1) \\ + \mathbf{a_1}(k+2) \\ + \vdots \\ + \mathbf{a_1}(k+H_p) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math @@ -202,12 +208,18 @@ transcription method. \mathbf{0_a} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} - \quad \text{and} \quad + \: , \quad \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k-N_k+p+1) \\ \mathbf{a_0}(k-N_k+p+2) \\ \vdots \\ \mathbf{a_0}(k+p) \end{bmatrix} + \quad \text{and} \quad + \mathbf{Ā} = \begin{bmatrix} + \mathbf{a_1}(k-N_k+p+1) \\ + \mathbf{a_1}(k-N_k+p+2) \\ + \vdots \\ + \mathbf{a_1}(k+p) \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 From 38e9d2ac023df432c69e83406ddace8f89c8f002 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 13:09:07 -0400 Subject: [PATCH 07/35] changed: idem --- src/transcription.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index 06b5ba427..f15dacd97 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -203,9 +203,10 @@ 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{Ā} \\ + \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} \: , \quad From a1ea688deea4370b397b3d4aa25716bb19cd96f5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 15:48:20 -0400 Subject: [PATCH 08/35] doc: minor clarification --- src/model/nonlinmodeldae.jl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index d2af0de1c..71ef1f446 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -402,10 +402,11 @@ end "Get the number of algebraic variable `na` in `model`." get_na(model::NonLinModelDAE) = model.na -get_na(model::SimModel) = 0 +get_na(::SimModel) = 0 -"Get length of the `ā` vector with all the algebraic variable collocation pts." -get_nā(model::NonLinModelDAE, transcription::CollocationMethod) = model.nx*transcription.no +"Get length of the `ā` vector with all the algebraic variable collocation pts (excl. τ=1)." +get_nā(model::NonLinModelDAE, transcription::OrthogonalCollocation) = model.na*transcription.no +get_nā(model::NonLinModelDAE, ::TrapezoidalCollocation) = model.na get_nā(::SimModel, ::TranscriptionMethod) = 0 "Get the number of elements in the optimization decision vector `Z` for DAE solving." From 2f30683da018cc41d628b48104d3a4ef7b1e5d38 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 15:49:03 -0400 Subject: [PATCH 09/35] doc: correction in `Z` for TC and OC --- src/transcription.jl | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index f15dacd97..578ffd6a0 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -192,10 +192,10 @@ transcription method. \mathbf{a_0}(k+H_p) \end{bmatrix} \quad \text{and} \quad \mathbf{Ā} = \begin{bmatrix} - \mathbf{a_1}(k+1) \\ - \mathbf{a_1}(k+2) \\ + \mathbf{a}(k+0) \\ + \mathbf{a}(k+1) \\ \vdots \\ - \mathbf{a_1}(k+H_p) \end{bmatrix} + \mathbf{a}(k+H_p-1) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math @@ -217,14 +217,16 @@ transcription method. \mathbf{a_0}(k+p) \end{bmatrix} \quad \text{and} \quad \mathbf{Ā} = \begin{bmatrix} - \mathbf{a_1}(k-N_k+p+1) \\ - \mathbf{a_1}(k-N_k+p+2) \\ + \mathbf{a}(k-N_k+p) \\ + \mathbf{a}(k-N_k+p) \\ \vdots \\ - \mathbf{a_1}(k+p) \end{bmatrix} + \mathbf{a}(k) \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``). + 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 for the `fq!` function. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -323,10 +325,10 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). algebraic variables at the collocation points: ```math \mathbf{ā}(k+j) = \begin{bmatrix} - \mathbf{ā}_1(k+j) \\ - \mathbf{ā}_2(k+j) \\ + \mathbf{a}_1(k+j) \\ + \mathbf{a}_2(k+j) \\ \vdots \\ - \mathbf{ā}_{n_o}(k+j) \end{bmatrix} + \mathbf{a}_{n_o}(k+j) \end{bmatrix} ``` Although not strictly needed, the current algebraic variable ``\mathbf{a_0}(k+0)`` is still included in the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): @@ -387,7 +389,9 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{ā}(k+p-1) \end{bmatrix} ``` 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 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 for the `fq!` function. The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. From 1d8a3382a6ba2cf0244143b63d81182596937d03 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 17:30:03 -0400 Subject: [PATCH 10/35] wip: idem --- src/estimator/mhe/construct.jl | 42 ++++++------- src/estimator/mhe/execute.jl | 6 +- src/estimator/mhe/transcription.jl | 98 +++++++++++++++++++----------- src/transcription.jl | 11 ++-- 4 files changed, 91 insertions(+), 66 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 40cf38976..2ef719b2e 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1439,11 +1439,11 @@ function get_nonlinobj_op( model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) - nk̄, nā = get_nk̄(model, estim.transcription), get_nā(model, estim.transcription) + nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) nŴ, nV̂, nX̂, nA, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) - nK̄, nQ̄, nU, nŶ = He*nk̄, He*nā, He*nu, He*nŷ + nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) J::Vector{JNT} = zeros(JNT, 1) @@ -1453,13 +1453,13 @@ function get_nonlinobj_op( V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K̄::Vector{JNT}, Q̄::Vector{JNT} = zeros(JNT, nK̄), zeros(JNT, nQ̄) + K̄::Vector{JNT} = zeros(JNT, nK̄) Û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̄, Q̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -1468,7 +1468,7 @@ function get_nonlinobj_op( Cache(x̂0arr), Cache(a0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation window for the preparation of the gradient: @@ -1557,13 +1557,13 @@ function get_nonlincon_oracle( model, con = estim.model, estim.con jac, hess = estim.jacobian, estim.hessian nx̂, nym, nŷ, nu, na = estim.nx̂, estim.nym, model.ny, model.nu, get_na(model) - nk̄, nā = get_nk̄(model, estim.transcription), get_nā(model, estim.transcription) + nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) nŴ, nV̂, nX̂, nA, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) - nK̄, nQ̄, nU, nŶ = He*nk̄, He*nā, He*nu, He*nŷ + nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myInf = convert(JNT, Inf) @@ -1573,25 +1573,25 @@ function get_nonlincon_oracle( V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K̄::Vector{JNT}, Q̄::Vector{JNT} = zeros(JNT, nK̄), zeros(JNT, nQ̄) + K̄::Vector{JNT} = zeros(JNT, nK̄) Û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) 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̄, Q̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end function ℓ_gi( - Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -1601,7 +1601,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(a0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation windows for the preparation of the gradient: @@ -1615,7 +1615,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(a0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) @@ -1662,17 +1662,17 @@ 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̄, Q̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end function ℓ_geq( - Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Q̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -1681,7 +1681,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(a0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) estim.Nk[] = He # see comment above @@ -1694,7 +1694,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(a0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Q̄), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) estim.Nk[] = He # see comment above diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ba1da9a3d..fca85ce51 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -884,7 +884,7 @@ end """ update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ā, Ŷ0, gc, g, geq, + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -893,7 +893,7 @@ 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, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription @@ -909,7 +909,7 @@ function update_predictions!( 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, A0, Û0, K̄, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 5eeb7e44e..17c6ec3df 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -2,7 +2,7 @@ get_nZ_mhe(::SingleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nŵ*He get_nZ_mhe(::MultipleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nx̂*He + nŵ*He function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, _ , nŵ, na=0) - return nx̂ + nx̂*He + na + na*He + nŵ*He + return nx̂ + nx̂*He + na + na*He + na*He + nŵ*He end function get_nZ_mhe(transcription::OrthogonalCollocation, He, nx̂, nk̄, nŵ, na=0) nā = na*transcription.no @@ -15,12 +15,15 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) na = get_na(estim.model) nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*estim.He - nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na + na*estim.He + nx̃_nX̂_He = nx̃ + nx̂*estim.He + na_nA_He = na + na*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He + nx̃_nX̂_na_nA_nĀ_He = nx̃_nX̂_na_nA_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA) - (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nŴ) + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nA) + (1 + nx̃_nX̂_na_nA_nĀ_He):(nx̃_nX̂_na_nA_nĀ_He + nŴ) ] return i_Z̃_NK end @@ -31,9 +34,10 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk nx̃ = estim.nε + nx̂ nx̃_nX̂_He = nx̃ + nx̂*estim.He - nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na + na*estim.He + na_nA_He = na + na*estim.He + nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He nx̃_nX̂_na_nA_nK̄_He = nx̃_nX̂_na_nA_He + nk̄*estim.He - nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃_nX̂_na_nA_nK̄_He + nā*estim.He + nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃_nX̂_na_nA_nK̄_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA); @@ -556,16 +560,17 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{I} \end{bmatrix} \\ \mathbf{E_S^a} &= \mathbf{0} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^ŵ} \end{bmatrix} + \mathbf{E_S^ā} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \end{aligned} ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::TranscriptionMethod, ::Bool, + model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) - na = get_na(model) + na, nā = get_na(model), get_nā(model, transcription) nx = nx̂ - nxs nŵ = nx̂ nŵd = nŵ - nxs @@ -577,7 +582,8 @@ function init_defectmat_mhe( end ESŵ = repeatdiag([zeros(NT, nxs, nŵd) I], He) ESa = zeros(nxs*He, na + na*He) - ES = [ESx̂ ESa ESŵ] + ESā = zeros(nxs*He, nā*He) + ES = [ESx̂ ESa ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -636,7 +642,8 @@ function init_defectmat_mhe( ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs - nk̄, na = get_nk̄(model, transcription), get_na(model) + na, nā = get_na(model), get_nā(model, transcription) + nk̄ = get_nk̄(model, transcription) λo_I = λo*I(nx) ESx̂ = [zeros(NT, nx̂*He, nx̂) -I] for j=1:He @@ -650,7 +657,7 @@ function init_defectmat_mhe( ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], He) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented ESa = zeros(nx̂*He, na + na*He) - ESā = zeros(nx̂*He, na*transcription.no*He) + ESā = zeros(nx̂*He, nā*He) ES = [ESx̂ ESa ESk̄ ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) @@ -769,6 +776,7 @@ function init_matconstraint_mhe( nx̂ = length(x̂0min) nAeq = size(Aeq, 1) # number of linear equality constraints neq = nZ̃ - nŴ - nε - nx̂ - nAeq # number of nonlinear equality constraints + @show neq end i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) @@ -1595,7 +1603,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, _ , Û0, K̄, _ , + geq, X̂0, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) -> geq @@ -1613,7 +1621,7 @@ for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). """ function con_nonlinprogeq_mhe!( - geq, X̂0, _ , Û0, K̄, _ , + geq, X̂0, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) @@ -1649,7 +1657,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, Q̄, + geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, x̂0arr, a0arr, Ŵ, Z̃ ) -> geq @@ -1659,7 +1667,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̇_1}(ℓ+j) + \mathbf{k̇_2}(ℓ+j)] - \mathbf{x̂_d}(ℓ+j+1) + \mathbf{ŵ_d}(ℓ+j) ``` for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the @@ -1668,22 +1676,26 @@ variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continu function [`fq!`](@ref) and: ```math \begin{aligned} -\mathbf{k̇}_1(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+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̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇_2}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂!`](@ref) documentation. The algebraic variable residuals for -[`NonLinModelDAE`](@ref) are also computed from ``j = 0, 1, ... , N_k-1`` and: +is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`](@ref) are +also computed from ``j = 0, 1, ... , N_k-1`` and: ```math \begin{aligned} -\mathbf{q}_1(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+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+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d̂_0}(ℓ+j+1), \mathbf{p}\Big) +\mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q_2}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` +and also one more residual at the arrival: +```math +\mathbf{q_0}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{a_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) +``` """ function con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, Q̄, + geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, a0arr, Ŵ, Z̃ ) @@ -1693,15 +1705,26 @@ function con_nonlinprogeq_mhe!( f_threads = transcription.f_threads Ts = model.Ts na = get_na(model) - nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) + nk̄ = get_nk̄(model, transcription) nw = nŵ - nxs - nx̃ = estim.nε + nx̂ - nx̃_nX̂ = nx̃ + nx̂*estim.He - nŜk̄, nQ̄ = nx*He, 2*na*He + nx̃ = estim.nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He + nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He + nX, nA = nx*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃, A0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)], Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] - Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - Ŝk̄, Q̄ = @views geq[1:nŜk̄], geq[(nŜk̄+1):nQ̄] + 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!(k̇0, q0arr, model, x̂darr, a0arr, û0arr, d0arr) + end @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1713,13 +1736,13 @@ function con_nonlinprogeq_mhe!( 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)] - q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] + q1 = @views Q̄[(1 + na*(j-1)):(na*j)] + q2 = @views Q0[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - a0next_Z̃ = @views A0_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] + a_Z̃ = @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] - q1, q2 = @views q̄[1:na], q̄[na+1:2na] d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the @@ -1740,14 +1763,15 @@ function con_nonlinprogeq_mhe!( end if Nk < He Ŝk[(nx*Nk + 1):end] .= 0 - Q̄[(nā*Nk + 1):end] .= 0 + Q0[(na*Nk + 1):end] .= 0 + Q̄[(na*Nk + 1):end] .= 0 end return geq end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, Q̄, + geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, x̂0arr, a0arr, _ , Z̃ ) -> geq @@ -1778,7 +1802,7 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, Q̄, + geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, x̂0arr, a0arr, _ , Z̃ ) @@ -1835,4 +1859,4 @@ function con_nonlinprogeq_mhe!( end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file diff --git a/src/transcription.jl b/src/transcription.jl index 578ffd6a0..6da72ff06 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -203,6 +203,7 @@ 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{Ā} \\ @@ -217,16 +218,16 @@ transcription method. \mathbf{a_0}(k+p) \end{bmatrix} \quad \text{and} \quad \mathbf{Ā} = \begin{bmatrix} - \mathbf{a}(k-N_k+p) \\ - \mathbf{a}(k-N_k+p) \\ + \mathbf{a}(k-N_k+p+0) \\ + \mathbf{a}(k-N_k+p+1) \\ \vdots \\ - \mathbf{a}(k) \end{bmatrix} + \mathbf{a}(k+p-1) \end{bmatrix} ``` See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at the beginning (``N_k < H_e``). The predicted outputs are computed from the algebraic variables in ``\mathbf{A_0}``, while the values in ``\mathbf{Ā}`` are - strictly for the `fq!` function. + strictly used in the the `fq!` function. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -391,7 +392,7 @@ 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 for the `fq!` function. + in ``\mathbf{Ā}`` are strictly used in the the `fq!` function. The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. From 2c52d919fe531fd515963518b37c25aea3479ef6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 17:43:25 -0400 Subject: [PATCH 11/35] wip: idem --- src/estimator/mhe/transcription.jl | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 17c6ec3df..5329f8855 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1739,27 +1739,27 @@ function con_nonlinprogeq_mhe!( q1 = @views Q̄[(1 + na*(j-1)):(na*j)] q2 = @views Q0[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] - x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - a_Z̃ = @views Ā_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] + 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] d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] 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!(k̇1, q1, model, x̂d_Z̃, a0, û0, d0) + fq!(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 Q̄[(1 + nā*(j-1)-na):(nā*(j-1))] # q2 of the last iter. j-1 + k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇2 of the last iter. j-1 + q1 .= @views Q0[(1 + na*(j-1)-na):(na*(j-1))] # q2 of the last iter. j-1 end if h < 1 - fq!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0, d0next) + fq!(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!(k̇2, q2, model, x̂dnext_Z̃, a0next_Z̃, û0next, d0next) + fq!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) end - ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + ŵd + ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd end if Nk < He Ŝk[(nx*Nk + 1):end] .= 0 From fbb71cfd969cfba915d765febfb826dbcd93fb4b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 17:44:24 -0400 Subject: [PATCH 12/35] wip: idem --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 5329f8855..4589c1fd7 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1706,7 +1706,7 @@ function con_nonlinprogeq_mhe!( Ts = model.Ts na = get_na(model) nk̄ = get_nk̄(model, transcription) - nw = nŵ - nxs + nw = nŵ - nxs nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He @@ -1761,7 +1761,7 @@ function con_nonlinprogeq_mhe!( end ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd end - if Nk < He + if Nk < He Ŝk[(nx*Nk + 1):end] .= 0 Q0[(na*Nk + 1):end] .= 0 Q̄[(na*Nk + 1):end] .= 0 From 79693ce6eab88a753953be600c8c23712835d09e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 17 Sep 2026 22:58:34 -0400 Subject: [PATCH 13/35] wip: idem --- src/estimator/construct.jl | 6 ++++-- src/estimator/mhe/transcription.jl | 26 ++++++++++++++------------ 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index def47e2f8..400383ac1 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -8,6 +8,7 @@ struct StateEstimatorBuffer{NT<:Real} V̂ ::Vector{NT} Ŵ ::Vector{NT} X̂ ::Vector{NT} + A ::Vector{NT} Ŷ ::Vector{NT} D ::Vector{NT} P̂ ::Matrix{NT} @@ -37,7 +38,7 @@ function StateEstimatorBuffer{NT}( transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) - nV̂, nŴ, nX̂, nŶ, nD = nym*He, nŵ*He, nx̂*He, ny*He, nd*(He+1) + nV̂, nŴ, nX̂, nA, nŶ, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) @@ -47,6 +48,7 @@ function StateEstimatorBuffer{NT}( V̂ = Vector{NT}(undef, nV̂) Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) + A = Vector{NT}(undef, nA) Ŷ = Vector{NT}(undef, nŶ) D = Vector{NT}(undef, nD) P̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -58,7 +60,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 4589c1fd7..1cfb645b9 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -776,7 +776,6 @@ function init_matconstraint_mhe( nx̂ = length(x̂0min) nAeq = size(Aeq, 1) # number of linear equality constraints neq = nZ̃ - nŴ - nε - nx̂ - nAeq # number of nonlinear equality constraints - @show neq end i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) @@ -903,9 +902,9 @@ function linconstraint!( return nothing end -"Set `b` excluding sensor noise bounds if for `NonLinModel` and non-`SingleShooting`." +"Set `b` excluding sensor noise bounds if not a `LinModel` and non-`SingleShooting`." function linconstraint!( - estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod ) nx̂, nŵ = estim.nx̂, estim.nx̂ # --- truncate vector and matrices if necessary --- @@ -1007,7 +1006,7 @@ end """ linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod ) By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@ref). @@ -1017,7 +1016,7 @@ vector is only zeros for this specific case. See [`init_defectmat_mhe`](@ref) fo equations. """ function linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod ) optim, con, Nk = estim.optim, estim.con, estim.Nk[] nŝ = size(con.Aeq, 1) ÷ estim.He # number of state defects per time step @@ -1051,7 +1050,7 @@ function linconstrainteq!( return nothing end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." -linconstrainteq!(::MovingHorizonEstimator, ::SimModelODE, ::SingleShooting) = nothing +linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing @doc raw""" set_warmstart_mhe!( @@ -1305,13 +1304,16 @@ function set_warmstart_mhe!( Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nŵ+1):(nx̃+nX̂+nŴ)] Z̃s[(nx̃+nX̂+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄ = buffer.x̂ - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations + x̄, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ + Û0, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old + a0arr = geta0arr!(a0arr, estim, transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!( + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+1:end] .= 0 # Ŵ = 0 @@ -1354,7 +1356,7 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, A0, _ , _ , + V̂, X̂0, A0, _ , _ , _ , estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, _ , _ , _ , Z̃ ) -> V̂, X̂0, A0 @@ -1762,7 +1764,7 @@ function con_nonlinprogeq_mhe!( ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd end if Nk < He - Ŝk[(nx*Nk + 1):end] .= 0 + Ŝk̄[(nx*Nk + 1):end] .= 0 Q0[(na*Nk + 1):end] .= 0 Q̄[(na*Nk + 1):end] .= 0 end From c9caa44aaef55e610487dff684f179596cb8cb65 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 11:08:42 -0400 Subject: [PATCH 14/35] added: MHE with DAE start to work! --- src/estimator/mhe/execute.jl | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index fca85ce51..c2e9c1d2b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -652,13 +652,16 @@ otherwise the state is for the next time step. """ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real model, buffer = estim.model, estim.buffer - nu, nk̄, nx̂, Nk = model.nu, model.nk̄, estim.nx̂, estim.Nk[] - x̂0arr = buffer.x̂ - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ + nu, nx̂, Nk = model.nu, estim.nx̂, estim.Nk[] + nk̄ = get_nk̄(model, estim.transcription) + x̂0arr, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getx̂0arr!(x̂0arr, estim, Z̃) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) + predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing end From 634799aad3b95ecd73453b7d25053b3703ff611c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 11:55:41 -0400 Subject: [PATCH 15/35] debug: various bugfixes with MHE and DAEs --- src/estimator/execute.jl | 4 +- src/estimator/mhe/execute.jl | 101 ++++++++++++++++------------- src/estimator/mhe/transcription.jl | 8 +-- 3 files changed, 63 insertions(+), 50 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 16dbe2679..db51895a3 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -144,7 +144,7 @@ end """ ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) -Same than [`ĥ!`](@ref) for [`NonLinModelDAE`](@ref) but with the algebraic variable `a0`. +Same than [`ĥ!`](@ref) for [`NonLinModelDAE`](@ref) but with algebraic variable `a0`. """ function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @@ -159,7 +159,7 @@ end Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. """ -ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _ , d0) = ĥ!(ŷ0, model, estim, x̂0, d0) +ĥ!(ŷ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/execute.jl b/src/estimator/mhe/execute.jl index c2e9c1d2b..fb38a788f 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -149,16 +149,19 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ Z̃ = estim.Z̃ info = Dict{Symbol, Any}() - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - x̂0arr = buffer.x̂ + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ + x̂0arr, a0arr = buffer.x̂, buffer.a x̄, Û0, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) - x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) - Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) - x̄ = getx̄!(x̄, estim, x̂0arr) - V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃) - Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr) - J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) - yopm = model.yop[estim.i_ym] + x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) + x̄ = getx̄!(x̄, estim, x̂0arr) + V̂, X̂0, A0 = predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + ) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, A0, x̂0arr, a0arr) + J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) + yopm = model.yop[estim.i_ym] Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)] Ym, U, D, Ŷ, X̂, x̂arr = Ym0, U0, D0, Ŷ0, X̂0, x̂0arr for i=1:Nk @@ -213,35 +216,36 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher # --- objective derivatives --- optim, con = estim.optim, estim.con hess = estim.hessian - nx̂, nym, nŷ, nu, nc = estim.nx̂, estim.nym, model.ny, model.nu, con.nc + nx̂, nym, nŷ, nu, nc, na = estim.nx̂, estim.nym, model.ny, model.nu, con.nc, get_na(model) nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) - nV̂, nX̂, nŴ = He*nym, He*nx̂, He*nx̂ - nK, nU, nŶ = He*nk̄, He*nu, He*nŷ + nV̂, nX̂, nA, nŴ = He*nym, He*nx̂, He*na, He*nx̂ + nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) + a0arr, A0 = zeros(NT, na), zeros(NT, nA) Ŵ = zeros(NT, nŴ) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) Ŵe = zeros(NT, nŴe) V̂e, X̂e = zeros(NT, nV̂e), zeros(NT, nX̂e) - K = zeros(NT, nK) + K̄ = zeros(NT, nK̄) Û0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) gc, g = zeros(NT, nc), zeros(NT, ng) geq = zeros(NT, neq) gi = zeros(NT, ngi) J_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -256,15 +260,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- inequality constraint derivatives --- ∇g_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing @@ -288,16 +292,18 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²g_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), - Cache(gc), Cache(g), Cache(geq), + Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(gc), Cache(g), Cache(geq), Cache(gi) ) - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi) + function ℓ_gi( + Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -310,15 +316,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- equality constraint derivatives --- ∇geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end @@ -341,15 +347,17 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²geq_cache = ( - Cache(x̂0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(a0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function ℓ_geq( + Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq + ) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -859,28 +867,33 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ end @doc raw""" - predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) -> Ŷ0 + predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) -> Ŷ0 Predict in-place the outputs of `estim` [`MovingHorizonEstimator`](@ref). This function is not used for the optimization, but it can be useful to predict the estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the -`Ŷ0` vector. The argument `X̂0` is computed from [`predict_mhe!`](@ref) and contains the -estimated states from ``k-N_k+1+p`` to ``k+p``. The argument `x̂0arr` is computed from -[`getx̂0arr!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. +`Ŷ0` vector. The arguments `X̂0` and `A0` are computed from [`predict_mhe!`](@ref) and +contains the estimated states and algebraic variable from ``k-N_k+1+p`` to ``k+p``. The +argument `x̂0arr` and `a0arr` are computed from [`getx̂0arr!`](@ref) and [`geta0arr!`](@ref). +They respectively contains the arrival state and algebraic estimates for the time step +``k-N_k+p``. """ -function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) +function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) model = estim.model nd, ny, nx̂, Nk = model.nd, model.ny, estim.nx̂, estim.Nk[] + na = get_na(model) D0 = estim.D0 p = estim.direct ? 0 : 1 x̂0 = @views estim.direct ? X̂0[1:nx̂] : x̂0arr[1:nx̂] + a0 = @views estim.direct ? A0[1:na] : a0arr[1:na] for j=1:Nk d0 = @views D0[(1 + nd*j):(nd*(j+1))] # 1st data in D0 is d0(k-Nk), not used here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] ĥ!(ŷ0, estim, estim.model, x̂0, d0) j < Nk || break x̂0 = @views X̂0[(1 + nx̂*(j-p)):(nx̂*(j-p+1))] + a0 = @views A0[(1 + na*(j-p)):(na*(j-p+1))] end return Ŷ0 end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 1cfb645b9..01159ccca 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1356,9 +1356,9 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, A0, _ , _ , _ , + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, - _ , _ , _ , Z̃ + x̂0arr, a0arr, Ŵ, Z̃ ) -> V̂, X̂0, A0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -1402,7 +1402,7 @@ end predict_mhe!( V̂, X̂0, A0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, _ , Ŵ, _ + x̂0arr, a0arr, Ŵ, Z̃ ) -> V̂, X̂0, A0 Compute the vectors when `model` is a [`NonLinModel`](@ref) with [`SingleShooting`](@ref). @@ -1730,7 +1730,7 @@ function con_nonlinprogeq_mhe!( @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0 = @views a0arr[1:nx] + a0 = @views a0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] From 1cac47a18275aafa2b46287b8dbcd54acf7e9d7f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 12:50:20 -0400 Subject: [PATCH 16/35] debug: MHE new predict function --- docs/src/internals/sim_model.md | 1 + src/estimator/mhe/transcription.jl | 8 +++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/src/internals/sim_model.md b/docs/src/internals/sim_model.md index 820d9d870..0d6962b04 100644 --- a/docs/src/internals/sim_model.md +++ b/docs/src/internals/sim_model.md @@ -22,6 +22,7 @@ ModelPredictiveControl.init_defectmat_dae ```@docs ModelPredictiveControl.f! ModelPredictiveControl.h! +ModelPredictiveControl.fq! ``` ## Init State diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 01159ccca..f873e8f5f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1103,13 +1103,15 @@ function set_warmstart_mhe!( Z̃s[(nx̃+1):(nx̃+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nŵ+1):(nx̃+nŴ)] Z̃s[(nx̃+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄ = buffer.x̂ - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ + x̄, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) + predict_mhe!( + V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] .= 0 # Ŵ = 0 From 42478b8426261938ac5bf5680d75506d4ee6e349 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 16:30:35 -0400 Subject: [PATCH 17/35] doc: change `Z` notation for DAE+TC simulations --- src/model/nonlinmodeldae.jl | 12 ++++++++---- src/transcription.jl | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 71ef1f446..d72909740 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -677,11 +677,11 @@ function con_nonlinprogeq!( ) nx, na = model.nx, model.na Ts = model.Ts - x0next_Z, a0_Z, a1_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] + 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)] 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, a1_Z, u0, d0, model.p) + model.fq!(k̇0, q0, x0, a0_Z, u0, d0, model.p) + model.fq!(k̇1, q1, x0next_Z, a0next_Z, u0, d0, model.p) sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end @@ -785,7 +785,11 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) return nothing end -"Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref)." +""" + fq!(ẋ0, q0, model, x0, a0, u0, d0) + +Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). +""" function fq!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) end diff --git a/src/transcription.jl b/src/transcription.jl index 6da72ff06..364f5ebe9 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -175,7 +175,7 @@ transcription method. \mathbf{Z} = \begin{bmatrix} \mathbf{x_0}(k+1) \\ \mathbf{a_0}(k+0) \\ - \mathbf{a_1}(k+0) \end{bmatrix} + \mathbf{a_0}(k+1) \end{bmatrix} ``` For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: ```math From b6d619a1701980d9c485ce90e86a1d7c9cc06f8e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 16:38:35 -0400 Subject: [PATCH 18/35] debug: warm-starting OC + MHE --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f873e8f5f..3c63cbacd 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1226,8 +1226,8 @@ function set_warmstart_mhe!( 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 = estim.x̂0arr_old - a0arr .= + x̂0arr = estim.x̂0arr_old + a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ā, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) From c6f5871df00ed4e5cdf4f65474b2ba095b9b8233 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 16:43:48 -0400 Subject: [PATCH 19/35] doc: minor corrections in `Z` for all transcriptions --- src/transcription.jl | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 364f5ebe9..6875610af 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -184,13 +184,13 @@ transcription method. \mathbf{X̂_0} \\ \mathbf{A_0} \\ \mathbf{Ā} \end{bmatrix} - \: , \quad + , \: \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k+1) \\ \mathbf{a_0}(k+2) \\ \vdots \\ \mathbf{a_0}(k+H_p) \end{bmatrix} - \quad \text{and} \quad + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{a}(k+0) \\ \mathbf{a}(k+1) \\ @@ -210,13 +210,13 @@ transcription method. \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} - \: , \quad + , \: \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k-N_k+p+1) \\ \mathbf{a_0}(k-N_k+p+2) \\ \vdots \\ \mathbf{a_0}(k+p) \end{bmatrix} - \quad \text{and} \quad + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{a}(k-N_k+p+0) \\ \mathbf{a}(k-N_k+p+1) \\ @@ -227,7 +227,7 @@ transcription method. 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 used in the the `fq!` function. + strictly reserved for the the `fq!` function. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -348,13 +348,13 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{A_0} \\ \mathbf{K̄} \\ \mathbf{Ā} \end{bmatrix} - \: , \quad + , \: \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k+1) \\ \mathbf{a_0}(k+2) \\ \vdots \\ \mathbf{a_0}(k+H_p) \end{bmatrix} - \quad \text{and} \quad + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k+0) \\ \mathbf{ā}(k+1) \\ @@ -376,13 +376,13 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} - \: , \quad + , \; \mathbf{A_0} = \begin{bmatrix} \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+2) \\ \vdots \\ \mathbf{a_0}(k+p) \end{bmatrix} - \quad \text{and} \quad + \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k-N_k+p+0) \\ \mathbf{ā}(k-N_k+p+1) \\ @@ -392,7 +392,7 @@ 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 used in the the `fq!` function. + in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. From c0842fb33888592168546d2061664ee0422b9bd0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 16:45:11 -0400 Subject: [PATCH 20/35] debug: correct var. name --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 3c63cbacd..75cb4511f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1222,7 +1222,7 @@ function set_warmstart_mhe!( 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̄, a0rr = buffer.x̂, buffer.a + 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 From e23e430de29197ed158d2344c914a3f51e7db218 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 16:47:35 -0400 Subject: [PATCH 21/35] debug: correct signature --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 75cb4511f..6a46d1b7b 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1230,7 +1230,7 @@ function set_warmstart_mhe!( a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ā, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) + predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 From 805c13ff8e5304093223073963f037465b205d7a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 18 Sep 2026 18:43:02 -0400 Subject: [PATCH 22/35] debug: julia 1.10 --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 6a46d1b7b..c65af2e4d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,6 +1,6 @@ "Get the number of elements in the optimization decision vector `Z`" -get_nZ_mhe(::SingleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nŵ*He -get_nZ_mhe(::MultipleShooting, He, nx̂ , _ , nŵ, ::Any=0) = nx̂ + nx̂*He + nŵ*He +get_nZ_mhe(::SingleShooting, He, nx̂ , _ , nŵ, na=0) = nx̂ + nŵ*He +get_nZ_mhe(::MultipleShooting, He, nx̂ , _ , nŵ, na=0) = nx̂ + nx̂*He + nŵ*He function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, _ , nŵ, na=0) return nx̂ + nx̂*He + na + na*He + na*He + nŵ*He end From 108b6054bfc80d7d3d8b470baed39aff0e170366 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 10:12:31 -0400 Subject: [PATCH 23/35] test: new simple estimation test for MHE+DAE+TC --- test/2_test_state_estim.jl | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b46cc981a..a56be2936 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1383,6 +1383,37 @@ end @test mhe11() ≈ [13] atol=5e-3 end +@testitem "MHE estim. & getinfo (NonLinModelDAE)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer + import ForwardDiff + + function fq!(ẋ, res, x, a, u, _ , p) + ẋ[] = -p[] * (x[] - 0.2 * u[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, _ , _ ) + y[] = 2 * x[] + a[] + end + p=[0.5] + dae = NonLinModelDAE(fq!, h!, 1.0, 1, 1, 1, 1; p) + + transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) + mhe = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) + preparestate!(mhe, [0.0]) + x̂ = updatestate!(mhe, [0.0], [0.0]) + @test x̂ ≈ zeros(mhe.nx̂) atol=1e-8 + @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-8 + + transcription = TrapezoidalCollocation(1) + mhe2 = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) + preparestate!(mhe2, [0.0]) + x̂ = updatestate!(mhe2, [0.0], [0.0]) + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-8 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-8 +end + @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin f(x,u,_,_) = 0.5x + u h(x,_,_) = x From d29bc567db6f0758d6ee424fd24636b76a73ae57 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 10:13:21 -0400 Subject: [PATCH 24/35] test: more precise test item name --- test/2_test_state_estim.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index a56be2936..887f8d9e6 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1383,7 +1383,7 @@ end @test mhe11() ≈ [13] atol=5e-3 end -@testitem "MHE estim. & getinfo (NonLinModelDAE)" setup=[SetupMPCtests] begin +@testitem "MHE estim. & getinfo (NonLinModelDAE, TC)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer import ForwardDiff From 49fae15caa51e0e1fae10fdc907dd132d0e1eb15 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 10:40:02 -0400 Subject: [PATCH 25/35] added: support `evaloutput` with MHE+DAE --- docs/src/internals/sim_model.md | 1 - docs/src/internals/state_estim.md | 2 ++ src/estimator/execute.jl | 42 ++++++++++++++++++------------ src/estimator/mhe/transcription.jl | 10 +++---- src/model/nonlinmodeldae.jl | 10 ------- 5 files changed, 33 insertions(+), 32 deletions(-) diff --git a/docs/src/internals/sim_model.md b/docs/src/internals/sim_model.md index 0d6962b04..820d9d870 100644 --- a/docs/src/internals/sim_model.md +++ b/docs/src/internals/sim_model.md @@ -22,7 +22,6 @@ ModelPredictiveControl.init_defectmat_dae ```@docs ModelPredictiveControl.f! ModelPredictiveControl.h! -ModelPredictiveControl.fq! ``` ## Init State diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 359299bfe..9313eb8c0 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -67,6 +67,8 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! +ModelPredictiveControl.fq_dae! +ModelPredictiveControl.ĥ_dae! ``` ## Remove Operating Points diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index db51895a3..18231a06e 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state update function ``\mathbf{f̂}`` of the augmented model. @@ -28,7 +28,7 @@ the function returns the next state of the augmented model, as deviation vectors ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, `û0` and `k` in place. The argument `û0` stores the disturbed input of the augmented model -``\mathbf{û_0}``, and `k`, the intermediate stage values of `model.solver`, when applicable. +``\mathbf{û_0}``, and `k̄`, the intermediate stage values of `model.solver`, when applicable. The model parameter `model.p` is not included in the function signature for conciseness. The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. @@ -61,8 +61,8 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for are computed by [`augment_model`](@ref) (almost always zeros in practice for [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) - return f̂!(x̂0next, û0, k, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, k̄, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end @doc raw""" @@ -92,28 +92,28 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) -Same than [`f̂!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. +Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k̄, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - f!(xdnext, k, model, xd, û0, d0, model.p) + f!(xdnext, k̄, model, xd, û0, d0, model.p) mul!(xsnext, As, xs) x̂0next .+= f̂op .- x̂op return nothing end @doc raw""" - ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) -> nothing + ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref). """ -function ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) return ĥ!(ŷ0, model, estim.Cs_y, x̂0, d0) end @@ -129,11 +129,11 @@ function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0) end """ - ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) + ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) -Same than [`ĥ!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. +Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) +function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] h!(ŷ0, model, xd, d0, model.p) # y0 = h(xd, d0) @@ -142,9 +142,19 @@ function ĥ!(ŷ0, model::SimModelODE, Cs_y::AbstractMatrix, x̂0, d0) end """ - ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) + fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) -Same than [`ĥ!`](@ref) for [`NonLinModelDAE`](@ref) but with algebraic variable `a0`. +Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). +""" +function fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) + return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +end +fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0, model.p) + +""" + ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) + +Similar than [`ĥ!`](@ref) but with a algebraic variable `a0` for [`NonLinModelDAE`](@ref) . """ function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @@ -159,7 +169,7 @@ end Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. """ -ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _ , d0) = ĥ!(ŷ0, estim, model, x̂0, d0) +ĥ!(ŷ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/transcription.jl b/src/estimator/mhe/transcription.jl index c65af2e4d..15f3d2b4f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1499,7 +1499,7 @@ function predict_mhe!( ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] v̂ = @views V̂[(1 + nym*(j-1)):(nym*j)] y0m = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] - ĥ!(ŷ0, estim, model, x̂0, a0, d0) + ĥ_dae!(ŷ0, estim, model, x̂0, a0, d0) ŷ0m = @views ŷ0[estim.i_ym] if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] @@ -1727,7 +1727,7 @@ function con_nonlinprogeq_mhe!( 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!(k̇0, q0arr, model, x̂darr, a0arr, û0arr, d0arr) + fq_dae!(k̇0, q0arr, model, x̂darr, a0arr, û0arr, d0arr) end @threadsif f_threads for j=1:Nk if j < 2 @@ -1751,17 +1751,17 @@ function con_nonlinprogeq_mhe!( 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!(k̇1, q1, model, x̂d_Z̃, ā, û0, d0) + 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 end if h < 1 - fq!(k̇2, q2, model, x̂dnext, a0, û0, d0next) + 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!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) + fq_dae!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) end ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd end diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index d72909740..21e1ad615 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -785,16 +785,6 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) return nothing end -""" - fq!(ẋ0, q0, model, x0, a0, u0, d0) - -Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). -""" -function fq!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) - return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) -end -fq!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0, model.p) - function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) model.beq .= @. -model.Fs From 696c0fdd7cfa2a8aeab9c196d81083cc5f9fab0f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 11:01:36 -0400 Subject: [PATCH 26/35] test: `getinfo!` and unmeasured disturbance est. for MHE+DAE+TC --- src/estimator/execute.jl | 6 +++--- src/estimator/mhe/execute.jl | 2 +- test/2_test_state_estim.jl | 41 +++++++++++++++++++++++++++--------- 3 files changed, 35 insertions(+), 14 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 18231a06e..ce21c5f21 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -156,7 +156,7 @@ fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0 Similar than [`ĥ!`](@ref) but with a algebraic variable `a0` for [`NonLinModelDAE`](@ref) . """ -function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) +function ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] model.h!(ŷ0, xd, a0, d0, model.p) @@ -165,11 +165,11 @@ function ĥ!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) end """ - ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x0, _ , d0) + ĥ_dae!(ŷ0, estim::StateEstimator, model::SimModelODE, x0, _ , d0) Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. """ -ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _, d0) = ĥ!(ŷ0, estim, model, x̂0, d0) +ĥ_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/execute.jl b/src/estimator/mhe/execute.jl index fb38a788f..008d4a2e6 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -145,7 +145,7 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3) """ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] - nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, get_nk̄(model, estim.transcription) nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ Z̃ = estim.Z̃ info = Dict{Symbol, Any}() diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 887f8d9e6..4034441ca 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1388,28 +1388,49 @@ end using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer import ForwardDiff - function fq!(ẋ, res, x, a, u, _ , p) - ẋ[] = -p[] * (x[] - 0.2 * u[]) + 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, _ , _ ) - y[] = 2 * x[] + a[] + function h!(y, x, a, d, _ ) + y[] = 2 * x[] + a[] + 0.1 * d[] end - p=[0.5] - dae = NonLinModelDAE(fq!, h!, 1.0, 1, 1, 1, 1; p) + Ts, p = 100.0, [0.01] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) mhe = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) - preparestate!(mhe, [0.0]) - x̂ = updatestate!(mhe, [0.0], [0.0]) + preparestate!(mhe, [0.0], [0.0]) + x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) @test x̂ ≈ zeros(mhe.nx̂) atol=1e-8 @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-8 + + preparestate!(mhe, [0], [0]) + info = getinfo(mhe) + @test info[:x̂] ≈ x̂ atol=1e-8 + @test info[:Ŷ][end-1:end] ≈ [0] atol=1e-8 + for i in 1:40 + preparestate!(mhe, [0], [0]) + updatestate!(mhe, [3.0], [0], [0]) + end + preparestate!(mhe, [0], [0]) + @test mhe([0]) ≈ [0] atol=1e-3 + for i in 1:40 + preparestate!(mhe, [7.0], [0]) + updatestate!(mhe, [0], [7.0], [0]) + end + preparestate!(mhe, [7.0], [0]) + @test mhe([0]) ≈ [7.0] atol=1e-3 + + + + transcription = TrapezoidalCollocation(1) mhe2 = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) - preparestate!(mhe2, [0.0]) - x̂ = updatestate!(mhe2, [0.0], [0.0]) + preparestate!(mhe2, [0.0], [0.0]) + x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-8 @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-8 end From d799b2d3da642a9ec8f712bd2c7b5a5b62325e0a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 11:16:45 -0400 Subject: [PATCH 27/35] test: debug tests --- test/2_test_state_estim.jl | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 4034441ca..ffd1e8369 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1399,18 +1399,16 @@ end Ts, p = 100.0, [0.01] dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) - transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) + transcription = TrapezoidalCollocation() mhe = MovingHorizonEstimator(dae; He=3, 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-8 @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-8 - - preparestate!(mhe, [0], [0]) info = getinfo(mhe) @test info[:x̂] ≈ x̂ atol=1e-8 - @test info[:Ŷ][end-1:end] ≈ [0] atol=1e-8 + @test info[:Ŷ][end] ≈ 0 atol=1e-8 for i in 1:40 preparestate!(mhe, [0], [0]) updatestate!(mhe, [3.0], [0], [0]) @@ -1424,11 +1422,8 @@ end preparestate!(mhe, [7.0], [0]) @test mhe([0]) ≈ [7.0] atol=1e-3 - - - - transcription = TrapezoidalCollocation(1) - mhe2 = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + mhe2 = MovingHorizonEstimator(dae; He=3, transcription) preparestate!(mhe2, [0.0], [0.0]) x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-8 From 5704e6a45f77f1f0aebb3183ab94e8a7f2a5e7c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 13:00:30 -0400 Subject: [PATCH 28/35] debug: Julia 1.10 needs `::Any` for the unused arguments here --- src/estimator/mhe/transcription.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 15f3d2b4f..2b7282e7f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,7 +1,7 @@ "Get the number of elements in the optimization decision vector `Z`" -get_nZ_mhe(::SingleShooting, He, nx̂ , _ , nŵ, na=0) = nx̂ + nŵ*He -get_nZ_mhe(::MultipleShooting, He, nx̂ , _ , nŵ, na=0) = nx̂ + nx̂*He + nŵ*He -function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, _ , nŵ, na=0) +get_nZ_mhe(::SingleShooting, He, nx̂ , ::Any, nŵ, ::Any=0) = nx̂ + nŵ*He +get_nZ_mhe(::MultipleShooting, He, nx̂ , ::Any, nŵ, ::Any=0) = nx̂ + nx̂*He + nŵ*He +function get_nZ_mhe(::TrapezoidalCollocation, He, nx̂, ::Any, nŵ, na=0) return nx̂ + nx̂*He + na + na*He + na*He + nŵ*He end function get_nZ_mhe(transcription::OrthogonalCollocation, He, nx̂, nk̄, nŵ, na=0) From 49013a030c27e2047d3aa93e14a8a76c0efbb1fe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 13:15:40 -0400 Subject: [PATCH 29/35] wip: warm-starting MHE with model values --- src/estimator/mhe/construct.jl | 1 + src/estimator/mhe/execute.jl | 29 ++++++++++++++++++++++++++++- src/model/linmodel.jl | 3 +++ src/model/nonlinmodel.jl | 3 +++ src/model/nonlinmodeldae.jl | 4 ++++ 5 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2ef719b2e..eed40312e 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -247,6 +247,7 @@ struct MovingHorizonEstimator{ direct, prepared, buffer ) + reset_warmstart!(estim, transcription) init_optimization!(estim, model, optim) return estim end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 008d4a2e6..6c70db587 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -1,9 +1,35 @@ +""" + reset_warmstart!(estim::MovingHorizonEstimator, transcription::CollocationMethod) + +Reset warm-starting values `estim.Z̃` at values stored in `estim.model` +""" +function reset_warmstart!( + estim::MovingHorizonEstimator, transcription::TrapezoidalCollocation +) + model = estim.model + as_0 = get_as_0(model) + nx, no, na = model.nx, transcription.no, get_na(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + #nk̄ = nx*no + #model.a0 .= a0s + #model.Z[1:nx] .= x0s + #model.Z[(nx+1):(nx+na)] .= a0s + #k̄_Z = @views model.Z[(nx+na+1):(nx+na+nk̄)] + #repeat!(k̄_Z, x0s, no) + #ā_Z = @views model.Z[(nx+na+nk̄+1):end] + #repeat!(ā_Z, a0s, no) + return nothing +end + + + "Reset the data windows and time-varying variables for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model nu, ny, nd = model.nu, model.ny, model.nd uop, yop, dop = model.uop, model.yop, model.dop - estim.Z̃ .= 0 estim.Y0m .= NaN estim.Yem .= NaN estim.U0 .= NaN @@ -33,6 +59,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.P̂arr_old .= estim.cov.P̂_0 invert_cov!(estim, estim.covestim) estim.x̂0arr_old .= 0 + reset_warmstart!(estim, estim.transcription) return nothing end diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 087d71d37..d0d5c1f66 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -22,6 +22,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} yname::Vector{String} dname::Vector{String} xname::Vector{String} + xs_0::Vector{NT} buffer::SimModelBuffer{NT} function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real} A, Bu = to_mat(A, 1, 1), to_mat(Bu, 1, 1) @@ -44,6 +45,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + xs_0 = zeros(NT, nx) uname = ["\$u_{$i}\$" for i in 1:nu] yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] @@ -60,6 +62,7 @@ struct LinModel{NT<:Real} <: SimModelODE{NT} nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, + xs_0, buffer ) end diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 9e7072980..e9f6b2ab7 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -44,6 +44,7 @@ struct NonLinModel{ yname::Vector{String} dname::Vector{String} xname::Vector{String} + xs_0::Vector{NT} jacobian::JB linfunc!::LF buffer::SimModelBuffer{NT} @@ -65,6 +66,7 @@ struct NonLinModel{ dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + xs_0 = zeros(NT, nx) uname = ["\$u_{$i}\$" for i in 1:nu] yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] @@ -83,6 +85,7 @@ struct NonLinModel{ nu, nx, ny, nd, nk̄, uop, yop, dop, xop, fop, uname, yname, dname, xname, + xs_0, jacobian, linfunc!, buffer ) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 21e1ad615..04e091481 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -409,6 +409,10 @@ get_nā(model::NonLinModelDAE, transcription::OrthogonalCollocation) = model.na get_nā(model::NonLinModelDAE, ::TrapezoidalCollocation) = model.na get_nā(::SimModel, ::TranscriptionMethod) = 0 +"Get the warm-starting value for the algebraic variable `as_0`." +get_as_0(model::NonLinModelDAE) = model.as_0 +get_as_0(model::SimModel) = model.buffer.a + "Get the number of elements in the optimization decision vector `Z` for DAE solving." function get_nZ_dae(transcription::OrthogonalCollocation, nx, na) return nx + transcription.no*nx + na + transcription.no*na From 08a1157e1992308fdde9b91fd2915f4f385f265b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 15:59:59 -0400 Subject: [PATCH 30/35] doc: debug cross-ref --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 2b7282e7f..7891f2fee 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1677,7 +1677,7 @@ state defects are computed with: for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the deterministic state and process noise estimates, respectively, extracted from the decision variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continuous-time -function [`fq!`](@ref) and: +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) \\ From 55ef4ce534fc20a25c6898e89ee971db878cc83d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 16:27:37 -0400 Subject: [PATCH 31/35] added: warm-starting MHE with model values --- src/estimator/mhe/execute.jl | 78 +++++++++++++++++++++++------------- 1 file changed, 51 insertions(+), 27 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6c70db587..fa8f11782 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -1,30 +1,3 @@ -""" - reset_warmstart!(estim::MovingHorizonEstimator, transcription::CollocationMethod) - -Reset warm-starting values `estim.Z̃` at values stored in `estim.model` -""" -function reset_warmstart!( - estim::MovingHorizonEstimator, transcription::TrapezoidalCollocation -) - model = estim.model - as_0 = get_as_0(model) - nx, no, na = model.nx, transcription.no, get_na(model) - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - a0s = as_0 - #nk̄ = nx*no - #model.a0 .= a0s - #model.Z[1:nx] .= x0s - #model.Z[(nx+1):(nx+na)] .= a0s - #k̄_Z = @views model.Z[(nx+na+1):(nx+na+nk̄)] - #repeat!(k̄_Z, x0s, no) - #ā_Z = @views model.Z[(nx+na+nk̄+1):end] - #repeat!(ā_Z, a0s, no) - return nothing -end - - - "Reset the data windows and time-varying variables for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model @@ -63,6 +36,57 @@ 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ε = model.nx, estim.nx̂, estim.nε + nx̃ = nε + nx̂ + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + for i in 1:estim.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ε, na = model.nx, estim.nx̂, estim.nε, 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 i in 1:estim.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) + 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) From 7990fc20092bde7053de520cd5f84ab1343cfc87 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 16:39:51 -0400 Subject: [PATCH 32/35] debug: warm-starting MHE with model values --- src/estimator/mhe/execute.jl | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index fa8f11782..123c5b810 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -43,13 +43,13 @@ Reset warm-starting values `estim.Z̃` at values stored in `estim.model` """ function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) model = estim.model - nx, nx̂, nε = model.nx, estim.nx̂, estim.nε + 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 i in 1:estim.He + for j in 1:He estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s end return nothing @@ -58,7 +58,7 @@ function reset_warmstart!( estim::MovingHorizonEstimator, ::TrapezoidalCollocation ) model = estim.model - nx, nx̂, nε, na = model.nx, estim.nx̂, estim.nε, get_na(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 @@ -67,14 +67,32 @@ function reset_warmstart!( estim.Z̃ .= 0 estim.Z̃[nε+1:nε+nx] = x0s estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s - for i in 1:estim.He + 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) +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) From 3b08d20914681857c689bcc558b44b86ba401a9b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 17:18:03 -0400 Subject: [PATCH 33/35] debug: `d0next=d0` at time `k` with `direct=false` for MHE This is a special case for all `CollocationMethod`s. The measured disturbance of the next time step is not available for `MovingHorizonEstimator`. --- src/estimator/mhe/execute.jl | 2 +- src/estimator/mhe/transcription.jl | 14 +++++++++++--- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 123c5b810..d57d03eaa 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -923,7 +923,7 @@ Objective function of the MHE when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ, Z̃) - Nk = estim.Nk[] + Nk = estim.Nk[] invP̄ = estim.cov.invP̄ invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 7891f2fee..8ccdad902 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1746,8 +1746,12 @@ function con_nonlinprogeq_mhe!( 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] - d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2nx] + if estim.direct || j ≥ Nk + d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + else + d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + end if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) @@ -1832,7 +1836,11 @@ function con_nonlinprogeq_mhe!( k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] - d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + if estim.direct || j ≥ Nk + d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + else + d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + end # ----------------- collocation constraint defects ----------------------------- Δk = k̄ for i=1:no From 46b935793ac5928b33045e4798b07e9f199f792b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 17:59:55 -0400 Subject: [PATCH 34/35] test: verify warm-start values for MHE and DAE --- test/1_test_sim_model.jl | 2 +- test/2_test_state_estim.jl | 26 ++++++++++++++++---------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 26dae5bda..559792804 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -566,7 +566,7 @@ end @test evaloutput(dae) ≈ zeros(1) atol=1e-6 transcription = TrapezoidalCollocation() - dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, as_0, xs_0) + dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, xs_0, as_0) @test updatestate!(dae2, u) ≈ zeros(1) atol=1e-6 @test updatestate!(dae2, u, d) ≈ zeros(1) atol=1e-6 @test dae2.x0 ≈ zeros(1) atol=1e-6 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index ffd1e8369..f4100d87c 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1397,18 +1397,19 @@ end 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) + as_0, xs_0 = [-1.0], [1.0] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) transcription = TrapezoidalCollocation() - mhe = MovingHorizonEstimator(dae; He=3, transcription, hessian=true) + 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-8 - @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-8 + @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-8 - @test info[:Ŷ][end] ≈ 0 atol=1e-8 + @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]) @@ -1421,13 +1422,18 @@ end end preparestate!(mhe, [7.0], [0]) @test mhe([0]) ≈ [7.0] atol=1e-3 - + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) - mhe2 = MovingHorizonEstimator(dae; He=3, transcription) + 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-8 - @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-8 + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 + initstate!(mhe2, [0], [0], [0]) + @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 end @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin From d826ab1f554545da63debad2d88762199a77f33d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Sep 2026 18:12:24 -0400 Subject: [PATCH 35/35] doc: default `covestim` for `NonLinModelDAE` --- src/estimator/mhe/construct.jl | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index eed40312e..914ed89f9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -256,9 +256,10 @@ end @doc raw""" MovingHorizonEstimator(model::SimModel; ) -Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). +Construct a moving horizon estimator (MHE) based on `model`. -It can handle constraints on the estimates. Additionally, `model` is not linearized like the +It supports ([`LinModel`](@ref), [`NonLinModel`](@ref)), [`NonLinModelDAE`](@ref)) and +constraints on the estimates. Additionally, `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the probability distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, however, since it minimizes the following objective function at each discrete time ``k``: @@ -501,7 +502,8 @@ MovingHorizonEstimator estimator with a sample time Ts = 5.0 s: the `f` and `h` functions must be compatible with this feature. See the [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref) - estimates the arrival covariance by default. + estimates the arrival covariance by default for [`NonLinModel`](@ref). The default is + a [`SteadyKalmanFilter`](@ref) for [`NonLinModelDAE`](@ref). Two exceptions about AD: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument and the `hessian=true` option default to this sparse backend: