From d98c209ae37838677483e8d3bf1ef29aa669f40c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 12:20:46 -0400 Subject: [PATCH 01/40] doc : moving `disturbedinput!` to state estimator internals --- docs/src/internals/predictive_control.md | 1 - docs/src/internals/state_estim.md | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 0700a5232..ffb5e7b7a 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -45,7 +45,6 @@ ModelPredictiveControl.linconstrainteq! ModelPredictiveControl.optim_objective!(::PredictiveController) ModelPredictiveControl.set_warmstart_mpc! ModelPredictiveControl.predict! -ModelPredictiveControl.disturbedinput! ModelPredictiveControl.con_nonlinprog! ModelPredictiveControl.con_nonlinprogeq! ModelPredictiveControl.getinput! diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 43a7f2c73..798952d41 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -81,6 +81,7 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel, ::Tr ```@docs ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) ModelPredictiveControl.set_warmstart_mhe! +ModelPredictiveControl.disturbedinput! ModelPredictiveControl.predict_mhe! ModelPredictiveControl.con_nonlinprog_mhe! ModelPredictiveControl.con_nonlinprogeq_mhe! From ccf3d5548a92111a5266559f438593c1a53a3d10 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 14:10:36 -0400 Subject: [PATCH 02/40] doc: new "Miscellaneous Internal Methods" section --- docs/make.jl | 1 + docs/src/internals/misc.md | 11 +++++++++++ docs/src/internals/predictive_control.md | 1 - 3 files changed, 12 insertions(+), 1 deletion(-) create mode 100644 docs/src/internals/misc.md diff --git a/docs/make.jl b/docs/make.jl index 4a1c3092b..d89f99a71 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -55,6 +55,7 @@ makedocs( "Plant Models" => "internals/sim_model.md", "State Estimators" => "internals/state_estim.md", "Predictive Controllers" => "internals/predictive_control.md", + "Miscellaneous" => "internals/misc.md" ], ], "Index" => "func_index.md" diff --git a/docs/src/internals/misc.md b/docs/src/internals/misc.md new file mode 100644 index 000000000..557d5f96b --- /dev/null +++ b/docs/src/internals/misc.md @@ -0,0 +1,11 @@ +# Functions: Miscellaneous Internal Methods + +```@contents +Pages = ["misc.md"] +``` + +## Orthogonal Collocation + +```@docs +ModelPredictiveControl.init_orthocolloc +``` diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index ffb5e7b7a..f5ea36706 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -15,7 +15,6 @@ The prediction methodology of this module is mainly based on Maciejowski textboo ModelPredictiveControl.move_blocking ModelPredictiveControl.init_ZtoΔU ModelPredictiveControl.init_ZtoU -ModelPredictiveControl.init_orthocolloc ModelPredictiveControl.init_predmat ModelPredictiveControl.init_defectmat ModelPredictiveControl.relaxU From 0e3288c466165b9248c5717d55281c36b5426ea4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 14:36:47 -0400 Subject: [PATCH 03/40] doc: making `OrthogonalCollocation` more generic --- src/transcription.jl | 65 +++++++++++++++++++++++++++++++++----------- 1 file changed, 49 insertions(+), 16 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 9fadfd123..e1991769e 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -177,8 +177,9 @@ Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s only. The `h` argument is the hold order for ``\mathbf{u}`` (`0` or `1`), and the `no` -argument, the number of collocation points ``n_o``. The decision variable is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points: +argument, the number of collocation points ``n_o``. The decision variable of +[`PredictiveController`](@ref) is similar to [`MultipleShooting`](@ref), but it also +includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -202,13 +203,15 @@ Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstrin descriptions of `f_threads` and `h_threads` keywords. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocation points. It is efficient for highly stiff systems, but generally more expensive than the -other methods for non-stiff systems. This transcription is not supported by the -[`MovingHorizonEstimator`](@ref) for now. See Extended Help for more details. +other methods for non-stiff systems. See Extended Help for more details and transcription +of [`MovingHorizonEstimator`](@ref) objects. !!! warning - The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the - construction of the [`NonLinModel`](@ref) to estimate the plant states, not orthogonal - collocation (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). + Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a + `OrthogonalCollocation` transcription, the built-in [`StateEstimator`](@ref) will still + use the `solver` provided at the construction of the [`NonLinModel`](@ref) to estimate + the plant states, not orthogonal collocation (see `supersample` option of + [`RungeKutta`](@ref) for stiff systems). Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for this transcription method (sparser formulation than [`MultipleShooting`](@ref)). @@ -219,6 +222,32 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). states are left out of the ``\mathbf{K}`` vector since collocation methods require continuous-time dynamics and the stochastic model is discrete. + For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0_x̂} \\ + \mathbf{K} \\ + \mathbf{0_k} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} + ``` + The Extended Help of [`SingleShooting`](@ref) and [`MultipleShooting`](@ref) introduces + all these variables, except for vector with the intermediate stages of the deterministic + states at the collation points: + ```math + \mathbf{K} = \begin{bmatrix} + \mathbf{k}_{1}(k+N_k+p+0) \\ + \mathbf{k}_{2}(k+N_k+p+0) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+N_k+p+0) \\ + \mathbf{k}_{1}(k+N_k+p+1) \\ + \mathbf{k}_{2}(k+N_k+p+1) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+p-1) \end{bmatrix} + ``` + The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme @@ -287,19 +316,23 @@ and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagran L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` -The collocation constraints are nonlinear, but the defects of deterministic states +The collocation constraints are nonlinear, but the defects of the deterministic states ``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: ```math -\mathbf{s_c}(k+j+1) = \mathbf{0} = +\begin{aligned} +\mathbf{s_c}(k+1) + &= \mathbf{C_o} \begin{bmatrix} - \mathbf{k}_1(k+j) \\ - \mathbf{k}_2(k+j) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+j) \end{bmatrix} - + λ_o \mathbf{x̂_d}(k+j) - \mathbf{x̂_d}(k+j+1) + \mathbf{k}_1(k) \\ + \mathbf{k}_2(k) \\ + \vdots \\ + \mathbf{k}_{n_o}(k) \end{bmatrix} + + λ_o \mathbf{x̂_d}(k) - \mathbf{x̂_d}(k+1) \\ + &= \mathbf{0} +\end{aligned} ``` -for ``j = 0, 1, ... , H_p-1``. The ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are all -directly extracted from the decision variable `Z̃`. +This is a purely linear relation since the ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are +all directly extracted from the decision variable `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From 22e9c7653906835d0852ad304c698c44aa540e0c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 16:17:47 -0400 Subject: [PATCH 04/40] doc: idem --- src/controller/transcription.jl | 2 +- src/estimator/mhe/construct.jl | 8 ++-- src/estimator/mhe/transcription.jl | 60 ++++++++++++++++++++++++++++++ src/transcription.jl | 47 ++++++++++++++++++++--- 4 files changed, 108 insertions(+), 9 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 1203e594a..890d860f5 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1462,7 +1462,7 @@ the model dynamics are computed by: \mathbf{k̇}_{n_o}(k+j) \end{bmatrix} ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are -extracted from the decision variable `Z̃`. The ``\mathbf{x̂_d}`` vectors are the +extracted from the decision variables in `Z̃`. The ``\mathbf{x̂_d}`` vectors are the deterministic states extracted from ``\mathbf{X̂_0}`` also in `Z̃`, and they correspond to the states at the beginning of the interval ``τ_0=0``. The ``\mathbf{k̇}_i`` derivative for the ``i``th collocation point is computed from the continuous-time function `model.f!` and: diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fdd4de96d..b3303538b 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -108,6 +108,9 @@ struct MovingHorizonEstimator{ nyu::Int nxs::Int p::PT + Mo::SparseMatrixCSC{NT, Int} + Co::SparseMatrixCSC{NT, Int} + λo::NT As ::Matrix{NT} Cs_u::Matrix{NT} Cs_y::Matrix{NT} @@ -171,9 +174,6 @@ struct MovingHorizonEstimator{ Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) validate_transcription(model, transcription) - if transcription isa OrthogonalCollocation - error("OrthogonalCollocation is not supported for the MHE for now.") - end As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs @@ -186,6 +186,7 @@ struct MovingHorizonEstimator{ E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ = init_predmat_mhe( model, transcription, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) + Mo, Co, λo = init_orthocolloc(model, transcription) ES, GS, JS, BS = init_defectmat_mhe( model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, As, direct ) @@ -224,6 +225,7 @@ struct MovingHorizonEstimator{ He, nε, i_ym, nx̂, nym, nyu, nxs, p, + Mo, Co, λo, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Tŵ, diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 36adf555d..bb5a9ec35 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -543,6 +543,66 @@ function init_defectmat_mhe( return ES, GS, JS, BS end + + +@doc raw""" + init_defectmat_mhe( + model::SimModel, transcription::OrthogonalCollocation, He, Â, _ , _ , _ , _ , As, _ + ) -> ES, GS, JS, BS + +Init the matrices for computing the continuity constraints and stochastic state defects. + +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^ŵ} &= \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} + ``` +""" +function init_defectmat_mhe( + model::SimModel{NT}, ::OrthogonalCollocation, He, Â, _ , _ , _ , _ , As, _ +) where {NT<:Real} + 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̂) .+ nx̂*(j-1) + ESx̂[iRow, iCol] = As + end + 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)) + BS = zeros(NT, nxs*He) + return ES, GS, JS, BS +end + + + "Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModel` (N/A)." function init_defectmat_mhe( model::SimModel{NT}, transcription::SingleShooting, He, Â, _ , _ , _ , _ , _ , _ diff --git a/src/transcription.jl b/src/transcription.jl index e1991769e..dba29ba2d 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -316,13 +316,50 @@ and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagran L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` -The collocation constraints are nonlinear, but the defects of the deterministic states -``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: +The ``\mathbf{M_o}`` matrix is used in the nonlinear collocation constraints. The defects +between the deterministic state derivative at the ``n_o`` collocation points and the model +dynamics at the discrete time ``k`` are given by: +```math +\begin{aligned} +\mathbf{s_k}(k) + &= \mathbf{M_o} \begin{bmatrix} + \mathbf{k}_1(k) - \mathbf{x̂_d}(k) \\ + \mathbf{k}_2(k) - \mathbf{x̂_d}(k) \\ + \vdots \\ + \mathbf{k}_{n_o}(k) - \mathbf{x̂_d}(k) \end{bmatrix} + - \begin{bmatrix} + \mathbf{k̇}_1(k) \\ + \mathbf{k̇}_2(k) \\ + \vdots \\ + \mathbf{k̇}_{n_o}(k) \end{bmatrix} \\ + &= \mathbf{0} +\end{aligned} +``` +knowing that the ``\mathbf{k}_i(k)`` vectors are directly extracted from the decision +variables in `Z̃`. The ``\mathbf{x̂_d}`` vector is the deterministic state at the beginning of +the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for +the ``i``th collocation point is computed from the continuous-time function `model.f!` and: +```math +\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) +``` +Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs +and disturbances are piecewise constant or linear: +```math +\begin{aligned} +\mathbf{û}_i(k) &= \begin{cases} + \mathbf{û_0}(k) & h = 0 \\ + (1-τ_i)\mathbf{û_0}(k) + τ_i\mathbf{û_0}(k+1) & h = 1 \end{cases} \\ +\mathbf{d̂}_i(k) &= (1-τ_i)\mathbf{d̂_0}(k) + τ_i\mathbf{d̂_0}(k+1) +\end{aligned} +``` +The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). + +The defects of the deterministic states ``\mathbf{x̂_d}`` for the continuity constraints are +in fact linear equality constraints: ```math \begin{aligned} \mathbf{s_c}(k+1) - &= - \mathbf{C_o} \begin{bmatrix} + &= \mathbf{C_o} \begin{bmatrix} \mathbf{k}_1(k) \\ \mathbf{k}_2(k) \\ \vdots \\ @@ -332,7 +369,7 @@ The collocation constraints are nonlinear, but the defects of the deterministic \end{aligned} ``` This is a purely linear relation since the ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are -all directly extracted from the decision variable `Z̃`. +all directly extracted the decision variables in `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From ce92212d3b83a3fb6ed7ac87bb74eb1e49bd9c83 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 16:21:30 -0400 Subject: [PATCH 05/40] doc: minor corrections --- src/transcription.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index dba29ba2d..186e8d656 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -342,8 +342,8 @@ the ``i``th collocation point is computed from the continuous-time function `mod ```math \mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) ``` -Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs -and disturbances are piecewise constant or linear: +Based on the normalized time ``τ_i`` and the hold order `transcription.h`, the inputs and +disturbances are piecewise constant or linear: ```math \begin{aligned} \mathbf{û}_i(k) &= \begin{cases} @@ -352,7 +352,7 @@ and disturbances are piecewise constant or linear: \mathbf{d̂}_i(k) &= (1-τ_i)\mathbf{d̂_0}(k) + τ_i\mathbf{d̂_0}(k+1) \end{aligned} ``` -The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). +The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The defects of the deterministic states ``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: @@ -369,7 +369,7 @@ in fact linear equality constraints: \end{aligned} ``` This is a purely linear relation since the ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are -all directly extracted the decision variables in `Z̃`. +all extracted the decision variables in `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From 13a84b07912ae407af254f2e9b1c25fe7b8aa61d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 16:36:11 -0400 Subject: [PATCH 06/40] doc: minor correction --- src/transcription.jl | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 186e8d656..35af29faa 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -309,15 +309,15 @@ matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed wit λ_o &= L_0(1) \end{aligned} ``` -where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values w/o the coefficients -and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagrange polynomial -``L_j(τ)`` bases are defined as: +where ``T_s`` is the sampling time `model.Ts`, ``\mathbf{P_o}`` is a matrix to evaluate the +polynomial values w/o the coefficients and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate +its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: ```math L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` The ``\mathbf{M_o}`` matrix is used in the nonlinear collocation constraints. The defects -between the deterministic state derivative at the ``n_o`` collocation points and the model +between the deterministic state derivative for the ``n_o`` collocation points and the model dynamics at the discrete time ``k`` are given by: ```math \begin{aligned} @@ -336,9 +336,10 @@ dynamics at the discrete time ``k`` are given by: \end{aligned} ``` knowing that the ``\mathbf{k}_i(k)`` vectors are directly extracted from the decision -variables in `Z̃`. The ``\mathbf{x̂_d}`` vector is the deterministic state at the beginning of -the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for -the ``i``th collocation point is computed from the continuous-time function `model.f!` and: +variables in `Z̃`. The ``\mathbf{x̂_d}(k)`` vector is the estimated deterministic state at the +beginning of the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\mathbf{k̇}_i`` +derivatives for the ``i``th collocation point are computed from the continuous-time function +`model.f!` and: ```math \mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) ``` @@ -369,7 +370,7 @@ in fact linear equality constraints: \end{aligned} ``` This is a purely linear relation since the ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are -all extracted the decision variables in `Z̃`. +all extracted from the decision variables in `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From b7eaf5fffc09bad1321504b112f9dafb1855293a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 17:23:48 -0400 Subject: [PATCH 07/40] doc: minor correction --- src/controller/transcription.jl | 26 +++++-------------- src/transcription.jl | 44 +++++++++++++++++++-------------- 2 files changed, 31 insertions(+), 39 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 890d860f5..5e0222609 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1378,8 +1378,8 @@ The deterministic state defects are computed with: - \mathbf{x̂_d}(k+j+1) ``` for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x̂_d}`` are the deterministic states -extracted from the decision variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated -from the continuous-time function `model.f!` and: +extracted from the decision variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from +the continuous-time function `model.f!` and: ```math \begin{aligned} \mathbf{k̇}_1(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ @@ -1464,24 +1464,10 @@ the model dynamics are computed by: for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are extracted from the decision variables in `Z̃`. The ``\mathbf{x̂_d}`` vectors are the deterministic states extracted from ``\mathbf{X̂_0}`` also in `Z̃`, and they correspond to the -states at the beginning of the interval ``τ_0=0``. The ``\mathbf{k̇}_i`` derivative for the -``i``th collocation point is computed from the continuous-time function `model.f!` and: -```math -\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û}_i(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big) -``` -Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs -and disturbances are piecewise constant or linear: -```math -\begin{aligned} -\mathbf{û}_i(k+j) &= \begin{cases} - \mathbf{û_0}(k+j) & h = 0 \\ - (1-τ_i)\mathbf{û_0}(k+j) + τ_i\mathbf{û_0}(k+j+1) & h = 1 \end{cases} \\ -\mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) -\end{aligned} -``` -The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The defects for the -continuity constraints and stochastic states are linear equality constraints (see -[`init_defectmat`](@ref)). +states at the beginning of the interval ``τ_0=0``. The ``\mathbf{k̇}_i`` vectors are +evaluated from the continuous-time function `model.f`, as described in [`init_orthocolloc`](@ref) +documentation. The defects for the continuity constraints and the stochastic states are +linear equality constraints (see [`init_defectmat`](@ref) for details). """ function con_nonlinprogeq!( geq, _ , Û0, K̇, diff --git a/src/transcription.jl b/src/transcription.jl index 35af29faa..137552b41 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -288,25 +288,26 @@ end Init the differentiation and continuity matrices for [`OrthogonalCollocation`](@ref). Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the -interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct -polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity -matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: +interval ``[0, 1]`` with ``τ_0=0``, the trajectories for each state are approximated by a +distinct polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, the +continuity matrix ``\mathbf{C_o}`` and the continuity coefficient ``λ_o`` are pre-computed +with the identity matrix ``\mathbf{I}`` of size `(model.nx, model.nx)` and: ```math \begin{aligned} - \mathbf{P_o} &= \begin{bmatrix} - τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ - τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ - \mathbf{Ṗ_o} &= \begin{bmatrix} - τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ - τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ - \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - \mathbf{C_o} &= \begin{bmatrix} - L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ - λ_o &= L_0(1) +\mathbf{P_o} &= \begin{bmatrix} + τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ + τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ +\mathbf{Ṗ_o} &= \begin{bmatrix} + τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ + τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ +\mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ +\mathbf{C_o} &= \begin{bmatrix} + L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ + λ_o &= L_0(1) \end{aligned} ``` where ``T_s`` is the sampling time `model.Ts`, ``\mathbf{P_o}`` is a matrix to evaluate the @@ -344,7 +345,7 @@ derivatives for the ``i``th collocation point are computed from the continuous-t \mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) ``` Based on the normalized time ``τ_i`` and the hold order `transcription.h`, the inputs and -disturbances are piecewise constant or linear: +disturbances are either piecewise constant or linear: ```math \begin{aligned} \mathbf{û}_i(k) &= \begin{cases} @@ -396,7 +397,12 @@ function init_orthocolloc( λo = lagrange_end(0, transcription) return Mo, Co, λo end -"Return empty sparse matrices and `NaN` for other [`TranscriptionMethod`](@ref)" + +""" + init_orthocolloc(model::SimModel, transcription::TranscriptionMethod) + +Return empty sparse matrices and `NaN` value for other [`TranscriptionMethod`](@ref) types. +""" init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN "Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." From 6582370112baf8441752e20f059f3efb406b8630 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 17:33:44 -0400 Subject: [PATCH 08/40] doc: more details in `OrthogonalCollocation` docstring --- src/transcription.jl | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 137552b41..0df046b59 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -147,10 +147,11 @@ transcription method. # Extended Help !!! details "Extended Help" - Note that the stochastic model of the unmeasured disturbances is strictly discrete-time, - as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods - require continuous-time dynamics. Because of this, the stochastic states are transcribed - separately using a [`MultipleShooting`](@ref) method. See [`con_nonlinprogeq!`](@ref) + Note that the stochastic model of the unmeasured disturbances is strictly linear and + discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). + Collocation methods require continuous-time dynamics. Because of this, and also to + reduce the number of nonlinear constraints, the stochastic states are transcribed + separately using a linear [`MultipleShooting`](@ref) method. See [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. """ struct TrapezoidalCollocation <: CollocationMethod @@ -219,8 +220,9 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). # Extended Help !!! details "Extended Help" As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic - states are left out of the ``\mathbf{K}`` vector since collocation methods require - continuous-time dynamics and the stochastic model is discrete. + states are left out of the ``\mathbf{K}`` vector to reduce the dimensions, and also + because collocation methods require continuous-time dynamics and the stochastic model is + discrete. For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): ```math @@ -234,8 +236,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{0_ŵ} \end{bmatrix} ``` The Extended Help of [`SingleShooting`](@ref) and [`MultipleShooting`](@ref) introduces - all these variables, except for vector with the intermediate stages of the deterministic - states at the collation points: + all these variables, except for the vector with the intermediate stages of the + deterministic states at the collation points: ```math \mathbf{K} = \begin{bmatrix} \mathbf{k}_{1}(k+N_k+p+0) \\ @@ -252,7 +254,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`con_nonlinprogeq!`](@ref) for implementation details. + See [`init_orthocolloc`](@ref) and [`con_nonlinprogeq!`](@ref) for details. """ struct OrthogonalCollocation <: CollocationMethod h::Int From 9403be6f957e1e17cfae578c8c4bd22acd2f3f76 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 10 Aug 2026 17:44:04 -0400 Subject: [PATCH 09/40] doc: removing `finite element` in `OrthogonalCollocation` Finite elements would be additional subdivisions, AFAIK. --- src/transcription.jl | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 0df046b59..063b17e6a 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -198,14 +198,14 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} ``` and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation -point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` -keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or -Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for -descriptions of `f_threads` and `h_threads` keywords. This transcription computes the -predictions by enforcing the collocation and continuity constraints at the collocation -points. It is efficient for highly stiff systems, but generally more expensive than the -other methods for non-stiff systems. See Extended Help for more details and transcription -of [`MovingHorizonEstimator`](@ref) objects. +point at the ``j``th stage/interval (details in Extended Help). The `roots` keyword argument +is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or Gauss-Legendre quadrature, +respectively. See [`MultipleShooting`](@ref) docstring for descriptions of `f_threads` and +`h_threads` keywords. This transcription computes the predictions by enforcing the +collocation and continuity constraints at the collocation points. It is efficient for highly +stiff systems, but generally more expensive than the other methods for non-stiff systems. +See Extended Help for details and the transcription of [`MovingHorizonEstimator`](@ref) +objects. !!! warning Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a From 1944dae064b18888e3717a123e23d00306938148 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 08:54:09 -0400 Subject: [PATCH 10/40] doc: new internal section on homepage --- docs/src/index.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/src/index.md b/docs/src/index.md index 43564999a..aa8bd4a1a 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -59,5 +59,6 @@ Pages = [ joinpath("internals", "sim_model.md"), joinpath("internals", "state_estim.md"), joinpath("internals", "predictive_control.md") + joinpath("internals", "misc.md") ] ``` From f0903abc92ecb8f77ae4642c350aeb025b65f06b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 08:54:45 -0400 Subject: [PATCH 11/40] doc: idem --- docs/src/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/index.md b/docs/src/index.md index aa8bd4a1a..7b7c5e820 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -58,7 +58,7 @@ Depth = 1 Pages = [ joinpath("internals", "sim_model.md"), joinpath("internals", "state_estim.md"), - joinpath("internals", "predictive_control.md") + joinpath("internals", "predictive_control.md"), joinpath("internals", "misc.md") ] ``` From d9f7380e2b789d82dfe7cccd6bdcd5caecec08a6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 09:13:17 -0400 Subject: [PATCH 12/40] doc: changing title for consistency --- docs/src/internals/misc.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/internals/misc.md b/docs/src/internals/misc.md index 557d5f96b..a1eba67e9 100644 --- a/docs/src/internals/misc.md +++ b/docs/src/internals/misc.md @@ -1,4 +1,4 @@ -# Functions: Miscellaneous Internal Methods +# Functions: Miscellaneous Internals ```@contents Pages = ["misc.md"] From 0d5d333867cee109aa92ba81c6001dab56a3c529 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 09:17:07 -0400 Subject: [PATCH 13/40] doc: correct mistake --- src/transcription.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 063b17e6a..239f8cbd7 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -240,12 +240,12 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). deterministic states at the collation points: ```math \mathbf{K} = \begin{bmatrix} - \mathbf{k}_{1}(k+N_k+p+0) \\ - \mathbf{k}_{2}(k+N_k+p+0) \\ + \mathbf{k}_{1}(k-N_k+p+0) \\ + \mathbf{k}_{2}(k-N_k+p+0) \\ \vdots \\ - \mathbf{k}_{n_o}(k+N_k+p+0) \\ - \mathbf{k}_{1}(k+N_k+p+1) \\ - \mathbf{k}_{2}(k+N_k+p+1) \\ + \mathbf{k}_{n_o}(k-N_k+p+0) \\ + \mathbf{k}_{1}(k-N_k+p+1) \\ + \mathbf{k}_{2}(k-N_k+p+1) \\ \vdots \\ \mathbf{k}_{n_o}(k+p-1) \end{bmatrix} ``` From 2c9d4a3126702540872715e545ae469bb0be4b90 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 12:40:36 -0400 Subject: [PATCH 14/40] doc: adding hat on state defect equations Both are kinda valid, but I think I prefer with a hat, to convey that it all comes from a state estimator at the fundamental level, and also that they are predictions in the future for the MPC. --- src/controller/transcription.jl | 16 ++++++++-------- src/transcription.jl | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5e0222609..6fc361578 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1356,9 +1356,9 @@ function con_nonlinprogeq!( k = @views K[(1 + nk*(j-1)):(nk*j)] 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)] + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] f!(x̂dnext, k, model, x̂d_Z̃, û0, d̂0, model.p) - sdnext .= @. x̂dnext - x̂dnext_Z̃ + ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end return geq end @@ -1414,7 +1414,7 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - sdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] if f_threads || h < 1 || j < 2 @@ -1431,7 +1431,7 @@ function con_nonlinprogeq!( û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, p) end - sdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) end return geq end @@ -1449,7 +1449,7 @@ Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocat The defects between the deterministic state derivative at the ``n_o`` collocation points and the model dynamics are computed by: ```math -\mathbf{s_k}(k+j) +\mathbf{ŝ_k}(k+j) = \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(k+j) - \mathbf{x̂_d}(k+j) \\ \mathbf{k}_2(k+j) - \mathbf{x̂_d}(k+j) \\ @@ -1496,14 +1496,14 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - sk = @views geq[(1 + nk*(j-1)):(nk*j)] + ŝk = @views geq[(1 + nk*(j-1)):(nk*j)] # ----------------- collocation constraint defects ----------------------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end - mul!(sk, Mo, Δk) + mul!(ŝk, Mo, Δk) d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)] if h > 0 ûi = similar(û0) # TODO: remove this allocation @@ -1521,7 +1521,7 @@ function con_nonlinprogeq!( model.f!(k̇i, ki_Z̃, ûi, d̂i, p) end end - sk .-= k̇ + ŝk .-= k̇ end return geq end diff --git a/src/transcription.jl b/src/transcription.jl index 239f8cbd7..8c786a47e 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -324,7 +324,7 @@ between the deterministic state derivative for the ``n_o`` collocation points an dynamics at the discrete time ``k`` are given by: ```math \begin{aligned} -\mathbf{s_k}(k) +\mathbf{ŝ_k}(k) &= \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(k) - \mathbf{x̂_d}(k) \\ \mathbf{k}_2(k) - \mathbf{x̂_d}(k) \\ @@ -362,7 +362,7 @@ The defects of the deterministic states ``\mathbf{x̂_d}`` for the continuity co in fact linear equality constraints: ```math \begin{aligned} -\mathbf{s_c}(k+1) +\mathbf{ŝ_c}(k+1) &= \mathbf{C_o} \begin{bmatrix} \mathbf{k}_1(k) \\ \mathbf{k}_2(k) \\ From 2067809e920f51589bbe6918b5eca8c8b3930e46 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 13:20:17 -0400 Subject: [PATCH 15/40] doc: clarification --- src/controller/transcription.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6fc361578..3b6b6370e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -533,8 +533,9 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H \mathbf{0} & \mathbf{0} \end{bmatrix} \end{aligned} ``` - Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented so - the rows of ``\mathbf{E_S}`` and ``\mathbf{K_S}`` with an even index are removed. + Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented + hence the rows of ``\mathbf{E_S}`` and ``\mathbf{K_S}`` related to the stochastic states + are removed (2nd row of blocks, 4th row of blocks, and so on). """ function init_defectmat( model::NonLinModel, estim::StateEstimator, transcription::OrthogonalCollocation, From f105d7cdd034b815fa4ed95f7cddf6df36d4e37d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 14:00:10 -0400 Subject: [PATCH 16/40] doc: handling of estimated process noises for `OrthogonalCollocation` --- src/transcription.jl | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 8c786a47e..a56539edb 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -368,12 +368,14 @@ in fact linear equality constraints: \mathbf{k}_2(k) \\ \vdots \\ \mathbf{k}_{n_o}(k) \end{bmatrix} - + λ_o \mathbf{x̂_d}(k) - \mathbf{x̂_d}(k+1) \\ + + λ_o \mathbf{x̂_d}(k) + \mathbf{ŵ_d}(k) - \mathbf{x̂_d}(k+1) \\ &= \mathbf{0} \end{aligned} ``` -This is a purely linear relation since the ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are -all extracted from the decision variables in `Z̃`. +The estimated process noises of the deterministic states ``\mathbf{ŵ_d}(k) = \mathbf{0}`` +for [`NonLinMPC`](@ref) objects (only used for [`MovingHorizonEstimator`]). This is a +purely linear equation since the ``\mathbf{k}_i``, ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` +vectors are all extracted from the decision variables in `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From d8af8b0a72916e4fcf2a036aeceaf576d51d20a5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 14:52:38 -0400 Subject: [PATCH 17/40] doc: clarification on stochastic states --- src/estimator/mhe/transcription.jl | 51 +++++++++++++++++------------- src/transcription.jl | 10 +++--- 2 files changed, 35 insertions(+), 26 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index bb5a9ec35..2968620aa 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -406,7 +406,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, As, direct ) -> ES, GS, JS, BS Init the matrices for computing the defects over the predicted states. @@ -496,7 +496,7 @@ 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: +over ``H_e`` is therefore computed by: ```math \mathbf{Ŝ} = \mathbf{E_S Z} ``` @@ -504,8 +504,9 @@ 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: + Using the stochastic matrix ``\mathbf{A_s}`` of [`init_estimstoch`](@ref)), and + updating the stochastic states by adding their associated process noise estimates + ``\mathbf{ŵ_s}``, the defect matrices are computed with: ```math \begin{aligned} \mathbf{E_S^x̂} &= \begin{bmatrix} @@ -543,8 +544,6 @@ function init_defectmat_mhe( return ES, GS, JS, BS end - - @doc raw""" init_defectmat_mhe( model::SimModel, transcription::OrthogonalCollocation, He, Â, _ , _ , _ , _ , As, _ @@ -552,9 +551,9 @@ end Init the matrices for computing the continuity constraints and stochastic state defects. -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: +The documentation of [`init_orthocolloc`](@ref) shows that continuity constraints of the +[`OrthogonalCollocation`](@ref) are in fact linear. Combined with the stochastic state +defects, the linear equality constraints for this transcription is given by: ```math \mathbf{Ŝ} = \mathbf{E_S Z} ``` @@ -562,21 +561,29 @@ 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: + Using the stochastic matrix ``\mathbf{A_s}`` of [`init_estimstoch`](@ref)), and by + updating the states by adding their process noise estimates ``\mathbf{ŵ}``, the defect + matrices are 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^ŵ} &= \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} + \mathbf{E_S^x̂} &= \begin{bmatrix} + λ_o\mathbf{I} & \mathbf{0} &\mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & λ_o\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{A_s} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_S^k} &= \begin{bmatrix} + \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \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} \\ \end{aligned} ``` """ diff --git a/src/transcription.jl b/src/transcription.jl index a56539edb..2fd2a5a6a 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -372,10 +372,12 @@ in fact linear equality constraints: &= \mathbf{0} \end{aligned} ``` -The estimated process noises of the deterministic states ``\mathbf{ŵ_d}(k) = \mathbf{0}`` -for [`NonLinMPC`](@ref) objects (only used for [`MovingHorizonEstimator`]). This is a -purely linear equation since the ``\mathbf{k}_i``, ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` -vectors are all extracted from the decision variables in `Z̃`. +This is a purely linear equation since the ``\mathbf{k}_i``, ``\mathbf{x̂_d}`` and +``\mathbf{ŵ_d}`` vectors are all extracted from the decision variables in `Z̃`. The estimated +process noises of the deterministic states ``\mathbf{ŵ_d}(k) = \mathbf{0}`` for [`NonLinMPC`](@ref) +objects (only used for [`MovingHorizonEstimator`](@ref)). Note that handling the estimated +process noise in the continuity constraint implicitly assumes that it's a discrete +stochastic process (like all the other [`StateEstimator`](@ref) types in this package). """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation From 474139a76c316a28e219f9e14b934b4528df2b71 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 15:02:51 -0400 Subject: [PATCH 18/40] changed: argument order in `init_predmat_mhe` and `init_defectmat_mhe` --- src/estimator/mhe/construct.jl | 4 +-- src/estimator/mhe/execute.jl | 4 +-- src/estimator/mhe/transcription.jl | 51 +++++++++++++++--------------- 3 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index b3303538b..5046becac 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -184,11 +184,11 @@ struct MovingHorizonEstimator{ x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Tŵ = init_ZtoŴ(model, transcription, He, nx̂, nŵ) E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ = init_predmat_mhe( - model, transcription, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + model, transcription, direct, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) Mo, Co, λo = init_orthocolloc(model, transcription) ES, GS, JS, BS = init_defectmat_mhe( - model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, As, direct + model, transcription, direct, He, Â, B̂u, B̂d, x̂op, f̂op, As ) # dummy values (updated just before optimization): F, fx̄ = zeros(NT, nym*He), zeros(NT, nx̂) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 25453e58a..5b69c77ce 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -982,8 +982,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.As, estim.direct + model, transcription, estim.direct, He, + estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, estim.As ) Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true) con.ẼS .= ẼS diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 2968620aa..11604bb3f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -19,8 +19,8 @@ end @doc raw""" init_predmat_mhe( - model::LinModel, transcription::SingleShooting, - He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + model::LinModel, 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̂ Construct the MHE prediction matrices for [`LinModel`](@ref) and [`SingleShooting`](@ref). @@ -149,7 +149,7 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain All these matrices are truncated when ``N_k < H_e`` (at the beginning). """ function init_predmat_mhe( - model::LinModel{NT}, ::SingleShooting, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + model::LinModel{NT}, ::SingleShooting, direct::Bool, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) where {NT<:Real} nu, nd = model.nu, model.nd nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -261,8 +261,8 @@ end @doc raw""" init_predmat_mhe( - model::LinModel, transcription::MultipleShooting, - He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + model::LinModel, transcription::MultipleShooting, direct::Bool, + He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Construct them for [`LinModel`](@ref) and [`MultipleShooting`](@ref). @@ -325,8 +325,8 @@ matrices are defined in the Extended Help section. operator `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning). """ function init_predmat_mhe( - model::LinModel{NT}, ::MultipleShooting, - He, Â, _ , Ĉm, _ , D̂dm, _ , _ , direct + model::LinModel{NT}, ::MultipleShooting, direct::Bool, + He, Â, _ , Ĉm, _ , D̂dm, _ , _ ) where {NT<:Real} nu, nd = model.nu, model.nd nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -351,15 +351,15 @@ end """ init_predmat_mhe( - model::SimModel, ::SingleShooting, - He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + 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::SimModel{NT}, transcription::SingleShooting, - He, Â, _ , Ĉm, _ , _ , _ , _ , _ + model::SimModel{NT}, transcription::SingleShooting, ::Bool, + He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ @@ -378,15 +378,15 @@ end """ init_predmat_mhe( - model::SimModel, ::TranscriptionMethod, - He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + 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::SimModel{NT}, transcription::TranscriptionMethod, - He, Â, _ , Ĉm, _ , _ , _ , _ , _ + model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, + He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ @@ -405,8 +405,8 @@ end @doc raw""" init_defectmat_mhe( - model::LinModel, transcription::MultipleShooting, - He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, As, direct + model::LinModel, transcription::MultipleShooting, direct::Bool, + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, As ) -> ES, GS, JS, BS Init the matrices for computing the defects over the predicted states. @@ -463,7 +463,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, direct::Bool, He, Â, B̂u, B̂d, x̂op, f̂op, _ ) where {NT<:Real} nd = model.nd nx̂ = size(Â, 2) @@ -489,7 +489,8 @@ end @doc raw""" init_defectmat_mhe( - model::SimModel, transcription::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ + model::SimModel, transcription::TranscriptionMethod, direct::Bool, + He, Â, _ , _ , _ , _ , As ) -> ES, GS, JS, BS Init the matrices for computing the defects of the stochastic states only. @@ -524,7 +525,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::TranscriptionMethod, He, Â, _ , _ , _ , _ , As, _ + model::SimModel{NT}, ::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs @@ -546,7 +547,8 @@ end @doc raw""" init_defectmat_mhe( - model::SimModel, transcription::OrthogonalCollocation, He, Â, _ , _ , _ , _ , As, _ + model::SimModel, transcription::OrthogonalCollocation, direct::Bool + He, Â, _ , _ , _ , _ , As ) -> ES, GS, JS, BS Init the matrices for computing the continuity constraints and stochastic state defects. @@ -588,7 +590,8 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::OrthogonalCollocation, He, Â, _ , _ , _ , _ , As, _ + model::SimModel{NT}, ::OrthogonalCollocation, ::Bool, + He, Â, _ , _ , _ , _ , As ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) nx = nx̂ - nxs @@ -608,11 +611,9 @@ function init_defectmat_mhe( return ES, GS, JS, BS 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, ::Bool, He, Â, _ , _ , _ , _ , _ ) where {NT<:Real} nx̂ = size(Â, 2) nŵ = nx̂ From ecf12bdb44819c1a8b3b5a7cab9334ebd455ddaf Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 15:13:43 -0400 Subject: [PATCH 19/40] changed: idem --- src/estimator/mhe/execute.jl | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 5b69c77ce..60fa29767 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -965,9 +965,8 @@ function setmodel_estimator!( estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- predictions matrices --- E, G, J, B, _ , EX̂, GX̂, JX̂, BX̂ = init_predmat_mhe( - model, transcription, - He, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, estim.x̂op, estim.f̂op, - estim.direct + model, transcription, estim.direct, + He, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, estim.x̂op, estim.f̂op ) 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ε) From 99dd0e3eb85d0609b18db8458bc629112fd64090 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 16:33:05 -0400 Subject: [PATCH 20/40] added: `get_nZ_mhe` method for `OrthogonalCollocation` --- src/controller/transcription.jl | 2 +- src/estimator/construct.jl | 2 +- src/estimator/mhe/construct.jl | 17 +++++---- src/estimator/mhe/execute.jl | 7 ++-- src/estimator/mhe/transcription.jl | 55 +++++++++++++++++------------- 5 files changed, 49 insertions(+), 34 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 3b6b6370e..6b4ada585 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -484,7 +484,7 @@ end @doc raw""" init_defectmat( model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, - Hp, Hc, nb, Co=nothing, λo=nothing + Hp, Hc, _ , Co, λo ) -> ES, GS, JS, KS, VS, BS Init the matrices for computing the continuity constraints and stochastic state defects. diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 873573ea9..e9286aae1 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -34,7 +34,7 @@ function StateEstimatorBuffer{NT}( He::Int=0, nŵ::Int=nx̂, nε::Int=0, transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) nV̂, nŴ, nX̂, nŶ = nym*He, nŵ*He, nx̂*He, ny*He u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 5046becac..492144229 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -169,7 +169,8 @@ struct MovingHorizonEstimator{ GCfunc<:Function, CE<:KalmanEstimator{NT} } - nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nu, ny, nd = model.nu, model.ny, model.nd + nk = get_nk(model, transcription) 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) @@ -188,7 +189,7 @@ struct MovingHorizonEstimator{ ) Mo, Co, λo = init_orthocolloc(model, transcription) ES, GS, JS, BS = init_defectmat_mhe( - model, transcription, direct, He, Â, B̂u, B̂d, x̂op, f̂op, As + model, transcription, direct, He, Â, B̂u, B̂d, x̂op, f̂op, As, Co, λo ) # dummy values (updated just before optimization): F, fx̄ = zeros(NT, nym*He), zeros(NT, nx̂) @@ -200,7 +201,7 @@ struct MovingHorizonEstimator{ ES, GS, JS, BS, gc!, nc ) - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) @@ -1075,9 +1076,10 @@ in which ``\mathbf{T_{ŵ}} = [\begin{smallmatrix} \mathbf{0} & \mathbf{I} \end{ and ``\mathbf{0}`` is properly sized for the `transcription` instance. """ function init_ZtoŴ( - ::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ + model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} - nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(model, transcription) + nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nk, nŵ) Tŵ = [spzeros(NT, nŴ, nZ-nŴ) I] return Tŵ end @@ -1299,11 +1301,12 @@ end Init the decision variable box constraints `Z̃min` and `Z̃max` for [`MovingHorizonEstimator`](@ref). """ function init_boxconstraint_mhe( - ::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ, nε, + model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ, nε, 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} - nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(model, transcription) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) 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 60fa29767..34f146bb3 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -421,7 +421,9 @@ function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT< F, C, optim = estim.F, estim.C, estim.optim fx̄, r = estim.fx̄, estim.r nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nYm, nZ = estim.nym*Nk, get_nZ_mhe(estim.transcription, Nk, nx̂, nŵ) + nYm = estim.nym*Nk + nk = get_nk(model, estim.transcription) + nZ = get_nZ_mhe(estim.transcription, Nk, nx̂, nk, 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) @@ -470,7 +472,8 @@ getx̄!(x̄, estim::MovingHorizonEstimator, x̂0arr) = (x̄ .= estim.x̂0arr_old "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̂ - nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(estim.model, transcription) + nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) Ŵ[1:nŵ*He] .= @views Z̃[(nZ̃ - nŵ*He + 1):end] return Ŵ end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 11604bb3f..f12a0042f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1,6 +1,7 @@ "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(::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 the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) @@ -390,7 +391,8 @@ function init_predmat_mhe( ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ - nZ = get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) 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)] @@ -406,7 +408,7 @@ end @doc raw""" init_defectmat_mhe( model::LinModel, transcription::MultipleShooting, direct::Bool, - He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, As + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, _ , _ , _ ) -> ES, GS, JS, BS Init the matrices for computing the defects over the predicted states. @@ -463,7 +465,8 @@ 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, direct::Bool, He, Â, B̂u, B̂d, x̂op, f̂op, _ + model::LinModel{NT}, ::MultipleShooting, direct::Bool, + He, Â, B̂u, B̂d, x̂op, f̂op, _ , _ , _ ) where {NT<:Real} nd = model.nd nx̂ = size(Â, 2) @@ -490,7 +493,7 @@ end @doc raw""" init_defectmat_mhe( model::SimModel, transcription::TranscriptionMethod, direct::Bool, - He, Â, _ , _ , _ , _ , As + He, Â, _ , _ , _ , _ , As, _ , _ ) -> ES, GS, JS, BS Init the matrices for computing the defects of the stochastic states only. @@ -525,19 +528,20 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As + model::SimModel{NT}, ::TranscriptionMethod, ::Bool, + He, Â, _ , _ , _ , _ , As, _ , _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) - nx = nx̂ - nxs - nŵ = nx̂ - nw = nŵ - nxs + nx = nx̂ - nxs + nŵ = nx̂ + nŵd = 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̂) .+ nx̂*(j-1) ESx̂[iRow, iCol] = As end - ESŵ = repeatdiag([zeros(NT, nxs, nw) I], He) + ESŵ = repeatdiag([zeros(NT, nxs, nŵd) I], He) ES = [ESx̂ ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) @@ -548,7 +552,7 @@ end @doc raw""" init_defectmat_mhe( model::SimModel, transcription::OrthogonalCollocation, direct::Bool - He, Â, _ , _ , _ , _ , As + He, Â, _ , _ , _ , _ , As, Co, λo ) -> ES, GS, JS, BS Init the matrices for computing the continuity constraints and stochastic state defects. @@ -590,21 +594,25 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::OrthogonalCollocation, ::Bool, - He, Â, _ , _ , _ , _ , As + model::SimModel{NT}, transcription::OrthogonalCollocation, ::Bool, + He, Â, _ , _ , _ , _ , As, Co, λo ) where {NT<:Real} 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)] + nx = nx̂ - nxs + nk = get_nk(model, transcription) + λo_I = λo*I(nx) + ESx̂ = [zeros(NT, nx̂*He, nx̂) -I] for j=1:He - iRow = (1:nxs) .+ nxs*(j-1) - iCol = (nx+1:nx̂) .+ nx̂*(j-1) + iRow = (1:nx) .+ (j-1)*nx̂ + iCol = (1:nx) .+ (j-1)*nx̂ + ESx̂[iRow, iCol] = λo_I + iRow = (nx+1:nx̂) .+ (j-1)*nx̂ + iCol = (nx+1:nx̂) .+ (j-1)*nx̂ ESx̂[iRow, iCol] = As end - ESŵ = repeatdiag([zeros(NT, nxs, nw) I], He) - ES = [ESx̂ ESŵ] + ESk = repeatdiag([Co; zeros(NT, nxs, nk)], He) + ESŵ = I # will be different if nŵ ≠ nx̂ is implemented + ES = [ESx̂ ESk ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -613,7 +621,8 @@ end "Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModel` (N/A)." function init_defectmat_mhe( - model::SimModel{NT}, transcription::SingleShooting, ::Bool, He, Â, _ , _ , _ , _ , _ + model::SimModel{NT}, transcription::SingleShooting, ::Bool, + He, Â, _ , _ , _ , _ , _ , _ , _ ) where {NT<:Real} nx̂ = size(Â, 2) nŵ = nx̂ From a17e9779e978b5f0f8793cd0ec90d391e60f482f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 11 Aug 2026 17:06:42 -0400 Subject: [PATCH 21/40] doc: minor correction in `init_orthocolloc` --- src/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index 2fd2a5a6a..4ddcdbf2a 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -353,7 +353,7 @@ disturbances are either piecewise constant or linear: \mathbf{û}_i(k) &= \begin{cases} \mathbf{û_0}(k) & h = 0 \\ (1-τ_i)\mathbf{û_0}(k) + τ_i\mathbf{û_0}(k+1) & h = 1 \end{cases} \\ -\mathbf{d̂}_i(k) &= (1-τ_i)\mathbf{d̂_0}(k) + τ_i\mathbf{d̂_0}(k+1) +\mathbf{d}_i(k) &= (1-τ_i)\mathbf{d_0}(k) + τ_i\mathbf{d_0}(k+1) \end{aligned} ``` The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). From 74fc29e4f551a86fd7186c2295c83a620e42c904 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:10:00 -0400 Subject: [PATCH 22/40] =?UTF-8?q?added:=20`OrthogonalCollocation`=20for=20?= =?UTF-8?q?`MovingHorizonEstimator`=20IT'S=20ALIVE=20=F0=9F=8D=BE?= =?UTF-8?q?=F0=9F=A5=B3=F0=9F=8D=BE=F0=9F=A5=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 27 +++--- src/estimator/construct.jl | 8 +- src/estimator/mhe/transcription.jl | 142 +++++++++++++++++++++++++---- 3 files changed, 141 insertions(+), 36 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6b4ada585..f9d883bce 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1399,7 +1399,7 @@ function con_nonlinprogeq!( Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads - Ts, p = model.Ts, model.p + Ts = model.Ts nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] @@ -1412,12 +1412,12 @@ function con_nonlinprogeq!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] 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) @@ -1426,11 +1426,11 @@ function con_nonlinprogeq!( 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, d̂0next, p) + model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, model.p) else # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, p) + model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, model.p) end ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) end @@ -1462,13 +1462,11 @@ the model dynamics are computed by: \vdots \\ \mathbf{k̇}_{n_o}(k+j) \end{bmatrix} ``` -for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are -extracted from the decision variables in `Z̃`. The ``\mathbf{x̂_d}`` vectors are the -deterministic states extracted from ``\mathbf{X̂_0}`` also in `Z̃`, and they correspond to the -states at the beginning of the interval ``τ_0=0``. The ``\mathbf{k̇}_i`` vectors are -evaluated from the continuous-time function `model.f`, as described in [`init_orthocolloc`](@ref) -documentation. The defects for the continuity constraints and the stochastic states are -linear equality constraints (see [`init_defectmat`](@ref) for details). +for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` and +``\mathbf{x̂_d}(k+j)`` vectors are extracted from the decision variables in `Z̃`. The +``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as +described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the +stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). """ function con_nonlinprogeq!( geq, _ , Û0, K̇, @@ -1479,7 +1477,6 @@ function con_nonlinprogeq!( Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads - p = model.p Mo, no, τ = mpc.Mo, transcription.no, transcription.τ nk = get_nk(model, transcription) D̂0 = mpc.D̂0 @@ -1494,12 +1491,12 @@ function con_nonlinprogeq!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŝk = @views geq[(1 + nk*(j-1)):(nk*j)] # ----------------- collocation constraint defects ----------------------------- - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ @@ -1514,12 +1511,12 @@ function con_nonlinprogeq!( ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next if h < 1 - model.f!(k̇i, ki_Z̃, û0, d̂i, p) + model.f!(k̇i, ki_Z̃, û0, d̂i, model.p) else # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next - model.f!(k̇i, ki_Z̃, ûi, d̂i, p) + model.f!(k̇i, ki_Z̃, ûi, d̂i, model.p) end end ŝk .-= k̇ diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index e9286aae1..328593021 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -8,6 +8,7 @@ struct StateEstimatorBuffer{NT<:Real} Ŵ ::Vector{NT} X̂ ::Vector{NT} Ŷ ::Vector{NT} + D ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} R̂ ::Matrix{NT} @@ -35,7 +36,7 @@ function StateEstimatorBuffer{NT}( transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ) - nV̂, nŴ, nX̂, nŶ = nym*He, nŵ*He, nx̂*He, ny*He + 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) @@ -45,6 +46,7 @@ function StateEstimatorBuffer{NT}( Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) Ŷ = Vector{NT}(undef, nŶ) + D = Vector{NT}(undef, nD) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) R̂ = Matrix{NT}(undef, nym, nym) @@ -53,7 +55,9 @@ function StateEstimatorBuffer{NT}( ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, k, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) + return StateEstimatorBuffer{NT}( + u, û, k, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + ) end "Include all the covariance matrices for the Kalman filters and moving horizon estimator." diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f12a0042f..c24af7c86 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -5,11 +5,28 @@ get_nZ_mhe(::OrthogonalCollocation, He, nx̂, nk, nŵ) = nx̂ + nx̂*He + nk*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[] + nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, 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Ŵ)] + nx̃ = estim.nε + nx̂ + nx̃_nX̂_He = nx̃ + nx̂*estim.He + i_Z̃_NK = [ + (1):(nx̃ + nX̂); + (1 + nx̃_nX̂_He):(nx̃_nX̂_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 + nx̃ = estim.nε + nx̂ + nx̃_nX̂_He = nx̃ + nx̂*estim.He + nx̃_nX̂_nK_He = nx̃_nX̂_He + nk*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Ŵ); + ] return i_Z̃_NK end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::SingleShooting) @@ -610,7 +627,9 @@ function init_defectmat_mhe( iCol = (nx+1:nx̂) .+ (j-1)*nx̂ ESx̂[iRow, iCol] = As end + display(ESx̂) ESk = repeatdiag([Co; zeros(NT, nxs, nk)], He) + display(Matrix(ESk)) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented ES = [ESx̂ ESk ESŵ] GS = zeros(NT, nxs*He, model.nu*He) @@ -1007,9 +1026,6 @@ end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing - - - @doc raw""" set_warmstart_mhe!( estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var @@ -1247,7 +1263,7 @@ function predict_mhe!( d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] ĥ!(ŷ0next, estim, model, x̂0next, d0next) ŷ0nextm = @views ŷ0next[estim.i_ym] - if any(isnan, y0nextm) + if any(isnan, y0nextm) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0nextm = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0nextm, ŷ0nextm)] end v̂next .= y0nextm .- ŷ0nextm @@ -1299,15 +1315,15 @@ function predict_mhe!( else x̂0 = @views j < 2 ? x̂0arr[1:nx̂] : X̂0[(1+nx̂*(j-2)):(nx̂*(j-1))] end - d0 = @views estim.D0[(1+nd*j):(nd*(j+1))] # the 1st nd elements are not needed here + 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) ŷ0m = @views ŷ0[estim.i_ym] - if any(isnan, y0m) - y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] - end + if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 + y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] + end v̂ .= y0m .- ŷ0m end if Nk < estim.He # fill unused values with 0s for tracer sparsity detection: @@ -1455,10 +1471,10 @@ function con_nonlinprogeq_mhe!( ŵ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)] + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] f!(x̂dnext, k, model, x̂d_Z̃, û0, d0, model.p) x̂dnext .+= ŵd - sdnext .= @. x̂dnext - x̂dnext_Z̃ + ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end Nk < He && (geq[nx̂*Nk+1:end] .= 0) return geq @@ -1466,7 +1482,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̇, + geq, _ , Û0, K̇, estim::MovingHorizonEstimator, model::NonLinModel, ::TrapezoidalCollocation, x̂0arr, Ŵ, Z̃ ) -> geq @@ -1519,7 +1535,7 @@ function con_nonlinprogeq_mhe!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*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)] - sdnext = @views geq[(1 + nx*(j-1)):(nx*j)] + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] if f_threads || h < 1 || j < 2 @@ -1532,16 +1548,104 @@ function con_nonlinprogeq_mhe!( if h < 1 model.f!(k̇2, x̂dnext_Z̃, û0, d0next, model.p) else - # special case: û0(k+p) ≈ û0(k+p-1), since û0(k+p) is not available at k! + # 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) end - sdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) - sdnext .+= ŵd + ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + ŝdnext .+= ŵd end Nk < He && (geq[nx̂*Nk+1:end] .= 0) return geq end +@doc raw""" + con_nonlinprogeq_mhe!( + geq, _ , Û0, K̇, + estim::MovingHorizonEstimator, model::NonLinModel, ::OrthogonalCollocation, + x̂0arr, _ , Z̃ + ) -> geq + +Nonlinear MHE equality constrains for [`NonLinModel`](@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{M_o} \begin{bmatrix} + \mathbf{k}_1(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ + \mathbf{k}_2(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(ℓ+j) - \mathbf{x̂_d}(ℓ+j) \end{bmatrix} + - \begin{bmatrix} + \mathbf{k̇}_1(ℓ+j) \\ + \mathbf{k̇}_2(ℓ+j) \\ + \vdots \\ + \mathbf{k̇}_{n_o}(ℓ+j) \end{bmatrix} +``` +for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and +``\mathbf{x̂_d}(ℓ+j)`` vectors are extracted from the decision variables in `Z̃`. The +``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as +described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the +stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). +""" +function con_nonlinprogeq_mhe!( + geq, _ , Û0, K̇, + estim::MovingHorizonEstimator, model::NonLinModel, transcription::OrthogonalCollocation, + x̂0arr, _ , Z̃ +) + nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h + nx̂, He = estim.nx̂, estim.He + Nk = estim.Nk[] + f_threads = transcription.f_threads + Mo, no, τ = estim.Mo, transcription.no, transcription.τ + nk = get_nk(model, transcription) + nx̃ = estim.nε + nx̂ + p = estim.direct ? 0 : 1 + X̂0_Z̃, K_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)], Z̃[(nx̃+nx̂*He+1):(nx̃+nx̂*He+nk*He)] + Dtemp = estim.buffer.D + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + @threadsif f_threads for j=1:Nk + if j < 2 + x̂d_Z̃ = @views x̂0arr[1:nx] + else + 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))] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] + k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] + ŝk = @views geq[(1 + nk*(j-1)):(nk*j)] + d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] + # ----------------- collocation constraint defects ----------------------------- + Δk = k̇ + for i=1:no + Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ + end + mul!(ŝk, Mo, Δk) + di = @views Dtemp[(1 + nd*(j-1)):(nd*j)] + if h > 0 + ûi = similar(û0) # TODO: remove this allocation + end + for i=1:no + k̇i = @views k̇[(1 + (i-1)*nx):(i*nx)] + ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] + di .= (1-τ[i]).*d0 .+ τ[i].*d0next + if h < 1 + model.f!(k̇i, ki_Z̃, û0, di, model.p) + else + # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k + û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next + model.f!(k̇i, ki_Z̃, ûi, di, model.p) + end + end + ŝk .-= k̇ + end + Nk < He && (geq[nk*Nk+1:end] .= 0) + return geq +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 From 8f1288666432254f63c1316002eb2cc294599fcc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:15:36 -0400 Subject: [PATCH 23/40] debug: correct signature --- src/estimator/mhe/transcription.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index c24af7c86..de4639d95 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -652,7 +652,8 @@ function init_defectmat_mhe_empty( model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} nu, nd = model.nu, model.nd - nZ = get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) ES = zeros(NT, 0, nZ) GS = zeros(NT, 0, nu*He) JS = zeros(NT, 0, nd*(He+1)) From 4fd58fd1def907bfc2026f6f197533ad03df4e51 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:35:41 -0400 Subject: [PATCH 24/40] doc: moving optimization method at end of state estim internals --- docs/src/internals/state_estim.md | 38 +++++++++++++++---------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 798952d41..d6269e70e 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -69,25 +69,6 @@ ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! ``` -## Update Quadratic Optimization - -```@docs -ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel) -ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod) -``` - -## Solve Optimization Problem - -```@docs -ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) -ModelPredictiveControl.set_warmstart_mhe! -ModelPredictiveControl.disturbedinput! -ModelPredictiveControl.predict_mhe! -ModelPredictiveControl.con_nonlinprog_mhe! -ModelPredictiveControl.con_nonlinprogeq_mhe! -ModelPredictiveControl.getstate! -``` - ## Remove Operating Points ```@docs @@ -119,3 +100,22 @@ ModelPredictiveControl.correct_estimate! ```@docs ModelPredictiveControl.update_estimate! ``` + +## Update Quadratic Optimization + +```@docs +ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel) +ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod) +``` + +## Solve Optimization Problem + +```@docs +ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) +ModelPredictiveControl.set_warmstart_mhe! +ModelPredictiveControl.disturbedinput! +ModelPredictiveControl.predict_mhe! +ModelPredictiveControl.con_nonlinprog_mhe! +ModelPredictiveControl.con_nonlinprogeq_mhe! +ModelPredictiveControl.getstate! +``` From 4cd917fc3a237c2216ab4ff42d50cf7407a31016 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:39:13 -0400 Subject: [PATCH 25/40] =?UTF-8?q?doc:=20comment=20in=20`OrthogonalCollocat?= =?UTF-8?q?ion`=20internals=20on=20`w=CC=82`=20handling?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/transcription.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index de4639d95..1d4f7d982 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1589,7 +1589,8 @@ for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{x̂_d}(ℓ+j)`` vectors are extracted from the decision variables in `Z̃`. The ``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the -stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). +stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). The +estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( geq, _ , Û0, K̇, From 1d3aa9ee659fd840d76afff0600d59ef0a7be943 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:43:46 -0400 Subject: [PATCH 26/40] doc: clearer sentence --- 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 1d4f7d982..f54cb3a81 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -585,7 +585,7 @@ 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)), and by - updating the states by adding their process noise estimates ``\mathbf{ŵ}``, the defect + updating the states by adding the process noise estimates ``\mathbf{ŵ}``, the defect matrices are computed with: ```math \begin{aligned} From c2c9f53298ef4858ea3a688068048d7b99500fec Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 09:44:58 -0400 Subject: [PATCH 27/40] doc: minor addition --- src/transcription.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index 4ddcdbf2a..4b13727b3 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -254,7 +254,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`init_orthocolloc`](@ref) and [`con_nonlinprogeq!`](@ref) for details. + See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe](@ref) + for more details. """ struct OrthogonalCollocation <: CollocationMethod h::Int From 6d938a0c89af6430e4cad2094238de21e1f7241b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 10:25:15 -0400 Subject: [PATCH 28/40] debug: `Hp` -> `Nk` for the MHE --- src/estimator/mhe/transcription.jl | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f54cb3a81..9f823ef6d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -627,9 +627,7 @@ function init_defectmat_mhe( iCol = (nx+1:nx̂) .+ (j-1)*nx̂ ESx̂[iRow, iCol] = As end - display(ESx̂) ESk = repeatdiag([Co; zeros(NT, nxs, nk)], He) - display(Matrix(ESk)) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented ES = [ESx̂ ESk ESŵ] GS = zeros(NT, nxs*He, model.nu*He) @@ -1638,7 +1636,7 @@ function con_nonlinprogeq_mhe!( model.f!(k̇i, ki_Z̃, û0, di, model.p) else # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k - û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] + û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next model.f!(k̇i, ki_Z̃, ûi, di, model.p) end From ce05fba2dc9cf005ec6414ea1a32b5aa64ca3278 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 10:39:54 -0400 Subject: [PATCH 29/40] debug: correct `get_nZ_mhe` signature --- src/estimator/mhe/transcription.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 9f823ef6d..09c96e941 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -381,7 +381,8 @@ function init_predmat_mhe( ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) nŵ = nx̂ - nZ = get_nZ_mhe(transcription, He, nx̂, nŵ) + nk = get_nk(model, transcription) + nZ = get_nZ_mhe(transcription, He, nx̂, nk, nŵ) E = zeros(NT, 0, nZ) ex̄ = [-I zeros(NT, nx̂, nZ - nx̂)] EX̂ = zeros(NT, 0, nZ) From 254644109381b7a7d3117ffd570ca5736dd779dd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 10:40:28 -0400 Subject: [PATCH 30/40] added: pretty-print the number of collocation points --- src/estimator/mhe.jl | 2 +- src/predictive_control.jl | 2 +- src/transcription.jl | 7 ++++++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index ddf475e59..c94eccb68 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -8,7 +8,7 @@ get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε) "Print optimizer and other information for `MovingHorizonEstimator`." function print_details(io::IO, estim::MovingHorizonEstimator) println(io, "├ optimizer: $(JuMP.solver_name(estim.optim)) ") - println(io, "├ transcription: $(nameof(typeof(estim.transcription)))") + println(io, "├ transcription: $(transcription_str(estim.transcription))") print_backends(io, estim, estim.model) println(io, "├ arrival covariance: $(nameof(typeof(estim.covestim))) ") println(io, "├ direct: $(estim.direct)") diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 765e5b00a..d46aca2bc 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -37,7 +37,7 @@ function Base.show(io::IO, mpc::PredictiveController) println(io, "├ estimator: $(nameof(typeof(mpc.estim)))") println(io, "├ model: $(nameof(typeof(model)))") println(io, "├ optimizer: $(JuMP.solver_name(mpc.optim)) ") - println(io, "├ transcription: $(nameof(typeof(mpc.transcription)))") + println(io, "├ transcription: $(transcription_str(estim.transcription))") print_backends(io, mpc) println(io, "└ dimensions:") println(io, " │ ├$(lpad(Hp, n)) prediction steps Hp") diff --git a/src/transcription.jl b/src/transcription.jl index 4b13727b3..1c4a6e297 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -442,4 +442,9 @@ validate_transcription(::SimModel, ::TranscriptionMethod) = nothing "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 \ No newline at end of file +get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no + +transcription_str(transription::TranscriptionMethod) = string(nameof(typeof(transription))) +function transcription_str(transription::OrthogonalCollocation) + return "$(nameof(typeof(transription))) ($(transription.no) collocation points)" +end \ No newline at end of file From 21b1c27444663b555a4bfa204890560d731a805c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 11:27:06 -0400 Subject: [PATCH 31/40] debug: correct warm-starting for `OrthogonalCollocation` in MHE --- src/controller/transcription.jl | 4 +- src/estimator/mhe/transcription.jl | 99 +++++++++++++++++++++++++++--- 2 files changed, 94 insertions(+), 9 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f9d883bce..faaa53ece 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1010,7 +1010,7 @@ end @doc raw""" set_warmstart_mpc!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s -Set and return the warm-start value of `Z̃var` for [`OrthogonalCollocation`](@ref). +Do the same but for [`OrthogonalCollocation`](@ref). It warm-starts the solver at: ```math @@ -1066,7 +1066,7 @@ end @doc raw""" set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s -Set and return the warm-start value of `Z̃var` for other [`TranscriptionMethod`](@ref). +Do the same but for other [`TranscriptionMethod`](@ref). It warm-starts the solver at: ```math diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 09c96e941..d33d62b69 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1061,7 +1061,8 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu, nk = model.nu, model.nk + nu = model.nu + nk = get_nk(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ = nε + nx̂, nŵ*He Z̃s = estim.buffer.Z̃ @@ -1095,12 +1096,12 @@ end @doc raw""" set_warmstart_mhe!( - estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃var + estim::MovingHorizonEstimator, transcription::OrthogonalCollocation, Z̃var ) -> Z̃s -Do the same but for other transcription [`TranscriptionMethod`](@ref). +Do the same but for [`OrthogonalCollocation`](@ref). -If supported by `estim.optim`, it warm-starts the solver at: +It warm-starts the solver at: ```math \mathbf{Z̃_s} = \begin{bmatrix} @@ -1113,6 +1114,13 @@ If supported by `estim.optim`, it warm-starts the solver at: \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 \\ @@ -1124,14 +1132,91 @@ If supported by `estim.optim`, it warm-starts the solver at: ``` 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}}``. See the Extended Help of [`MultipleShooting`](@ref) for the defintion -of vectors ``\mathbf{0_x̂}`` and ``\mathbf{0_ŵ}``. +``\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) + 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 + Z̃s = estim.buffer.Z̃ + û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k + # --- 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)] + # --- 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̂ + 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 + 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) + Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) + if !isfinite(Js) + Z̃s[nx̃+nX̂+nK+1:end] .= 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 in Z̃ at 1. Since estim.Ŵ 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 + JuMP.set_start_value.(Z̃var, Z̃s) + return Z̃s +end + +@doc raw""" + set_warmstart_mhe!( + estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃var + ) -> Z̃s + +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} +``` """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu, nk = model.nu, model.nk + nu = model.nu + nk = get_nk(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He Z̃s = estim.buffer.Z̃ From f9ab2efb5cc32192fd0c6bee92322bfd7575549b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 12:50:08 -0400 Subject: [PATCH 32/40] removed: useless indexing --- 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 34f146bb3..2185d334a 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -474,7 +474,7 @@ function getŴ!(Ŵ, estim::MovingHorizonEstimator, transcription::Transcriptio 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ŵ) - Ŵ[1:nŵ*He] .= @views Z̃[(nZ̃ - nŵ*He + 1):end] + Ŵ .= @views Z̃[(nZ̃ - nŵ*He + 1):end] return Ŵ end From 8d5cf9d8947778faf596c2f046bb4d5816e37dc3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 12:52:03 -0400 Subject: [PATCH 33/40] removed: useless assigns in warm-starting --- src/estimator/mhe/transcription.jl | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index d33d62b69..eda5f11d9 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1083,11 +1083,11 @@ function set_warmstart_mhe!( predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+1:end] .= 0 + Z̃s[nx̃+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 in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # 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̃+nŵ*Nk+1:end] .= 1 JuMP.set_start_value.(Z̃var, Z̃s) @@ -1146,7 +1146,6 @@ function set_warmstart_mhe!( 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 Z̃s = estim.buffer.Z̃ - û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- @@ -1170,11 +1169,11 @@ function set_warmstart_mhe!( predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+nX̂+nK+1:end] .= 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 in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # 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 JuMP.set_start_value.(Z̃var, Z̃s) @@ -1220,7 +1219,6 @@ function set_warmstart_mhe!( nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He Z̃s = estim.buffer.Z̃ - û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- @@ -1241,11 +1239,11 @@ function set_warmstart_mhe!( predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+nX̂+1:end] .= 0 + Z̃s[nx̃+nX̂+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 in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # 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̂+nŵ*Nk+1:end] .= 1 JuMP.set_start_value.(Z̃var, Z̃s) From 08d45da0cb02268245320de005c3aef5ddf5c475 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 14:52:25 -0400 Subject: [PATCH 34/40] debug: correct `rethrow` signature --- 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 2185d334a..e9fc2ac2b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -589,7 +589,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real MOIU.reset_optimizer(optim) JuMP.optimize!(optim) else - rethrow(err) + rethrow() end end # -------- error handling ------------------------- From e0fb9000d807bcff5ef2296fc4b2a784df59b4bb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 15:13:10 -0400 Subject: [PATCH 35/40] debug: pretty-print transcription for MPCs --- src/predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/predictive_control.jl b/src/predictive_control.jl index d46aca2bc..981f6dd28 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -37,7 +37,7 @@ function Base.show(io::IO, mpc::PredictiveController) println(io, "├ estimator: $(nameof(typeof(mpc.estim)))") println(io, "├ model: $(nameof(typeof(model)))") println(io, "├ optimizer: $(JuMP.solver_name(mpc.optim)) ") - println(io, "├ transcription: $(transcription_str(estim.transcription))") + println(io, "├ transcription: $(transcription_str(mpc.transcription))") print_backends(io, mpc) println(io, "└ dimensions:") println(io, " │ ├$(lpad(Hp, n)) prediction steps Hp") From f4f97195ce71a0527ced3f03482aa02fbe1f2e1b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 15:50:06 -0400 Subject: [PATCH 36/40] doc: debug method name --- src/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index 1c4a6e297..7a0be3c0c 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -254,7 +254,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe](@ref) + See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe`](@ref) for more details. """ struct OrthogonalCollocation <: CollocationMethod From cdc2c275df88c4fc1f29a0412d61576c5f1ad981 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 16:41:52 -0400 Subject: [PATCH 37/40] added: new `fill0unused!` method for `OrthogonalCollocation` --- src/estimator/mhe/transcription.jl | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 53821b4ae..c92fa04f8 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1260,8 +1260,20 @@ end function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::TranscriptionMethod) nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃ = estim.nε + nx̂ - Z̃[(nx̃ + nx̂*Nk + 1):(nx̃ + nx̂*He)] .= 0 # unused decision variables after X̂0 vector - Z̃[(nx̃ + nx̂*He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nx̃_nX̂_He = nx̃ + nx̂*He + Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector + Z̃[(nx̃_nX̂_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + return nothing +end +function fill0unused!(Z̃, estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) + nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nx̃ = estim.nε + nx̂ + nk = get_nk(estim.model, transcription) + nx̃_nX̂_He = nx̃ + nx̂*He + nx̃_nX̂_nK_He = nx̃_nX̂_He + nk*He + Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector + Z̃[(nx̃_nX̂_He + nk*Nk + 1):(nx̃_nX̂_nK_He)] .= 0 # unused decision variables after K vector + Z̃[(nx̃_nX̂_nK_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector return nothing end From 772adad289b9c70b6f1100c8cd72c40120448d4b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 16:58:26 -0400 Subject: [PATCH 38/40] doc: debug method name --- src/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index 7a0be3c0c..ac82730ba 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -254,7 +254,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe`](@ref) + See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. """ struct OrthogonalCollocation <: CollocationMethod From 4245ce3eb345c0f5412c603beb0bf9e51ef6f785 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 19:17:39 -0400 Subject: [PATCH 39/40] test: MHE estimation tests with `OrthogonalColloction` --- Project.toml | 2 +- test/2_test_state_estim.jl | 51 +++++++++++++++++++++++++++----------- 2 files changed, 38 insertions(+), 15 deletions(-) diff --git a/Project.toml b/Project.toml index 268fc62f9..0edb5cb89 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.11.0" +version = "2.12.0" authors = ["Francis Gagnon"] [deps] diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 0b971315b..3c98ebc2c 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1174,13 +1174,13 @@ end updatestate!(mhe1, [11, 52], [50, 30], [5]) end preparestate!(mhe1, [50, 30], [5]) - @test mhe1([5]) ≈ [50, 30] atol=1e-3 + @test mhe1([5]) ≈ [50, 30] atol=5e-3 for i in 1:50 preparestate!(mhe1, [51, 32], [5]) updatestate!(mhe1, [10, 50], [51, 32], [5]) end preparestate!(mhe1, [51, 32], [5]) - @test mhe1([5]) ≈ [51, 32] atol=1e-3 + @test mhe1([5]) ≈ [51, 32] atol=5e-3 mhe1b = MovingHorizonEstimator(nonlinmodel, He=2, nint_u=[1, 1], nint_ym=0, direct=false) JuMP.set_attribute(mhe1b.optim, "tol", 1e-7) @@ -1210,12 +1210,12 @@ end preparestate!(mhe1c, [50, 30], [5]) updatestate!(mhe1c, [11, 52], [50, 30], [5]) end - @test mhe1c([5]) ≈ [50, 30] atol=1e-3 + @test mhe1c([5]) ≈ [50, 30] atol=5e-3 for i in 1:50 preparestate!(mhe1c, [51, 32], [5]) updatestate!(mhe1c, [10, 50], [51, 32], [5]) end - @test mhe1c([5]) ≈ [51, 32] atol=1e-3 + @test mhe1c([5]) ≈ [51, 32] atol=5e-3 gc!(gc, args...) = (gc[1] = 0) #for coverage only mhe2 = MovingHorizonEstimator( @@ -1227,7 +1227,7 @@ end updatestate!(mhe2, [11, 52], [50, 30], [5]) end preparestate!(mhe2, [50, 30], [5]) - @test mhe2([5]) ≈ [50, 30] atol=1e-3 + @test mhe2([5]) ≈ [50, 30] atol=5e-3 mhe3 = MovingHorizonEstimator( nonlinmodel, He=3, direct=false, transcription=MultipleShooting(f_threads=true) @@ -1237,7 +1237,7 @@ end updatestate!(mhe3, [11, 52], [50, 30], [5]) end preparestate!(mhe3, [50, 30], [5]) - @test mhe3([5]) ≈ [50, 30] atol=1e-3 + @test mhe3([5]) ≈ [50, 30] atol=5e-3 initstate!(mhe3, [10, 50], [50, 30], [5]) setstate!(mhe3, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) preparestate!(mhe3, [50, 30], [5]) @@ -1264,6 +1264,7 @@ end f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u h! = (y,x,_,_) -> y .= x nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) + transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) mhe6 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=false, transcription @@ -1273,7 +1274,7 @@ end updatestate!(mhe6, [-6], [13]) end preparestate!(mhe6, [13]) - @test mhe6() ≈ [13] atol=1e-3 + @test mhe6() ≈ [13] atol=5e-3 transcription = TrapezoidalCollocation(1) mhe7 = MovingHorizonEstimator( @@ -1284,7 +1285,7 @@ end updatestate!(mhe7, [-6], [13]) end preparestate!(mhe7, [13]) - @test mhe7() ≈ [13] atol=1e-3 + @test mhe7() ≈ [13] atol=5e-3 initstate!(mhe7, [0.0], [0.0]) setstate!(mhe7, [0.0, 0.0]) preparestate!(mhe7, [0.0]) @@ -1292,6 +1293,28 @@ end @test info[:Ŵ] ≈ [0.0, 0.0] @test info[:V̂] ≈ [0.0] + transcription = OrthogonalCollocation(f_threads=true, h_threads=true) + mhe10 = MovingHorizonEstimator( + nonlinmodel_c; He=3, direct=false, transcription + ) + for i in 1:50 + preparestate!(mhe10, [13]) + updatestate!(mhe10, [-6], [13]) + end + preparestate!(mhe10, [13]) + @test mhe10() ≈ [13] atol=5e-3 + + transcription = OrthogonalCollocation(1, 2, roots=:gausslegendre) + mhe11 = MovingHorizonEstimator( + nonlinmodel_c; He=3, direct=true, transcription + ) + for i in 1:50 + preparestate!(mhe11, [13]) + updatestate!(mhe11, [-6], [13]) + end + preparestate!(mhe11, [13]) + @test mhe11() ≈ [13] atol=5e-3 + # coverage of the branch with error termination status (with an infeasible problem): mhe_infeas = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf) mhe_infeas = setconstraint!(mhe_infeas, v̂min=[1, 1], v̂max=[-1, -1]) @@ -1302,9 +1325,9 @@ end ) # for coverage of NLP functions, the univariate syntax of JuMP.@operator - mhe6 = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf) - setconstraint!(mhe6, v̂min=[-51,-52], v̂max=[53,54]) - x̂ = preparestate!(mhe6, [50, 30], [5]) + mhe7 = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf) + setconstraint!(mhe7, v̂min=[-51,-52], v̂max=[53,54]) + x̂ = preparestate!(mhe7, [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6)) @@ -1315,12 +1338,12 @@ end coloring_algorithm=GreedyColoringAlgorithm(), ) - mhe7 = MovingHorizonEstimator(nonlinmodel; He=2, hessian) + mhe8 = MovingHorizonEstimator(nonlinmodel; He=2, hessian) @test_logs( (:warn, "NaN values in the MHE measurements ym: ignoring them in the objective"), - preparestate!(mhe7, [50, NaN], [5]) + preparestate!(mhe8, [50, NaN], [5]) ) - @test mhe7.x̂0 ≈ zeros(6) atol=1e-9 + @test mhe8.x̂0 ≈ zeros(6) atol=1e-9 end From 5c139831b70eacbc855bb3a17570bbddc5046cc2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Aug 2026 21:49:19 -0400 Subject: [PATCH 40/40] debug: correct signatures --- src/controller/transcription.jl | 2 +- src/estimator/mhe/execute.jl | 2 +- src/estimator/mhe/transcription.jl | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index faaa53ece..8e5225881 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1421,7 +1421,7 @@ function con_nonlinprogeq!( 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, d̂0, p) + model.f!(k̇1, x̂d_Z̃, û0, d̂0, model.p) else k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index e9fc2ac2b..ae3a22e02 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -985,7 +985,7 @@ function setmodel_estimator!( # --- defect matrices --- ES, GS, JS, BS = init_defectmat_mhe( model, transcription, estim.direct, He, - estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, estim.As + estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, estim.As, estim.Co, estim.λo ) Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true) con.ẼS .= ẼS diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index c92fa04f8..7ef21432d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -426,7 +426,7 @@ end @doc raw""" init_defectmat_mhe( model::LinModel, transcription::MultipleShooting, direct::Bool, - He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, _ , _ , _ + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, As, Co, λo ) -> ES, GS, JS, BS Init the matrices for computing the defects over the predicted states.