From ba9641230f3b818047fb5638150cff1e05973539 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 15:44:03 -0400 Subject: [PATCH 1/8] doc: `init_predmat_mhe` prediction matrices for `MultipleShooting` --- docs/src/internals/state_estim.md | 3 ++ src/controller/transcription.jl | 2 +- src/estimator/mhe/transcription.jl | 55 ++++++++++++++++++++++++++++-- 3 files changed, 57 insertions(+), 3 deletions(-) diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index e473954ae..49785fe43 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -14,6 +14,7 @@ ModelPredictiveControl.init_ukf ModelPredictiveControl.init_internalmodel ModelPredictiveControl.init_ZtoŴ ModelPredictiveControl.init_predmat_mhe +ModelPredictiveControl.init_defectmat_mhe ModelPredictiveControl.relaxarrival ModelPredictiveControl.relaxX̂ ModelPredictiveControl.relaxŴ @@ -44,6 +45,8 @@ ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) ModelPredictiveControl.set_warmstart_mhe! ModelPredictiveControl.predict_mhe! ModelPredictiveControl.con_nonlinprog_mhe! +ModelPredictiveControl.con_nonlinprogeq_mhe! +ModelPredictiveControl.getstate! ``` ## Remove Operating Points diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9c1fbef9c..8f0413add 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -202,7 +202,7 @@ end model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ -Construct the prediction matrices for [`LinModel`](@ref) and [`MultipleShooting`](@ref). +Construct them for [`LinModel`](@ref) and [`MultipleShooting`](@ref). They are defined in the Extended Help section. diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index fc2f4afb7..152c39205 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -244,7 +244,7 @@ function init_predmat_mhe( return E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ end -""" +@doc raw""" init_predmat_mhe( model::LinModel, transcription::MultipleShooting, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct @@ -252,7 +252,58 @@ end Construct them for [`LinModel`](@ref) and [`MultipleShooting`](@ref). -TBW +The ``\mathbf{e_x̄}`` is identical to the [`SingleShooting`](@ref) transcription. The other +matrices are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + The matrices are compute by (notice the minus signs after the equalities): + ```math + \begin{aligned} + \mathbf{E} &= - \begin{bmatrix} \mathbf{E^x̂} & \mathbf{E^ŵ} \end{bmatrix} \\ + \mathbf{G} &= \mathbf{0} \\ + \mathbf{J} &= - \begin{bmatrix} + \mathbf{0} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ + \mathbf{B} &= \mathbf{0} + \end{aligned} + ``` + The ``\mathbf{E^ŵ}`` matrix is an appropriately size ``\mathbf{0}`` matrix and, for + ``p=0`` we have: + ```math + \mathbf{E^x̂} = \begin{bmatrix} + \mathbf{0} & \mathbf{Ĉ^m} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{Ĉ^m} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ĉ^m} \end{bmatrix} + ``` + or, for ``p=1``: + ```math + \mathbf{E^x̂} = \begin{bmatrix} + \mathbf{Ĉ^m} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{Ĉ^m} & \cdots & \mathbf{0} & \\ + \vdots & \vdots & \ddots & \vdots & \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ĉ^m} & \end{bmatrix} + ``` + The matrices for the estimated states are computed by: + ```math + \begin{aligned} + \mathbf{E_X̂} &= \begin{bmatrix} \mathbf{E_X̂^x̂} & \mathbf{E_X̂^ŵ} \end{bmatrix} \\ + \mathbf{G_X̂} &= \mathbf{0} \\ + \mathbf{J_X̂} &= \mathbf{0} \\ + \mathbf{B_X̂} &= \mathbf{0} + \end{aligned} + ``` + The ``\mathbf{E_X̂^ŵ}`` matrix is an appropriately size ``\mathbf{0}`` matrix and: + ```math + \mathbf{E_X̂^x̂} = \begin{bmatrix} + \mathbf{0} & \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{I} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{I} \end{bmatrix} + ``` """ function init_predmat_mhe( model::LinModel{NT}, ::MultipleShooting, From 40742419a73ca0a00a58f3f3476286b4cc55d054 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 15:56:34 -0400 Subject: [PATCH 2/8] doc: correct mistakes --- src/estimator/mhe/transcription.jl | 34 +++++++++++++++++++++++------- 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 152c39205..ca5b754e3 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -41,11 +41,11 @@ from ``j=N_k-1`` to ``0``, also in deviation form, are computed with: ```math \begin{aligned} \mathbf{X̂_0} &= \mathbf{E_X̂ Z + G_X̂ U_0 + J_X̂ D_0 + B_X̂} \\ - &= \mathbf{E_X̂ Z + F_x̂} + &= \mathbf{E_X̂ Z + F_X̂} \end{aligned} ``` The matrices ``\mathbf{E, G, J, B, E_X̂, G_X̂, J_X̂, B_X̂}`` are defined in the Extended Help -section. The vectors ``\mathbf{F, F_x̂, f_x̄}`` are recalculated at each discrete time step, +section. The vectors ``\mathbf{F, F_X̂, f_x̄}`` are recalculated at each discrete time step, see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstraint!(::MovingHorizonEstimator, ::LinModel)`](@ref). # Extended Help @@ -271,7 +271,7 @@ matrices are defined in the Extended Help section. \end{aligned} ``` The ``\mathbf{E^ŵ}`` matrix is an appropriately size ``\mathbf{0}`` matrix and, for - ``p=0`` we have: + ``p=0``, we have: ```math \mathbf{E^x̂} = \begin{bmatrix} \mathbf{0} & \mathbf{Ĉ^m} & \mathbf{0} & \cdots & \mathbf{0} \\ @@ -283,9 +283,9 @@ matrices are defined in the Extended Help section. ```math \mathbf{E^x̂} = \begin{bmatrix} \mathbf{Ĉ^m} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{Ĉ^m} & \cdots & \mathbf{0} & \\ - \vdots & \vdots & \ddots & \vdots & \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ĉ^m} & \end{bmatrix} + \mathbf{0} & \mathbf{Ĉ^m} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ĉ^m} & \mathbf{0} \end{bmatrix} ``` The matrices for the estimated states are computed by: ```math @@ -384,13 +384,31 @@ function init_predmat_mhe( return E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ 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 ) -> ES, GS, JS, BS -TBW +Init the matrices for computing the defects over the predicted states. + +With a [`MultipleShooting`](@ref) transcription, the decision vector ``\mathbf{Z}`` contains +the arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)``, the following states ``\mathbf{X̂_0}`` +(both defined as deviation vector from ``\mathbf{x̂_{op}}``) and the estimated process noise +``\mathbf{Ŵ}``, an equation similar to the prediction matrices (see [`init_predmat_mhe`](@ref)) +computes the defects of the estimated states over ``H_e``: +```math +\begin{aligned} + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S U_0} + \mathbf{J_S D̂_0} + \mathbf{B_S} \\ + &= \mathbf{E_S Z} + \mathbf{F_S} +\end{aligned} +``` +They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality constraints. The +matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help section. + +# Extended Help +!!! Extended Help + The defect matrices are computed with: """ function init_defectmat_mhe( model::LinModel{NT}, ::MultipleShooting, He, Â, B̂u, B̂d, x̂op, f̂op, direct From 3dc03f42b041abd36f3bc71d4bb306f6f5479d3d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 15:57:50 -0400 Subject: [PATCH 3/8] doc: correct mistakes --- 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 ca5b754e3..1118804aa 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -407,7 +407,7 @@ They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality const matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help section. # Extended Help -!!! Extended Help +!!! details "Extended Help" The defect matrices are computed with: """ function init_defectmat_mhe( From 3eed65036b2e70ce7ac8826b649506442f5b41c0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 17:22:32 -0400 Subject: [PATCH 4/8] doc: defect matrices for `MultipleShooting` of MHE and `LinModel` --- src/estimator/mhe/transcription.jl | 38 ++++++++++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 1118804aa..cbc59158e 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -260,7 +260,8 @@ matrices are defined in the Extended Help section. The matrices are compute by (notice the minus signs after the equalities): ```math \begin{aligned} - \mathbf{E} &= - \begin{bmatrix} \mathbf{E^x̂} & \mathbf{E^ŵ} \end{bmatrix} \\ + \mathbf{E} &= - \begin{bmatrix} + \mathbf{E^x̂} & \mathbf{E^ŵ} \end{bmatrix} \\ \mathbf{G} &= \mathbf{0} \\ \mathbf{J} &= - \begin{bmatrix} \mathbf{0} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ @@ -290,7 +291,8 @@ matrices are defined in the Extended Help section. The matrices for the estimated states are computed by: ```math \begin{aligned} - \mathbf{E_X̂} &= \begin{bmatrix} \mathbf{E_X̂^x̂} & \mathbf{E_X̂^ŵ} \end{bmatrix} \\ + \mathbf{E_X̂} &= \begin{bmatrix} + \mathbf{E_X̂^x̂} & \mathbf{E_X̂^ŵ} \end{bmatrix} \\ \mathbf{G_X̂} &= \mathbf{0} \\ \mathbf{J_X̂} &= \mathbf{0} \\ \mathbf{B_X̂} &= \mathbf{0} @@ -304,6 +306,8 @@ matrices are defined in the Extended Help section. \vdots & \vdots & \vdots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{I} \end{bmatrix} ``` + The appropriate rows and columns on these matrices are selected using the slicing + operators `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). """ function init_predmat_mhe( model::LinModel{NT}, ::MultipleShooting, @@ -409,6 +413,36 @@ matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help sectio # Extended Help !!! details "Extended Help" The defect matrices are computed with: + ```math + \begin{aligned} + \mathbf{E_S} &= \begin{bmatrix} + \mathbf{E_S^x̂} & \mathbf{E_S^ŵ} \end{bmatrix} \\ + \mathbf{E_S^x̂} &= \begin{bmatrix} + \mathbf{Â} & \mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{Â} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_S^ŵ} &= \begin{bmatrix} + \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{)} & \mathbf{I} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{I} \end{bmatrix} \\ + \mathbf{G_S} &= \begin{bmatrix} + \mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{)} & \mathbf{B̂_u} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{J_S} &= \begin{bmatrix} + \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{B̂_u} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ + \mathbf{B_S} &= \begin{bmatrix} + \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} + \end{aligned} + ``` + The appropriate rows and columns on these matrices are selected using the slicing + operators `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 From bcdf9c12d976999976fe88de12250fc50aea5a77 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 17:47:47 -0400 Subject: [PATCH 5/8] doc: correcting mistakes --- src/estimator/mhe/transcription.jl | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index cbc59158e..2ba49b1e8 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -307,7 +307,7 @@ matrices are defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{I} \end{bmatrix} ``` The appropriate rows and columns on these matrices are selected using the slicing - operators `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). + operator `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). """ function init_predmat_mhe( model::LinModel{NT}, ::MultipleShooting, @@ -397,10 +397,10 @@ end Init the matrices for computing the defects over the predicted states. With a [`MultipleShooting`](@ref) transcription, the decision vector ``\mathbf{Z}`` contains -the arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)``, the following states ``\mathbf{X̂_0}`` -(both defined as deviation vector from ``\mathbf{x̂_{op}}``) and the estimated process noise -``\mathbf{Ŵ}``, an equation similar to the prediction matrices (see [`init_predmat_mhe`](@ref)) -computes the defects of the estimated states over ``H_e``: +the arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)``, the stage states ``\mathbf{X̂_0}`` +(both defined as deviation vector from ``\mathbf{x̂_{op}}``) and the estimated process noises +``\mathbf{Ŵ}``. Knowing this, an equation similar to the prediction matrices (see +[`init_predmat_mhe`](@ref)) computes the defects of the estimated states over ``H_e``: ```math \begin{aligned} \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S U_0} + \mathbf{J_S D̂_0} + \mathbf{B_S} \\ @@ -424,25 +424,28 @@ matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help sectio \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & \mathbf{-I} \end{bmatrix} \\ \mathbf{E_S^ŵ} &= \begin{bmatrix} \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{)} & \mathbf{I} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{I} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{I} \end{bmatrix} \\ \mathbf{G_S} &= \begin{bmatrix} \mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{)} & \mathbf{B̂_u} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{B̂_u} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{J_S} &= \begin{bmatrix} - \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{B̂_u} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ + \mathbf{J_S^†} &= \begin{bmatrix} + \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{B̂_d} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} \end{bmatrix} \ , \quad + \mathbf{J_S} &= \begin{cases} + [\begin{smallmatrix} \mathbf{J_S^†} & \mathbf{0} \end{smallmatrix}] & p=0 \\ + [\begin{smallmatrix} \mathbf{0} & \mathbf{J_S^†} \end{smallmatrix}] & p=1 \end{cases} \\ \mathbf{B_S} &= \begin{bmatrix} \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} \end{aligned} ``` The appropriate rows and columns on these matrices are selected using the slicing - operators `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). + 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 From fd020f6cfcf2f6067b39a8310d406d1ff1bd3e8e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 17:49:17 -0400 Subject: [PATCH 6/8] doc: correct mistakes --- 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 2ba49b1e8..393497bc4 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -403,7 +403,7 @@ the arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)``, the stage states ``\math [`init_predmat_mhe`](@ref)) computes the defects of the estimated states over ``H_e``: ```math \begin{aligned} - \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S U_0} + \mathbf{J_S D̂_0} + \mathbf{B_S} \\ + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S U_0} + \mathbf{J_S D_0} + \mathbf{B_S} \\ &= \mathbf{E_S Z} + \mathbf{F_S} \end{aligned} ``` From 1400eed50af153c0386777047823a5c3a48913eb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 18:01:08 -0400 Subject: [PATCH 7/8] doc: correct mistakes --- src/estimator/mhe/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c80e986f3..19d12e9e2 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -411,8 +411,8 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ r &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} \end{aligned} -See [`init_ZtoŴ`](@ref) for the definition of the conversion matrix ``\mathbf{T_ŵ}``. ``` +See [`init_ZtoŴ`](@ref) for the definition of the conversion matrix ``\mathbf{T_ŵ}``. """ function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT<:Real invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He From a141f6614519fa807e29dafcd9ff6f83ffbe2ae5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 2 Aug 2026 18:13:02 -0400 Subject: [PATCH 8/8] changed: consistency in capitalization for MHE state predictions --- src/estimator/mhe/construct.jl | 26 +++++++++++++------------- src/estimator/mhe/execute.jl | 6 +++--- src/estimator/mhe/transcription.jl | 26 +++++++++++++------------- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index d7ab4e29a..dab45443f 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -16,8 +16,8 @@ the former is always a linear inequality constraint (it's a decision variable). """ struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # matrices for the estimated state constraints: - Ẽx̂ ::Matrix{NT} - Fx̂ ::Vector{NT} + ẼX̂ ::Matrix{NT} + FX̂ ::Vector{NT} GX̂ ::Matrix{NT} JX̂ ::Matrix{NT} BX̂ ::Vector{NT} @@ -1110,7 +1110,7 @@ function init_defaultcon_mhe( C_ŵmin, C_ŵmax = fill(0.0, nŴ), fill(0.0, nŴ) C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm) A_x̂min, A_x̂max, ẽx̄ = relaxarrival(ex̄, c_x̂min, c_x̂max, nε) - A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(EX̂, C_x̂min, C_x̂max, nε) + A_X̂min, A_X̂max, ẼX̂ = relaxX̂(EX̂, C_x̂min, C_x̂max, nε) A_Ŵmin, A_Ŵmax = relaxŴ(Tŵ, C_ŵmin, C_ŵmax, nε) A_V̂min, A_V̂max, Ẽ = relaxV̂(E, C_v̂min, C_v̂max , nε) Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true) @@ -1125,10 +1125,10 @@ function init_defaultcon_mhe( A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, Aeq ) # dummy vectors (updated just before optimization): - Fx̂, FS = zeros(NT, nx̂*He), zeros(NT, nS) + FX̂, FS = zeros(NT, nx̂*He), zeros(NT, nS) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = EstimatorConstraint{NT, GCfunc}( - Ẽx̂, Fx̂, GX̂, JX̂, BX̂, + ẼX̂, FX̂, GX̂, JX̂, BX̂, ẼS, FS, GS, JS, BS, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, Z̃min, Z̃max, @@ -1179,13 +1179,13 @@ function relaxarrival(ex̄::AbstractMatrix{NT}, c_x̂min, c_x̂max, nε) where N end @doc raw""" - relaxX̂(EX̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, Ẽx̂ + relaxX̂(EX̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, ẼX̂ Augment estimated state constraints with slack variable ε for softening the MHE. Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = -[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ_x̂}`` -matrix that appears in estimated states equation ``\mathbf{X̂} = \mathbf{Ẽ_x̂ Z̃ + F_x̂}``. It +[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ_X̂}`` +matrix that appears in estimated states equation ``\mathbf{X̂_0} = \mathbf{Ẽ_X̂ Z̃ + F_X̂}``. It also returns the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} @@ -1193,8 +1193,8 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints: \mathbf{A_{X̂_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - - \mathbf{(X̂_{min} - X̂_{op}) + F_x̂} \\ - + \mathbf{(X̂_{max} - X̂_{op}) - F_x̂} + - \mathbf{(X̂_{min} - X̂_{op}) + F_X̂} \\ + + \mathbf{(X̂_{max} - X̂_{op}) - F_X̂} \end{bmatrix} ``` in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains @@ -1209,12 +1209,12 @@ function relaxX̂(EX̂::AbstractMatrix{NT}, C_x̂min, C_x̂max, nε) where NT<:R # ε impacts estimated process noise constraint calculations: A_X̂min, A_X̂max = -[C_x̂min EX̂], [-C_x̂max EX̂] # ε has no impact on estimated process noises: - Ẽx̂ = [zeros(NT, size(EX̂, 1), 1) EX̂] + ẼX̂ = [zeros(NT, size(EX̂, 1), 1) EX̂] else # Z̃ = Z (only hard constraints) - Ẽx̂ = EX̂ + ẼX̂ = EX̂ A_X̂min, A_X̂max = -EX̂, EX̂ end - return A_X̂min, A_X̂max, Ẽx̂ + return A_X̂min, A_X̂max, ẼX̂ end @doc raw""" diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 19d12e9e2..0da59d0e7 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.H̃ .= 0 estim.q̃ .= 0 estim.r .= 0 - estim.con.Fx̂ .= 0 + estim.con.FX̂ .= 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] @@ -918,14 +918,14 @@ function setmodel_estimator!( He, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, estim.x̂op, estim.f̂op, estim.direct ) - A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(EX̂, con.C_x̂min, con.C_x̂max, nε) + A_X̂min, A_X̂max, ẼX̂ = relaxX̂(EX̂, con.C_x̂min, con.C_x̂max, nε) A_V̂min, A_V̂max, Ẽ = relaxV̂(E, con.C_v̂min, con.C_v̂max, nε) estim.Ẽ .= Ẽ estim.G .= G estim.J .= J estim.B .= B # --- linear inequality constraints --- - con.Ẽx̂ .= Ẽx̂ + con.ẼX̂ .= ẼX̂ con.GX̂ .= GX̂ con.JX̂ .= JX̂ con.BX̂ .= BX̂ diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 393497bc4..7628bd6ee 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -743,7 +743,7 @@ end Set `b` vector for the linear model 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 +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). """ function linconstraint!( @@ -757,20 +757,20 @@ function linconstraint!( BX̂ = estim.con.BX̂[1:nX̂] GX̂, U0 = estim.con.GX̂[1:nX̂, 1:nU], estim.U0[1:nU] JX̂, D0 = estim.con.JX̂[1:nX̂, 1:nD], estim.D0[1:nD] - Fx̂ = @views estim.con.Fx̂[1:nX̂] + FX̂ = @views estim.con.FX̂[1:nX̂] else BX̂ = estim.con.BX̂ GX̂, U0 = estim.con.GX̂, estim.U0 JX̂, D0 = estim.con.JX̂, estim.D0 - Fx̂ = estim.con.Fx̂ + FX̂ = estim.con.FX̂ end X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂) Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, nŵ) V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, nym) - # --- update Fx̂ vectors for MHE state constraints --- - Fx̂ .= BX̂ - mul!(Fx̂, GX̂, U0, 1, 1) - model.nd > 0 && mul!(Fx̂, JX̂, D0, 1, 1) + # --- update FX̂ vectors for MHE state constraints --- + FX̂ .= BX̂ + mul!(FX̂, GX̂, U0, 1, 1) + model.nd > 0 && mul!(FX̂, JX̂, D0, 1, 1) # --- update b vector for linear inequality constraints --- nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min) nx̂ = length(estim.con.x̂0min) @@ -779,9 +779,9 @@ function linconstraint!( n += nx̂ estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max n += nx̂ - estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂ + estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.FX̂ n += nX̂_He - estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂ + estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.FX̂ n += nX̂_He estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin n += nŴ_He @@ -1040,7 +1040,7 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k ```math \begin{aligned} \mathbf{V̂} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ -\mathbf{X̂_0} &= \mathbf{Ẽ_x̂ Z̃} + \mathbf{F_x̂} +\mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂} \end{aligned} ``` """ @@ -1055,16 +1055,16 @@ function predict_mhe!( nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk nZ̃ = nε + estim.nx̂ + nŴ Ẽ, F = estim.Ẽ[1:nYm, 1:nZ̃], estim.F[1:nYm] - Ẽx̂, Fx̂ = estim.con.Ẽx̂[1:nX̂, 1:nZ̃], estim.con.Fx̂[1:nX̂] + ẼX̂, FX̂ = estim.con.ẼX̂[1:nX̂, 1:nZ̃], estim.con.FX̂[1:nX̂] Z̃ = Z̃[1:nZ̃] V̂_res, X̂0_res = @views V̂[1:nYm], X̂0[1:nX̂] else Ẽ, F = estim.Ẽ, estim.F - Ẽx̂, Fx̂ = estim.con.Ẽx̂, estim.con.Fx̂ + ẼX̂, FX̂ = estim.con.ẼX̂, estim.con.FX̂ V̂_res, X̂0_res = V̂, X̂0 end V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F - X̂0_res .= mul!(X̂0_res, Ẽx̂, Z̃) .+ Fx̂ + X̂0_res .= mul!(X̂0_res, ẼX̂, Z̃) .+ FX̂ return V̂, X̂0 end