From 6c8c144d88e005b5cec24606a843c37a318d6855 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 4 Aug 2026 13:08:48 -0400 Subject: [PATCH 1/8] wip: stochastic defects as lin. constraints in MHE --- src/estimator/mhe/construct.jl | 2 +- src/estimator/mhe/transcription.jl | 56 ++++++++++++++++++++++++++---- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index bdb17ff27..5c0b71c86 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -183,7 +183,7 @@ struct MovingHorizonEstimator{ model, transcription, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) ES, GS, JS, BS = init_defectmat_mhe( - model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, direct + model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, As, direct ) # dummy values (updated just before optimization): F, fx̄ = zeros(NT, nym*He), zeros(NT, nx̂) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 7628bd6ee..588afd5f5 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -391,7 +391,7 @@ end @doc raw""" init_defectmat_mhe( model::LinModel, transcription::MultipleShooting, - He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, _ , direct ) -> ES, GS, JS, BS Init the matrices for computing the defects over the predicted states. @@ -448,7 +448,7 @@ matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help sectio operator `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). """ function init_defectmat_mhe( - model::LinModel{NT}, ::MultipleShooting, He, Â, B̂u, B̂d, x̂op, f̂op, direct + model::LinModel{NT}, ::MultipleShooting, He, Â, B̂u, B̂d, x̂op, f̂op, _ , direct ) where {NT<:Real} nd = model.nd nx̂ = size(Â, 2) @@ -472,14 +472,56 @@ function init_defectmat_mhe( return ES, GS, JS, BS end -"Return empty matrices for [`SimModel`](@ref) (will change in the future)." +@doc raw""" + init_defectmat_mhe( + model::SimModel{NT}, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ +) where {NT<:Real} + +Init the matrices for computing the defects of the stochastic states only. + +The documentation of [`init_estimstoch`](@ref) shows that the stochastic model of the +unmeasured disturbances is linear and discrete-time. The defect of the stochastic states +over ``H_p`` is therefore computed by: +```math + \mathbf{Ŝ} = \mathbf{E_S Z} +``` +The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + Using the stochastic matrix ``\mathbf{A_s}`` of [`init_estimstoch`](@ref)), the defect + matrices is computed with: + ```math + \begin{aligned} + \mathbf{E_S^x̂} &= \begin{bmatrix} + \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \mathbf{0} & \mathbf{0} & \ddots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_S^ŵ} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^ŵ} \end{bmatrix} + \end{aligned} + ``` +""" function init_defectmat_mhe( - model::SimModel{NT}, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , _ + model::SimModel{NT}, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ ) where {NT<:Real} - nx̂ = size(Â, 2) + nx̂, nxs = size(Â, 2), size(As, 2) + nx = nx̂ - nxs nŵ = nx̂ - # TODO: handle stochastic defects as linear equality constraints - return init_defectmat_mhe_empty(model, transcription, He, nx̂, nŵ) + ESx̂ = [zeros(NT, nxs*He, nx̂) repeatdiag([zeros(NT, nxs, nx) -I], He)] + for j=1:He + iRow = (1:nxs) .+ nxs*(j-1) + iCol = (nx+1:nx̂) .+ nxs*(j-1) + ESx̂[iRow, iCol] = As + end + ESŵ = zeros(NT, nxs*He, nŵ*He) + ES = [ESx̂ ESŵ] + display(ES) + GS = zeros(NT, nxs*He, model.nu*He) + JS = zeros(NT, nxs*He, model.nd*(He+1)) + BS = zeros(NT, nxs*He) + return ES, GS, JS, BS end "Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModel` (N/A)." From c1fb54d62c50271a9f5e1e4ad0df365a9c74e9eb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 10:39:02 -0400 Subject: [PATCH 2/8] wip: idem --- src/controller/execute.jl | 32 ------------------------------ src/controller/transcription.jl | 6 +++--- src/estimator/execute.jl | 31 +++++++++++++++++++++++++++++ src/estimator/internal_model.jl | 2 ++ src/estimator/mhe/transcription.jl | 15 +++++++------- 5 files changed, 43 insertions(+), 43 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 94f7fbc78..be072e29b 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -326,38 +326,6 @@ end "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) -""" - disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) -> nothing - -Fill disturbed inputs of the augmented model `Û0` in-place with stochastic states in `X̂0` - -Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+H_p-1``. The -predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+H_p-1`` (the current one -is stored in `estim.x̂0`). - -This function is needed for the collocation methods that directly call the state derivative -function `estim.model.f!` with the manipulated inputs augmented with the estimated -disturbances at model input (see [`init_estimstoch`](@ref)). This is also needed for -[`MultipleShooting`](@ref) since it calls the discrete-time update function of the -deterministic model [`f!`](@ref) to treat the stochastic defects as linear equality -constraints. Lastly, it's also necessary to prefill the `Û0` vector before anything else -since both `û0` and `û0next` are needed at each stage with hold order `h>0`, thus potential -race conditions with multi-threading. -""" -function disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) - nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂ - Cs_u = estim.Cs_u - Û0 .= U0 - for j=0:mpc.Hp-1 - xs = @views j < 1 ? estim.x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] - û0 = @views Û0[(1+nu*j):(nu*(j+1))] - mul!(û0, Cs_u, xs, 1, 1) # û0 = u0 + Cs_u*xs - end - return nothing -end -"No input disturbances for [`InternalModel`](@ref), hence do `Û0 .= U0`." -disturbedinput!(Û0, ::PredictiveController, ::InternalModel, U0, _) = (Û0 .= U0; nothing) - @doc raw""" linconstraint_custom!(mpc::PredictiveController, model::SimModel) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8f0413add..0e206031c 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1347,7 +1347,7 @@ function con_nonlinprogeq!( f_threads = transcription.f_threads D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) + Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] @@ -1406,7 +1406,7 @@ function con_nonlinprogeq!( nk = get_nk_mpc(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) + Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] @@ -1502,7 +1502,7 @@ function con_nonlinprogeq!( D̂0 = mpc.D̂0 X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] D̂temp = mpc.buffer.D̂ - disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) + Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 5c08477ec..62d6456af 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -141,6 +141,37 @@ function ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0) return nothing end +""" + disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 + +Fill disturbed inputs of the augmented model `Û0` in-place with the stochastic states in `X̂0`. + +Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+N-1``. The +predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+N``, and `x̂0` is the +value at ``k``. + +This function is needed for the collocation methods that directly call the state derivative +function `estim.model.f!` with the manipulated inputs augmented with the estimated +disturbances at model input (see [`init_estimstoch`](@ref)). This is also needed for +[`MultipleShooting`](@ref) since it calls the discrete-time update function of the +deterministic model [`f!`](@ref) to treat the stochastic defects as linear equality +constraints. Lastly, it's also necessary to prefill the `Û0` vector before anything else +since both `û0` and `û0next` are needed at each stage with hold order `h>0`, thus potential +race conditions with multi-threading. +""" +function disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) + nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂ + N = length(Û0) ÷ nu + Cs_u = estim.Cs_u + Û0 .= U0 + for j=0:N-1 + xs = @views j < 1 ? x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] + û0 = @views Û0[(1+nu*j):(nu*(j+1))] + mul!(û0, Cs_u, xs, 1, 1) # û0 = u0 + Cs_u*xs + end + return Û0 +end + @doc raw""" initstate!(estim::StateEstimator, u, ym, d=[]) -> x̂ diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index e50ad5b46..098dff1b0 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -191,6 +191,8 @@ function ĥ!(ŷ0, ::InternalModel, model::NonLinModel, x̂0, d0) return h!(ŷ0, model, x̂0, d0, model.p) end +"No input disturbances for [`InternalModel`](@ref), return `Û0 .= U0`." +disturbedinput!(Û0, ::InternalModel, _ , _ , U0) = (Û0 .= U0) @doc raw""" init_internalmodel(As, Bs, Cs, Ds) -> Âs, B̂s diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 588afd5f5..ef94f6b31 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -504,7 +504,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ + model::SimModel{NT}, ::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs @@ -517,7 +517,6 @@ function init_defectmat_mhe( end ESŵ = zeros(NT, nxs*He, nŵ*He) ES = [ESx̂ ESŵ] - display(ES) GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -1334,7 +1333,7 @@ function con_nonlinprogeq_mhe!( estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, Ŵ, Z̃ ) - nu, nd, nk = model.nu, model.nd, model.nk + nu, nx, nd, nk = model.nu, model.nx, model.nd, model.nk nε, nx̂, He = estim.nε, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads @@ -1342,23 +1341,23 @@ function con_nonlinprogeq_mhe!( nx̃ = nε + nx̂ p = estim.direct ? 0 : 1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) @threadsif f_threads for j=1:Nk if j < 2 - x̂0 = @views x̂0arr[1:nx̂] + x̂d_Z̃ = @views x̂0arr[1:nx] else - x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] + x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end - u0 = @views estim.U0[(1 + nu*(j-1)):(nu*j)] d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] k = @views K[(1 + nk*(j-1)):(nk*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] + sdnext = @views geq[(1 + nx*(j-1)):(nx*j)] f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) x̂0next .+= ŵ - ŝnext .= @. x̂0next - x̂0next_Z̃ + sdnext .= @. x̂dnext - x̂dnext_Z̃ end Nk < He && (geq[nx̂*Nk+1:end] .= 0) return geq From 2291ddd5c51210df919599611d64d51c691dd89c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 13:48:37 -0400 Subject: [PATCH 3/8] added: stochastic defect treat as linear constraints in MHE --- src/estimator/mhe/execute.jl | 52 +++++++- src/estimator/mhe/transcription.jl | 188 ++++++++++++++--------------- 2 files changed, 142 insertions(+), 98 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6242e86fc..b0e0ef009 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -17,6 +17,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.q̃ .= 0 estim.r .= 0 estim.con.FX̂ .= 0 + estim.con.FS .= 0 if estim.direct # add y0m(-1) to the extended data window (custom NL constraints): estim.Yem[1:ny] .= y0m .+ @views yop[estim.i_ym] @@ -422,7 +423,7 @@ function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT< nYm, nZ = estim.nym*Nk, get_nZ_mhe(estim.transcription, Nk, nx̂, nŵ) # --- truncate vectors and matrices if Nk < He --- U0, D0, Y0m = trunc_windows(estim) - Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim, estim.transcription) + Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim) invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He) # --- update F and fx̄ vectors for MHE predictions --- @@ -652,6 +653,55 @@ function trunc_windows(estim::MovingHorizonEstimator) return U0, D0, Y0m end +"Truncate or extract subparts of the prediction matrices if `Nk < He`" +function trunc_predmat(estim::MovingHorizonEstimator) + model, transcription = estim.model, estim.transcription + nŵ, nym, nε, Nk = estim.nx̂, estim.nym, estim.nε, estim.Nk[] + nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1) + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + i_Z̃_Nk = get_i_Z̃_Nk(estim, transcription) + i_Z_Nk = nε < 1 ? i_Z̃_Nk : i_Z̃_Nk[begin+1:end] .- 1 + Ẽ = estim.Ẽ[1:nYm, i_Z̃_Nk] + G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm] + ẽx̄ = estim.ẽx̄[:, i_Z̃_Nk] + Tŵ = estim.Tŵ[1:nŴ, i_Z_Nk] + F = @views estim.F[1:nYm] # views here since they will store results + H̃_data = @views estim.H̃.data[i_Z̃_Nk, i_Z̃_Nk] + H̃ = @views estim.H̃[i_Z̃_Nk, i_Z̃_Nk] + q̃ = @views estim.q̃[i_Z̃_Nk] + Z̃var = @views estim.optim[:Z̃var][i_Z̃_Nk] + else + Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B + ẽx̄, Tŵ = estim.ẽx̄, estim.Tŵ + H̃, H̃_data, q̃ = estim.H̃, estim.H̃.data, estim.q̃ + Z̃var = estim.optim[:Z̃var] + end + return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var +end + +"Extract subparts of the defect matrices if `Nk < He`" +function trunc_defectmat(estim::MovingHorizonEstimator) + model, transcription, con = estim.model, estim.transcription, estim.con + FS = con.FS + nx̂, Nk = estim.nx̂, estim.Nk[] + nU, nX̂, nD = model.nu*Nk, nx̂*Nk, model.nd*(Nk+1) + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + i_Z̃_Nk = get_i_Z̃_Nk(estim, transcription) + ẼS = con.ẼS[1:nX̂, i_Z̃_Nk] + GS, JS, BS = con.GS[1:nX̂, 1:nU], con.JS[1:nX̂, 1:nD], con.BS[1:nX̂] + FS = @views con.FS[1:nX̂] # views here since they will store results + Aeq = @views con.Aeq[1:nX̂, i_Z̃_Nk] + beq = @views con.beq[1:nX̂] + Z̃var = @views estim.optim[:Z̃var][i_Z̃_Nk] + else + ẼS, FS, GS, JS, BS = con.ẼS, con.FS, con.GS, con.JS, con.BS + Aeq = con.Aeq + beq = con.beq + Z̃var = estim.optim[:Z̃var] + end + return ẼS, FS, GS, JS, BS, Aeq, beq, Z̃var +end + "Truncate the inverse covariance `invA_He` to the window size `Nk` if `Nk < He`." function trunc_cov(invA_He::Hermitian{<:Real, <:AbstractMatrix}, n, Nk, He) if Nk < He diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ef94f6b31..f76d8c7dd 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -2,6 +2,21 @@ get_nZ_mhe(::SingleShooting, He, nx̂, nŵ) = nx̂ + nŵ*He get_nZ_mhe(::TranscriptionMethod, He, nx̂, nŵ) = nx̂ + nx̂*He + nŵ*He +"Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." +function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) + nx̂, nŵ, nε, Nk = estim.nx̂, estim.nx̂, estim.nε, estim.Nk[] + nŴ, nX̂ = nŵ*Nk, nx̂*Nk + nx̂_nX̂ = nx̂ + nX̂ + nx̂_nX̂_He = nx̂ + nx̂*estim.He + i_Z̃_NK = [(1):(nε + nx̂_nX̂); (nε + nx̂_nX̂_He + 1):(nε + nx̂_nX̂_He + nŴ)] + return i_Z̃_NK +end +function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::SingleShooting) + nŵ, Nk = estim.nx̂, estim.Nk[] + nx̃ = estim.nε + estim.nx̂ + return (1):(nx̃ + nŵ*Nk) +end + @doc raw""" init_predmat_mhe( model::LinModel, transcription::SingleShooting, @@ -498,7 +513,11 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \vdots & \vdots & \mathbf{0} & \mathbf{0} & \ddots & \vdots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ - \mathbf{E_S^ŵ} &= \mathbf{0} \\ + \mathbf{E_S^ŵ} &= \begin{bmatrix} + \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \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} \end{aligned} ``` @@ -509,13 +528,14 @@ function init_defectmat_mhe( nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs nŵ = nx̂ + nw = nŵ - nxs ESx̂ = [zeros(NT, nxs*He, nx̂) repeatdiag([zeros(NT, nxs, nx) -I], He)] for j=1:He iRow = (1:nxs) .+ nxs*(j-1) - iCol = (nx+1:nx̂) .+ nxs*(j-1) + iCol = (nx+1:nx̂) .+ nx̂*(j-1) ESx̂[iRow, iCol] = As end - ESŵ = zeros(NT, nxs*He, nŵ*He) + ESŵ = repeatdiag([zeros(NT, nxs, nw) I], He) ES = [ESx̂ ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) @@ -699,90 +719,12 @@ function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModel, ::TranscriptionMethod return i_Ŵmin, i_Ŵmax end -"For [`SingleShooting`](@ref), truncate the end of prediction matrices if `Nk < He`" -function trunc_predmat(estim::MovingHorizonEstimator, transcription::SingleShooting) - model = estim.model - nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1) - nZ = get_nZ_mhe(transcription, Nk, nx̂, nŵ) - nZ̃ = nε + nZ - if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! - Ẽ = estim.Ẽ[1:nYm, 1:nZ̃] - G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm] - ẽx̄ = estim.ẽx̄[:, 1:nZ̃] - Tŵ = estim.Tŵ[1:nŴ, 1:nZ] - F = @views estim.F[1:nYm] # views here since they will store results - H̃_data = @views estim.H̃.data[1:nZ̃, 1:nZ̃] - H̃ = @views estim.H̃[1:nZ̃, 1:nZ̃] - q̃ = @views estim.q̃[1:nZ̃] - Z̃var = @views estim.optim[:Z̃var][1:nZ̃] - else - Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B - ẽx̄, Tŵ = estim.ẽx̄, estim.Tŵ - H̃, H̃_data, q̃ = estim.H̃, estim.H̃.data, estim.q̃ - Z̃var = estim.optim[:Z̃var] - end - return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var -end - -"For [`MultipleShooting`](@ref), extract subparts of the prediction matrices if `Nk < He`." -function trunc_predmat(estim::MovingHorizonEstimator, ::MultipleShooting) - model = estim.model - nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1) - nx̂_nX̂ = nx̂ + nx̂*Nk - nx̂_nX̂_He = nx̂ + nx̂*estim.He - if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! - i_Z̃_He = [(1):(nε + nx̂_nX̂); (nε + nx̂_nX̂_He + 1):(nε + nx̂_nX̂_He + nŴ)] - i_Z_He = [(1):(nx̂_nX̂); (nx̂_nX̂_He + 1):(nx̂_nX̂_He + nŴ)] - Ẽ = estim.Ẽ[1:nYm, i_Z̃_He] - G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm] - ẽx̄ = estim.ẽx̄[:, i_Z̃_He] - Tŵ = estim.Tŵ[1:nŴ, i_Z_He] - F = @views estim.F[1:nYm] # views here since they will store results - H̃_data = @views estim.H̃.data[i_Z̃_He, i_Z̃_He] - H̃ = @views estim.H̃[i_Z̃_He, i_Z̃_He] - q̃ = @views estim.q̃[i_Z̃_He] - Z̃var = @views estim.optim[:Z̃var][i_Z̃_He] - else - Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B - ẽx̄, Tŵ = estim.ẽx̄, estim.Tŵ - H̃, H̃_data, q̃ = estim.H̃, estim.H̃.data, estim.q̃ - Z̃var = estim.optim[:Z̃var] - end - return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var -end - -function trunc_defectmat(estim::MovingHorizonEstimator) - model, con = estim.model, estim.con - FS = con.FS - nx̂, nŵ, nε, Nk = estim.nx̂, estim.nx̂, estim.nε, estim.Nk[] - nU, nŴ, nX̂, nD = model.nu*Nk, nŵ*Nk, nx̂*Nk, model.nd*(Nk+1) - nx̂_nX̂ = nx̂ + nX̂ - nx̂_nX̂_He = nx̂ + nx̂*estim.He - if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! - i_Z̃_He = [(1):(nε + nx̂_nX̂); (nε + nx̂_nX̂_He + 1):(nε + nx̂_nX̂_He + nŴ)] - ẼS = con.ẼS[1:nX̂, i_Z̃_He] - GS, JS, BS = con.GS[1:nX̂, 1:nU], con.JS[1:nX̂, 1:nD], con.BS[1:nX̂] - FS = @views con.FS[1:nX̂] # views here since they will store results - Aeq = @views con.Aeq[1:nX̂, i_Z̃_He] - beq = @views con.beq[1:nX̂] - Z̃var = @views estim.optim[:Z̃var][i_Z̃_He] - else - ẼS, FS, GS, JS, BS = con.ẼS, con.FS, con.GS, con.JS, con.BS - Aeq = con.Aeq - beq = con.beq - Z̃var = estim.optim[:Z̃var] - end - return ẼS, FS, GS, JS, BS, Aeq, beq, Z̃var -end - @doc raw""" linconstraint!( estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod ) -Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE. +Set `b` vector for the linear inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE. Also init ``\mathbf{F_X̂ = G_X̂ U_0 + J_X̂ D_0 + B_X̂}`` vector for the state constraints, see [`init_predmat_mhe`](@ref). @@ -861,12 +803,22 @@ function linconstraint!( return nothing end -""" +@doc raw""" linconstrainteq!( - estim::MovingHorizonEstimator, model::LinModel, ::TranscriptionMethod + estim::MovingHorizonEstimator, model::LinModel, ::MultipleShooting ) -TBW +Set `Aeq` matrix and `beq` vector for the linear equality constraints of MHE. + +They are defined by ``\mathbf{A_{eq} Z̃ ≤ b_{eq}}``. The method also inits +``\mathbf{F_S = G_S U_0 + J_S D_0 + B_S}`` vector for the state defect constraints, see +[`init_defectmat_mhe`](@ref). + +The number of linear equality constraints grows when ``N_k < H_e``. A temporary +`:linconstrainteq_temp` structure is overwritten at each time +step during this period. A permanent `:linconstrainteq` structure is created when +``N_k = H_e`` is reached. From this point on, only the the ``beq`` vector is updated +with `JuMP.set_normalized_rhs` function for efficiency. """ function linconstrainteq!( estim::MovingHorizonEstimator, model::LinModel, ::MultipleShooting @@ -881,7 +833,7 @@ function linconstrainteq!( end beq .= @. -FS Aeq .= @. ẼS - if haskey(optim, :linconstrainteq_temp) # temporary since only used once when Nk < He + if haskey(optim, :linconstrainteq_temp) JuMP.delete(optim, optim[:linconstrainteq_temp]) JuMP.unregister(optim, :linconstrainteq_temp) end @@ -900,8 +852,50 @@ function linconstrainteq!( end return nothing end -function linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod) - # TODO: handle stochastic defects as linear equality constraints + +""" + linconstrainteq!( + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod +) + +By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@refs). + +The linear equality constraints include the stochastic defects only, and the `beq` +vector is only zeros for this specific case. See [`init_defectmat_mhe`](@ref) for the +equations. +""" +function linconstrainteq!( + 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 + nŜ = nŝ*Nk + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + i_Z̃_Nk = get_i_Z̃_Nk(estim, transcription) + ẼS = con.ẼS[1:nŜ, i_Z̃_Nk] + Aeq, beq = @views con.Aeq[1:nŜ, i_Z̃_Nk], con.beq[1:nŜ] + Z̃var = @views optim[:Z̃var][i_Z̃_Nk] + else + ẼS = con.ẼS + Aeq, beq = con.Aeq, con.beq + Z̃var = optim[:Z̃var] + end + Aeq .= ẼS + if haskey(optim, :linconstrainteq_temp) + JuMP.delete(optim, optim[:linconstrainteq_temp]) + JuMP.unregister(optim, :linconstrainteq_temp) + end + if estim.Nk[] < estim.He + if haskey(optim, :linconstrainteq) + JuMP.delete(optim, optim[:linconstrainteq]) + JuMP.unregister(optim, :linconstrainteq) + end + @constraint(optim, linconstrainteq_temp, Aeq*Z̃var .== beq) + else + if !haskey(optim, :linconstrainteq) + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) + end + end return nothing end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." @@ -1334,11 +1328,11 @@ function con_nonlinprogeq_mhe!( x̂0arr, Ŵ, Z̃ ) nu, nx, nd, nk = model.nu, model.nx, model.nd, model.nk - nε, nx̂, He = estim.nε, estim.nx̂, estim.He + nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads - nŵ = nx̂ - nx̃ = nε + nx̂ + nw = nŵ - nxs + nx̃ = estim.nε + nx̂ p = estim.direct ? 0 : 1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) @@ -1349,15 +1343,15 @@ function con_nonlinprogeq_mhe!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] - ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] k = @views K[(1 + nk*(j-1)):(nk*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] + ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] + x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] sdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) - x̂0next .+= ŵ - sdnext .= @. x̂dnext - x̂dnext_Z̃ + f!(x̂dnext, k, model, x̂d_Z̃, û0, d0, model.p) + x̂dnext .+= ŵd + sdnext .= @. x̂dnext - x̂dnext_Z̃ end Nk < He && (geq[nx̂*Nk+1:end] .= 0) return geq From 9c190aced9ad274e5908a692a41c334a5f0a3f58 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 14:28:54 -0400 Subject: [PATCH 4/8] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 9cf322a82..df7a1d617 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.9.3" +version = "2.10.0" authors = ["Francis Gagnon"] [deps] From dd0daabac5c0d3ef7f800518dbbec288c975830e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 14:59:41 -0400 Subject: [PATCH 5/8] debug: one additional argument in `SingleShooting` method --- 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 f76d8c7dd..d244cdc35 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -545,7 +545,7 @@ end "Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModel` (N/A)." function init_defectmat_mhe( - model::SimModel{NT}, transcription::SingleShooting, He, Â, _ , _ , _ , _ , _ + model::SimModel{NT}, transcription::SingleShooting, He, Â, _ , _ , _ , _ , _ , _ ) where {NT<:Real} nx̂ = size(Â, 2) nŵ = nx̂ From 94e254ae82602bf635d609e265b7e3eb724b7d2e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 15:21:34 -0400 Subject: [PATCH 6/8] debug: new signature for `init_defectmat_mhe` --- src/estimator/mhe/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index b0e0ef009..72f9e035d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -981,8 +981,8 @@ function setmodel_estimator!( con.BX̂ .= BX̂ # --- defect matrices --- ES, GS, JS, BS = init_defectmat_mhe( - model, transcription, He, estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, - estim.direct + model, transcription, He, + estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, estim.As, estim.direct ) Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true) con.ẼS .= ẼS From 717ccb4763bb61993558769bfb3071cdca3dd05e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 16:54:53 -0400 Subject: [PATCH 7/8] doc: debug link --- 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 d244cdc35..f09a57339 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -858,7 +858,7 @@ end estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod ) -By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@refs). +By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@ref). The linear equality constraints include the stochastic defects only, and the `beq` vector is only zeros for this specific case. See [`init_defectmat_mhe`](@ref) for the From b267dff74ffe7caf4fe7595ca63c0a7df436ea71 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Aug 2026 17:15:26 -0400 Subject: [PATCH 8/8] doc: minor corrections --- src/estimator/mhe/transcription.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f09a57339..00e829808 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -489,8 +489,8 @@ end @doc raw""" init_defectmat_mhe( - model::SimModel{NT}, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ -) where {NT<:Real} + model::SimModel, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ + ) -> ES, GS, JS, BS Init the matrices for computing the defects of the stochastic states only. @@ -855,8 +855,8 @@ end """ linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod -) + estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod + ) By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@ref).