From ec6881760b53fea4b0f304b75403bbcf44f40f05 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 13 May 2026 10:57:34 -0400 Subject: [PATCH 01/56] changed: typo correction --- src/estimator/construct.jl | 2 +- src/general.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 8efbe17f6..2bbeda4d8 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -125,7 +125,7 @@ end """ validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) -Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices. +Validate sizes and Hermitianity of process `Q̂` and sensor `R̂` noises covariance matrices. Also validate initial estimate covariance `P̂_0`, if provided. """ diff --git a/src/general.jl b/src/general.jl index 0ef4f69b1..6329ec80b 100644 --- a/src/general.jl +++ b/src/general.jl @@ -211,7 +211,7 @@ to_hermitian(A) = A """ Compute the inverse of a the Hermitian positive definite matrix `A` in-place and return it. -There is 3 methods for this function: +There are 3 methods for this function: - If `A` is a `Hermitian{ for the source. From ff6a6afda05932c27476838894b8b595677d943a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 13 May 2026 11:28:50 -0400 Subject: [PATCH 02/56] test: verify inverted cov. for `setmodel!` on MHE --- src/estimator/construct.jl | 6 ++---- src/estimator/mhe/execute.jl | 20 ++++++++++++++++++-- test/2_test_state_estim.jl | 4 ++++ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 2bbeda4d8..255c6fe4d 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -95,6 +95,7 @@ struct KalmanCovariances{ rethrow() end end + invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L) try inv!(invR̂) catch err @@ -104,10 +105,7 @@ struct KalmanCovariances{ rethrow() end end - invQ̂_He = repeatdiag(invQ̂, He) - invR̂_He = repeatdiag(invR̂, He) - invQ̂_He = Hermitian(invQ̂_He, :L) - invR̂_He = Hermitian(invR̂_He, :L) + invR̂_He = Hermitian(repeatdiag(invR̂, He), :L) return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He) end end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d55696dce..3ff128fc9 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -952,14 +952,30 @@ function setmodel_estimator!( estim.cov.Q̂ .= to_hermitian(Q̂) invQ̂ = Hermitian(estim.buffer.Q̂, :L) invQ̂ .= estim.cov.Q̂ - inv!(invQ̂) + try + inv!(invQ̂) + catch err + if err isa PosDefException + error("Q̂ is not positive definite") + else + rethrow() + end + end estim.cov.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) end if !isnothing(R̂) estim.cov.R̂ .= to_hermitian(R̂) invR̂ = Hermitian(estim.buffer.R̂, :L) invR̂ .= estim.cov.R̂ - inv!(invR̂) + try + inv!(invR̂) + catch err + if err isa PosDefException + error("R̂ is not positive definite") + else + rethrow() + end + end estim.cov.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) end return nothing diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index f90cc9aa5..779435208 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1410,14 +1410,18 @@ end @test mhe.con.x̃0max ≈ [+1000 - 8.0] setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) @test mhe.cov.Q̂ ≈ [1e-3] + @test mhe.cov.invQ̂_He ≈ diagm(repeat([1e3], He)) @test mhe.cov.R̂ ≈ [1e-6] + @test mhe.cov.invR̂_He ≈ diagm(repeat([1e6], He)) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing) mhe2 = MovingHorizonEstimator(nonlinmodel; He, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) @test mhe2.cov.Q̂ ≈ [1e-3] + @test mhe2.cov.invQ̂_He ≈ diagm(repeat([1e3], He)) @test mhe2.cov.R̂ ≈ [1e-6] + @test mhe2.cov.invR̂_He ≈ diagm(repeat([1e6], He)) @test_throws ErrorException setmodel!(mhe2, deepcopy(nonlinmodel)) end From 42760bf4fbbafe7d0eae71c88d29bf54af884021 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 13 May 2026 12:47:29 -0400 Subject: [PATCH 03/56] test: negative matrices for MHE covs in `setmodel!` --- test/2_test_state_estim.jl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 779435208..b1dbabf00 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1423,6 +1423,8 @@ end @test mhe2.cov.R̂ ≈ [1e-6] @test mhe2.cov.invR̂_He ≈ diagm(repeat([1e6], He)) @test_throws ErrorException setmodel!(mhe2, deepcopy(nonlinmodel)) + @test_throws ErrorException setmodel!(mhe, Q̂=diagm([-0.1])) + @test_throws ErrorException setmodel!(mhe, R̂=diagm([-0.1])) end @testitem "MovingHorizonEstimator v.s. Kalman filters" setup=[SetupMPCtests] begin From 1fecaa1ef343b930e4a6919d8f89f802d1b5af32 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 13 May 2026 14:22:44 -0400 Subject: [PATCH 04/56] doc: wip `gc` function in MHE --- src/controller/nonlinmpc.jl | 2 +- src/estimator/mhe/construct.jl | 17 ++++++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index c8d97ff10..0266ff6e6 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -167,7 +167,7 @@ controller minimizes the following objective function at each discrete time ``k` ``` subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraints: ```math -\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0} +\mathbf{g_c}(\mathbf{U_e, Ŷ_e, D̂_e, p}, ϵ) ≤ \mathbf{0} ``` with the decision variables ``\mathbf{Z}`` and slack ``ϵ``. By default, a [`SingleShooting`](@ref) transcription method is used, hence ``\mathbf{Z=ΔU}``. The economic function ``J_E`` can diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 3408b5524..584850dfc 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -194,25 +194,28 @@ end Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). -It can handle constraints on the estimates, see [`setconstraint!`](@ref). Additionally, -`model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the probability -distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The computational -costs are drastically higher, however, since it minimizes the following objective function -at each discrete time ``k``: +It can handle constraints on the estimates. Additionally, `model` is not linearized like the +[`ExtendedKalmanFilter`](@ref), and the probability distribution is not approximated like +the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, +however, since it minimizes the following objective function at each discrete time ``k``: ```math \min_{\mathbf{x̂}_k(k-N_k+p), \mathbf{Ŵ}, ε} \mathbf{x̄}' \mathbf{P̄}^{-1} \mathbf{x̄} + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ} + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} + C ε^2 ``` -in which the arrival costs are evaluated from the states estimated at time ``k-N_k``: +subject to [`setconstraints!`](@ref) bounds, and the custom inequality constraints: +```math +\mathbf{g_c}(\mathbf{X̂, Ŵ, V̂, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} +``` +and in which the arrival costs are evaluated from the states estimated at time ``k-N_k``: ```math \begin{aligned} \mathbf{x̄} &= \mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂}_k(k-N_k+p) \\ \mathbf{P̄} &= \mathbf{P̂}_{k-N_k}(k-N_k+p) \end{aligned} ``` -and the covariances are repeated ``N_k`` times: +The covariances are repeated ``N_k`` times: ```math \begin{aligned} \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)} \\ From 716525bc0f10f454d3a29694a8527cd17b68bfa9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 15 May 2026 17:48:00 -0400 Subject: [PATCH 05/56] doc: MHE custom NL constraint argument table --- src/estimator/mhe/construct.jl | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index b3c6388b2..644167754 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -205,7 +205,7 @@ however, since it minimizes the following objective function at each discrete ti ``` subject to [`setconstraints!`](@ref) bounds, and the custom inequality constraints: ```math -\mathbf{g_c}(\mathbf{X̂, Ŵ, V̂, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} +\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} ``` and in which the arrival costs are evaluated from the states estimated at time ``k-N_k``: ```math @@ -230,9 +230,10 @@ N_k = \begin{cases} The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` respectively encompass the estimated process noises ``\mathbf{ŵ}(k-j+p)`` from ``j=N_k`` to ``1`` and sensor noises ``\mathbf{v̂}(k-j+1)`` from ``j=N_k`` to ``1``. The Extended Help defines the two vectors, the slack variable -``ε``, and the estimation of the covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If -the keyword argument `direct=true` (default value), the constant ``p=0`` in the equations -above, and the MHE is in the current form. Else ``p=1``, leading to the prediction form. +``ε``, the other vector arguments of the ``\mathbf{g_c}`` function, and the estimation of +the covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument +`direct=true` (default value), the constant ``p=0`` in the equations above, and the MHE is +in the current form. Else ``p=1``, leading to the prediction form. See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory @@ -338,6 +339,15 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: optimization by default, but it can be postponed to [`updatestate!`](@ref) with `direct=false`. + | VECTOR | DESCRIPTION | SIZE | FIRST TIME STEP | LAST TIME STEP | + | :--------------- | :--------------------------------------- | :-------------- | :------------------ | :------------- | + | ``\mathbf{X̂}`` | estimated states over the window | `(nx̂*(Nk+1),)` | ``k - N_k + 1 + p`` | ``k + p`` | + | ``\mathbf{V̂}`` | estimated sensor noises over the window | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | + | ``\mathbf{Ŵ}`` | estimated process noises over the window | `(nx̂*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | + | ``\mathbf{U}`` | manipulated inputs over the window | `(nu*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | + | ``\mathbf{Y^m}`` | measured outputs over the window | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | + | ``\mathbf{D}`` | measured disturbances over the window | `(nd*(Nk+1),)` | ``k - N_k`` | ``k`` | + The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref). From 3b2c844c084fe3c289b941ac8c96d0796d561e76 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 16 May 2026 11:53:20 -0400 Subject: [PATCH 06/56] added: `gc` argument in `MovingHorizonEstimator` --- src/controller/nonlinmpc.jl | 4 ++-- src/estimator/mhe/construct.jl | 26 ++++++++++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d061c7433..84f179b6a 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -222,8 +222,8 @@ This controller allocates memory at each time step for the optimization. - `JE=(_,_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``. - `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function - ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or - not (details in Extended Help). + ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e, D̂_e, p}, ϵ)``, mutating or not (details in + Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 644167754..e3f8021c1 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -119,6 +119,7 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, cov::KC, Cwt, + gc, nc, p, optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; direct=true ) where { @@ -203,7 +204,7 @@ however, since it minimizes the following objective function at each discrete ti + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} + C ε^2 ``` -subject to [`setconstraints!`](@ref) bounds, and the custom inequality constraints: +subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraints: ```math \mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} ``` @@ -271,6 +272,11 @@ transcription for now. - `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only. +- `gc=(_,_,_,_,_,_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function + ``\\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε)``, mutating or not (details in + Extended Help). +- `nc=0` : number of custom nonlinear inequality constraints. +- `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `optim=default_optim_mhe(model)` : a [`JuMP.Model`](@extref) object with a quadratic or nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). @@ -428,6 +434,10 @@ function MovingHorizonEstimator( sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, + gc!::Function = (_,_,_,_,_,_,_,_,_,_,_) -> nothing, + gc ::Function = gc!, + nc ::Int = 0, + p = model.p, optim::JM = default_optim_mhe(model), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, @@ -447,8 +457,8 @@ function MovingHorizonEstimator( R̂ = Diagonal([σR;].^2) isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; - direct, optim, gradient, jacobian, hessian + model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; + gc, gc!, nc, p, direct, optim, gradient, jacobian, hessian ) end @@ -458,6 +468,9 @@ default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_brid @doc raw""" MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; + gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing, + gc=gc!, + nc=0, optim=default_optim_mhe(model), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), @@ -479,6 +492,10 @@ of [`setstate!`](@ref) at the desired value. """ function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; + gc!::Function = (_,_,_,_,_,_,_,_,_,_,_) -> nothing, + gc ::Function = gc!, + nc = 0, + p = model.p, optim::JM = default_optim_mhe(model), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, @@ -492,7 +509,8 @@ function MovingHorizonEstimator( validate_covestim(cov, covestim) return MovingHorizonEstimator{NT}( model, - He, i_ym, nint_u, nint_ym, cov, Cwt, + He, i_ym, nint_u, nint_ym, cov, Cwt, + gc, nc, p, optim, gradient, jacobian, hessian, covestim; direct ) From 323592ef688b944487857f96d9dec5156231ea7c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 17 May 2026 10:44:28 -0400 Subject: [PATCH 07/56] doc: clearer `gc` arguments information for MHE --- src/controller/nonlinmpc.jl | 12 ++++---- src/estimator/mhe/construct.jl | 52 ++++++++++++++++++++-------------- 2 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 84f179b6a..dc839c343 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -276,13 +276,13 @@ NonLinMPC controller with a sample time Ts = 10.0 s: extended vectors ``\mathbf{U_e}``, ``\mathbf{Ŷ_e}`` and ``\mathbf{D̂_e}`` as arguments. They also receives the slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`. The following table details the vector sizes and the time steps of the first and last data - point in them: + point in them. - | VECTOR | SIZE | FIRST TIME STEP | LAST TIME STEP | - | :--------------- | :------------- | :-------------- | :------------- | - | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | + | :--------------- | :------------- | :----------- | :-----------| + | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e3f8021c1..0fdcce1b6 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -231,10 +231,10 @@ N_k = \begin{cases} The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` respectively encompass the estimated process noises ``\mathbf{ŵ}(k-j+p)`` from ``j=N_k`` to ``1`` and sensor noises ``\mathbf{v̂}(k-j+1)`` from ``j=N_k`` to ``1``. The Extended Help defines the two vectors, the slack variable -``ε``, the other vector arguments of the ``\mathbf{g_c}`` function, and the estimation of -the covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument -`direct=true` (default value), the constant ``p=0`` in the equations above, and the MHE is -in the current form. Else ``p=1``, leading to the prediction form. +``ε``, the other arguments of the ``\mathbf{g_c}`` function, and the estimation of the +covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument `direct=true` +(default value), the constant ``p=0`` in the equations above, and the MHE is in the current +form. Else ``p=1``, leading to the prediction form. See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory @@ -273,7 +273,7 @@ transcription for now. disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only. - `gc=(_,_,_,_,_,_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function - ``\\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε)``, mutating or not (details in + ``\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε)``, mutating or not (details in Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). @@ -343,16 +343,28 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: with ``p=1`` is particularly useful for the MHE since it moves its expensive computations after the MPC optimization. That is, [`preparestate!`](@ref) will solve the optimization by default, but it can be postponed to [`updatestate!`](@ref) with - `direct=false`. + `direct=false`. - | VECTOR | DESCRIPTION | SIZE | FIRST TIME STEP | LAST TIME STEP | - | :--------------- | :--------------------------------------- | :-------------- | :------------------ | :------------- | - | ``\mathbf{X̂}`` | estimated states over the window | `(nx̂*(Nk+1),)` | ``k - N_k + 1 + p`` | ``k + p`` | - | ``\mathbf{V̂}`` | estimated sensor noises over the window | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | - | ``\mathbf{Ŵ}`` | estimated process noises over the window | `(nx̂*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | - | ``\mathbf{U}`` | manipulated inputs over the window | `(nu*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | - | ``\mathbf{Y^m}`` | measured outputs over the window | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | - | ``\mathbf{D}`` | measured disturbances over the window | `(nd*(Nk+1),)` | ``k - N_k`` | ``k`` | + The argument ``\mathbf{p}`` in the ``\mathbf{g_c}`` function is a custom parameter + object of any type, but use a mutable one if you want to modify it later e.g.: a vector. + The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). + It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for + problems with two or more types of bounds, to ensure feasibility (e.g. on ``\mathbf{x̂}`` + and ``\mathbf{v̂}``). The following table details the other arguments of ``\mathbf{g_c}``, + including the time steps of the first and last sample in them. Note that the vectors + will grows with time until ``N_k = H_e`` is reached, and the windows don't start at the + same time step (a side-effect of the current form). + + | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | + | :--------------- | :-------------- | :-------------- | :-------------- | + | ``\mathbf{X̂}`` | `(nx̂*(Nk+1),)` | ``k - N_k + p`` | ``k + p`` | + | ``\mathbf{V̂}`` | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | + | ``\mathbf{Ŵ}`` | `(nx̂*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | + | ``\mathbf{U}`` | `(nu*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | + | ``\mathbf{Y^m}`` | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | + | ``\mathbf{D}`` | `(nd*(Nk+1),)` | ``k - N_k`` | ``k`` | + | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | + | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation @@ -411,14 +423,10 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: ) ``` that is, it will test many coloring orders at preparation and keep the best. - - The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). - It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for - problems with two or more types of bounds, to ensure feasibility (e.g. on the estimated - state ``\mathbf{x̂}`` and sensor noise ``\mathbf{v̂}``). Note that if `Cwt≠Inf`, the - attribute `nlp_scaling_max_gradient` of `Ipopt` is set to `10/Cwt` (if not already set), - to scale the small values of ``ε``. Use the second constructor to specify the arrival - covariance estimation method. + + Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to + `10/Cwt` (if not already set), to scale the small values of ``ε``. Use the second + constructor to specify the arrival covariance estimation method. """ function MovingHorizonEstimator( model::SM; From 2d210d2be3d0e989d03edbeea70dd1fc8b5fbd42 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 17 May 2026 11:03:22 -0400 Subject: [PATCH 08/56] changed: default to `Ipopt` if `nc>0` --- src/estimator/mhe/construct.jl | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 0fdcce1b6..453bca0d9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -204,7 +204,7 @@ however, since it minimizes the following objective function at each discrete ti + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} + C ε^2 ``` -subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraints: +subject to [`setconstraint!`](@ref) bounds, and the custom nonlinear inequality constraints: ```math \mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} ``` @@ -277,7 +277,7 @@ transcription for now. Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). -- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](@extref) object with a quadratic or +- `optim=default_optim_mhe(model,nc)` : a [`JuMP.Model`](@extref) object with a quadratic or nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective @@ -446,7 +446,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc ::Int = 0, p = model.p, - optim::JM = default_optim_mhe(model), + optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, hessian::Union{AbstractADType, Bool, Nothing} = false, @@ -470,8 +470,14 @@ function MovingHorizonEstimator( ) end -default_optim_mhe(::LinModel) = JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false) -default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false) +"Default optimizer for MHE, depending on the model and the number of custom NL constraints." +function default_optim_mhe(model::SimModel, nc) + if model isa LinModel && iszero(nc) + return JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false) + else + return JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false) + end +end @doc raw""" MovingHorizonEstimator( @@ -479,7 +485,7 @@ default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_brid gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing, gc=gc!, nc=0, - optim=default_optim_mhe(model), + optim=default_optim_mhe(model, nc), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), hessian=false, @@ -504,7 +510,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc = 0, p = model.p, - optim::JM = default_optim_mhe(model), + optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, hessian::Union{AbstractADType, Bool, Nothing} = false, From 8e7a6833525de237b6c28d0c7dd65bbfedddcbf6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 17 May 2026 14:54:04 -0400 Subject: [PATCH 09/56] wip : validation of `gc` function for MHE --- src/controller/nonlinmpc.jl | 25 ++++--- src/estimator/mhe/construct.jl | 126 +++++++++++++++++++++++++++++---- src/model/nonlinmodel.jl | 20 ++++-- 3 files changed, 140 insertions(+), 31 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index dc839c343..e187f69ae 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -121,7 +121,7 @@ struct NonLinMPC{ # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) + test_custom_function_mpc(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) Mo, Co, λo = init_orthocolloc(model, transcription) nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) @@ -442,7 +442,7 @@ function NonLinMPC( nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) validate_JE(NT, JE) - gc! = get_mutating_gc(NT, gc) + gc! = get_mutating_gc_mpc(NT, gc) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMPC_HESSIAN) return NonLinMPC{NT}( @@ -471,18 +471,23 @@ function validate_JE(NT, JE) end """ - validate_gc(NT, gc) -> ismutating + validate_gc_mpc(NT, gc) -> ismutating -Validate `gc` function argument signature and return `true` if it is mutating. +Validate `gc` function argument signature for MPC and return `true` if it is mutating. """ -function validate_gc(NT, gc) +function validate_gc_mpc(NT, gc) ismutating = hasmethod( gc, # LHS, Ue, Ŷe, D̂e, p, ϵ Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any, NT} ) + isnonmutating = hasmethod( + gc, + # Ue, Ŷe, D̂e, p, ϵ + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT} + ) # Ue, Ŷe, D̂e, p, ϵ - if !(ismutating || hasmethod(gc, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT})) + if !(ismutating || isnonmutating) error( "the custom constraint function has no method with type signature "* "gc(Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, D̂e::Vector{$(NT)}, p::Any, ϵ::$(NT)) "* @@ -494,8 +499,8 @@ function validate_gc(NT, gc) end "Get mutating custom constraint function `gc!` from the provided function in argument." -function get_mutating_gc(NT, gc) - ismutating_gc = validate_gc(NT, gc) +function get_mutating_gc_mpc(NT, gc) + ismutating_gc = validate_gc_mpc(NT, gc) gc! = if ismutating_gc gc else @@ -508,7 +513,7 @@ function get_mutating_gc(NT, gc) end """ - test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) + test_custom_function_mpc(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) Test the custom functions `JE` and `gc!` at the operating point `Uop`, `Yop`, `Dop`. @@ -516,7 +521,7 @@ This function is called at the end of `NonLinMPC` construction. It warns the use custom cost `JE` and constraint `gc!` functions crash at `model` operating points. This should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. """ -function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) +function test_custom_function_mpc(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) uop, dop, yop = model.uop, model.dop, model.yop Ue, Ŷe, D̂e = [Uop; uop], [yop; Yop], [dop; Dop] ϵ = zero(NT) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 453bca0d9..88d614647 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -17,12 +17,14 @@ the former is always a linear inequality constraint (it's a decision variable). `x̃min` and `x̃max` refer to the bounds at the arrival (augmented with the slack variable ε), and `X̂min` and `X̂max`, the others. """ -struct EstimatorConstraint{NT<:Real} +struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} + # matrices for the estimated state constraints: Ẽx̂ ::Matrix{NT} Fx̂ ::Vector{NT} Gx̂ ::Matrix{NT} Jx̂ ::Matrix{NT} Bx̂ ::Vector{NT} + # bounds over the estimation windows (deviation vectors from operating points): x̃0min ::Vector{NT} x̃0max ::Vector{NT} X̂0min ::Vector{NT} @@ -31,6 +33,7 @@ struct EstimatorConstraint{NT<:Real} Ŵmax ::Vector{NT} V̂min ::Vector{NT} V̂max ::Vector{NT} + # A matrcies for the linear inequality constraints: A_x̃min ::Matrix{NT} A_x̃max ::Matrix{NT} A_X̂min ::Matrix{NT} @@ -40,13 +43,20 @@ struct EstimatorConstraint{NT<:Real} A_V̂min ::Matrix{NT} A_V̂max ::Matrix{NT} A ::Matrix{NT} + # b vectir for the linear inequality constraints: b ::Vector{NT} + # constraint softness parameter vectors needing seperate strorage: C_x̂min ::Vector{NT} C_x̂max ::Vector{NT} C_v̂min ::Vector{NT} C_v̂max ::Vector{NT} + # indices of finite numbers in the b vector (linear inequality constraints): i_b ::BitVector + # indices of finite numbers in the g vectors (nonlinear inequality constraints): i_g ::BitVector + # custom nonlinear inequality constraints: + gc! ::GCfunc + nc ::Int end struct MovingHorizonEstimator{ @@ -57,6 +67,7 @@ struct MovingHorizonEstimator{ GB<:AbstractADType, JB<:AbstractADType, HB<:Union{AbstractADType, Nothing}, + GCfunc<:Function, CE<:KalmanEstimator, } <: StateEstimator{NT} model::SM @@ -64,7 +75,7 @@ struct MovingHorizonEstimator{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - con::EstimatorConstraint{NT} + con::EstimatorConstraint{NT, GCfunc} gradient::GB jacobian::JB hessian::HB @@ -119,7 +130,7 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, cov::KC, Cwt, - gc, nc, p, + gc!::GCfunc, nc, p, optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; direct=true ) where { @@ -130,6 +141,7 @@ struct MovingHorizonEstimator{ GB<:AbstractADType, JB<:AbstractADType, HB<:Union{AbstractADType, Nothing}, + GCfunc<:Function, CE<:KalmanEstimator{NT} } nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk @@ -143,14 +155,13 @@ struct MovingHorizonEstimator{ Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - r = direct ? 0 : 1 E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r + model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) # dummy values (updated just before optimization): F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He) con, nε, Ẽ, ẽx̄ = init_defaultcon_mhe( - model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂ + model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, gc!, nc ) nZ̃ = size(Ẽ, 2) # dummy values, updated before optimization: @@ -165,7 +176,7 @@ struct MovingHorizonEstimator{ P̂arr_old = copy(cov.P̂_0) Nk = [0] corrected = [false] - estim = new{NT, SM, KC, JM, GB, JB, HB, CE}( + estim = new{NT, SM, KC, JM, GB, JB, HB, GCfunc, CE}( model, cov, optim, con, @@ -204,7 +215,7 @@ however, since it minimizes the following objective function at each discrete ti + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} + C ε^2 ``` -subject to [`setconstraint!`](@ref) bounds, and the custom nonlinear inequality constraints: +subject to [`setconstraint!`](@ref) bounds and the custom nonlinear inequality constraints: ```math \mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} ``` @@ -519,6 +530,7 @@ function MovingHorizonEstimator( ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He) + gc! = get_mutating_gc_mhe(NT, gc) hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMHE_HESSIAN) validate_covestim(cov, covestim) return MovingHorizonEstimator{NT}( @@ -549,6 +561,86 @@ function validate_covestim(::KalmanCovariances, ::StateEstimator) "ExtendedKalmanFilter or UnscentedKalmanFilter") end +""" + validate_gc_mhe(NT, gc) -> ismutating + +Validate `gc` function argument signature for MHE and return `true` if it is mutating. +""" +function validate_gc_mhe(NT, gc) + ismutating = hasmethod( + gc, + Tuple{ + # LHS, , X̂ V̂ , Ŵ, + Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, + # U , Ym , D , P̄ , x̄ , p , ε + Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT + } + ) + isnonmutating = hasmethod( + gc, + Tuple{ + # X̂ V̂ , Ŵ, + Vector{NT}, Vector{NT}, Vector{NT}, + # U , Ym , D , P̄ , x̄ , p , ε + Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT + } + ) + if !(ismutating || isnonmutating) + error( + "the custom constraint function has no method with type signature "* + "gc(X̂::Vector{$(NT)}, V̂::Vector{$(NT)}, Ŵ::Vector{$(NT)}, "* + "U::Vector{$(NT)}, Ym::Vector{$(NT)}, D::Vector{$(NT)}, "* + "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT)) "* + "or mutating form gc!(LHS::Vector{$(NT)}, "* + "X̂::Vector{$(NT)}, V̂::Vector{$(NT)}, Ŵ::Vector{$(NT)}, "* + "U::Vector{$(NT)}, Ym::Vector{$(NT)}, D::Vector{$(NT)}, "* + "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT))" + ) + end + return ismutating +end + +"Get mutating custom constraint function `gc!` from the provided function in argument." +function get_mutating_gc_mhe(NT, gc) + ismutating_gc = validate_gc_mhe(NT, gc) + gc! = if ismutating_gc + gc + else + function gc!(LHS, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ϵ) + LHS .= gc(X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ϵ) + return nothing + end + end + return gc! +end + +""" + test_custom_function_mhe(NT, model::SimModel, gc!, nc, X̂op, Ymop, Uop, Dop, p) -> nothing + +Test the custom functions `gc!` at the operating points + +This function is called at the end of `MovingHorizonEstimator` construction. It warns the +user if the custom constraint `gc!` functions crash at `model` operating points. This +should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. +""" +function test_custom_function_mhe(NT, model::SimModel, gc!, nc, X̂op, Ymop, Uop, Dop, p) + uop, dop, yop = model.uop, model.dop, model.yop + gc = Vector{NT}(undef, nc) + try + gc!(gc, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, 0.0) + catch err + @warn( + """ + Calling the gc function with Ue, Ŷe, D̂e, ϵ arguments fixed at uop=$uop, + yop=$yop, dop=$dop, ϵ=0 failed with the following stacktrace. Did you + forget to set the keyword argument p or nc? + """, + exception=(err, catch_backtrace()) + ) + end + return nothing +end + @doc raw""" setconstraint!(estim::MovingHorizonEstimator; ) -> estim @@ -871,8 +963,9 @@ end Also return `Ẽ` and `ẽx̄` matrices for the the augmented decision vector `Z̃`. """ function init_defaultcon_mhe( - model::SimModel{NT}, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂ -) where {NT<:Real} + model::SimModel{NT}, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, + gc!::GCfunc = nothing, nc = 0 +) where {NT<:Real, GCfunc<:Union{Nothing, Function}} nŵ = nx̂ nZ̃, nX̂, nŴ, nYm = nx̂+nŵ*He, nx̂*He, nŵ*He, nym*He nε = isinf(C) ? 0 : 1 @@ -897,13 +990,14 @@ function init_defaultcon_mhe( A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max ) b = zeros(NT, size(A, 1)) # dummy b vector (updated just before optimization) - con = EstimatorConstraint{NT}( + con = EstimatorConstraint{NT, GCfunc}( Ẽx̂, Fx̂, Gx̂, Jx̂, Bx̂, x̃min, x̃max, X̂min, X̂max, Ŵmin, Ŵmax, V̂min, V̂max, A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, A, b, C_x̂min, C_x̂max, C_v̂min, C_v̂max, - i_b, i_g + i_b, i_g, + gc!, nc ) return con, nε, Ẽ, ẽx̄ end @@ -1065,7 +1159,7 @@ end @doc raw""" init_predmat_mhe( - model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, p + model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ Construct the [`MovingHorizonEstimator`](@ref) prediction matrices for [`LinModel`](@ref) `model`. @@ -1194,11 +1288,12 @@ 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}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, p + model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) where {NT<:Real} nu, nd = model.nu, model.nd nym, nx̂ = length(i_ym), size(Â, 2) nŵ = nx̂ + p = direct ? 0 : 1 # --- pre-compute matrix powers --- # Apow3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He Âpow3D = Array{NT}(undef, nx̂, nx̂, He+1) @@ -1305,10 +1400,11 @@ end "Return empty matrices if `model` is not a [`LinModel`](@ref), except for `ex̄`." function init_predmat_mhe( - model::SimModel{NT}, He, i_ym, Â, _ , _ , _ , _ , _ , _ , p + model::SimModel{NT}, He, i_ym, Â, _ , _ , _ , _ , _ , _ , direct ) where {NT<:Real} nym, nx̂ = length(i_ym), size(Â, 2) nŵ = nx̂ + p = direct ? 0 : 1 E = zeros(NT, 0, nx̂ + nŵ*He) ex̄ = [-I zeros(NT, nx̂, nŵ*He)] Ex̂ = zeros(NT, 0, nx̂ + nŵ*He) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index d1c68d0f4..282166199 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -249,11 +249,15 @@ Validate `f` function argument signature and return `true` if it is mutating. function validate_f(NT, f) ismutating = hasmethod( f, - # ẋ or xnext, x, u, d, p + # ẋ or xnext, x , u , d , p Tuple{ Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any} ) - # x, u, d, p - if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any})) + isnonmutating = hasmethod( + f, + # x, u , d , p + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) error( "the state function has no method with type signature "* "f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or "* @@ -272,11 +276,15 @@ Validate `h` function argument signature and return `true` if it is mutating. function validate_h(NT, h) ismutating = hasmethod( h, - # y, x, d, p + # y , x , d , p Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any} ) - # x, d, p - if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Any})) + isnonmutating = hasmethod( + h, + # x , d , p + Tuple{Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) error( "the output function has no method with type signature "* "h(x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or mutating form "* From 928445d3a281140392732a6f5e1db70d99744a73 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 12:00:31 -0400 Subject: [PATCH 10/56] added: windows with op. points in MHE This is needed for the custom NL constraints. --- src/estimator/mhe/construct.jl | 35 +++++++++++++++++++++++++++++----- src/estimator/mhe/execute.jl | 4 ++++ 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 88d614647..cb8d8b668 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -115,11 +115,17 @@ struct MovingHorizonEstimator{ q̃::Vector{NT} r::Vector{NT} C::NT - X̂op::Vector{NT} + X̂op ::Vector{NT} + Uop ::Vector{NT} + Yopm::Vector{NT} + Dop ::Vector{NT} X̂0 ::Vector{NT} Y0m::Vector{NT} + Ym ::Vector{NT} U0 ::Vector{NT} + U ::Vector{NT} D0 ::Vector{NT} + D ::Vector{NT} Ŵ ::Vector{NT} x̂0arr_old::Vector{NT} P̂arr_old ::Hermitian{NT, Matrix{NT}} @@ -167,9 +173,14 @@ struct MovingHorizonEstimator{ # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) - X̂op = repeat(x̂op, He) - X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He) - U0, D0 = zeros(NT, nu*He), zeros(NT, nd*(He+1)) + X̂op = repeat(x̂op, He) + Uop = repeat(model.uop, He) + Yopm = repeat(model.yop[i_ym], He) + Dop = repeat(model.dop, He+1) + X̂0 = zeros(NT, nx̂*He) + Y0m, Ym = zeros(NT, nym*He), zeros(NT, nym*He) + U0, U = zeros(NT, nu*He), zeros(NT, nu*He) + D0, D = zeros(NT, nd*(He+1)), zeros(NT, nd*(He+1)) Ŵ = zeros(NT, nx̂*He) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) x̂0arr_old = zeros(NT, nx̂) @@ -190,7 +201,8 @@ struct MovingHorizonEstimator{ Ẽ, F, G, J, B, ẽx̄, fx̄, H̃, q̃, r, Cwt, - X̂op, X̂0, Y0m, U0, D0, Ŵ, + X̂op, Uop, Yopm, Dop, + X̂0, Y0m, Ym, U0, U, D0, D, Ŵ, x̂0arr_old, P̂arr_old, Nk, direct, corrected, buffer @@ -377,6 +389,19 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | + If `LHS` represents the result of the left-hand side in the inequality + ``\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0}``, + the function `gc` can be implemented in two possible ways: + + 1. **Non-mutating function** (out-of-place): define it as `gc(X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, + p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. + 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, + p, ε) -> nothing`. This syntax reduces the allocations and potentially the + computational burden as well. + + The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for + the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). + The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref). diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 10d9eb971..c377fa471 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -326,6 +326,10 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ end estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] + # data windows including operating points, needed for custom NL constraints: + estim.U .= estim.U0 .+ estim.Uop + estim.Ym .= estim.Y0m .+ estim.Yopm + estim.D .= estim.D0 .+ estim.Dop return ismoving end From 883f2c1b970915dc190b2ccf2a4c6d073908f1f0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 12:01:25 -0400 Subject: [PATCH 11/56] doc: shorter custom NL constraints in `NonLinMPC` --- src/controller/nonlinmpc.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e187f69ae..e47a5fdf6 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -290,8 +290,8 @@ NonLinMPC controller with a sample time Ts = 10.0 s: are the current state estimator output and measured disturbance, respectively, and ``\mathbf{Ŷ}`` and ``\mathbf{D̂}``, their respective predictions from ``k+1`` to ``k+H_p``. If `LHS` represents the result of the left-hand side in the inequality - ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0}``, - the function `gc` can be implemented in two possible ways: + ``\mathbf{g_c}(\mathbf{U_e, Ŷ_e, D̂_e, p}, ϵ) ≤ \mathbf{0}``, the function `gc` can be + implemented in two possible ways: 1. **Non-mutating function** (out-of-place): define it as `gc(Ue, Ŷe, D̂e, p, ϵ) -> LHS`. This syntax is simple and intuitive but it allocates more memory. From 4ae4eb5859c79231f1e6bd18a6a510e582d48150 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 13:28:20 -0400 Subject: [PATCH 12/56] doc: organize the MHE extended Help --- src/estimator/mhe/construct.jl | 36 +++++++++++++++++----------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index cb8d8b668..abbadddc2 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -366,17 +366,24 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: with ``p=1`` is particularly useful for the MHE since it moves its expensive computations after the MPC optimization. That is, [`preparestate!`](@ref) will solve the optimization by default, but it can be postponed to [`updatestate!`](@ref) with - `direct=false`. + `direct=false`. + + The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances + and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation + scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref). + Note that the constructor does not validate the observability of the resulting augmented + [`NonLinModel`](@ref). In such cases, it is the user's responsibility to ensure that it + is still observable. The argument ``\mathbf{p}`` in the ``\mathbf{g_c}`` function is a custom parameter object of any type, but use a mutable one if you want to modify it later e.g.: a vector. The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). - It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for - problems with two or more types of bounds, to ensure feasibility (e.g. on ``\mathbf{x̂}`` - and ``\mathbf{v̂}``). The following table details the other arguments of ``\mathbf{g_c}``, - including the time steps of the first and last sample in them. Note that the vectors - will grows with time until ``N_k = H_e`` is reached, and the windows don't start at the - same time step (a side-effect of the current form). + It is disabled thus always zero by default for the MHE (from `Cwt=Inf`) but it should be + activated for problems with two or more types of bounds, to ensure feasibility (e.g. on + ``\mathbf{x̂}`` and ``\mathbf{v̂}``). The following table details the other arguments of + ``\mathbf{g_c}``, including the time steps of the first and last sample in them. Note + that the vectors will grows with time until ``N_k = H_e`` is reached, and the windows + don't start at the same time step (a side-effect of the current form). | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | | :--------------- | :-------------- | :-------------- | :-------------- | @@ -393,21 +400,14 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: ``\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0}``, the function `gc` can be implemented in two possible ways: - 1. **Non-mutating function** (out-of-place): define it as `gc(X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, - p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. - 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, + 1. **Non-mutating function** (out-of-place): define it as `gc(X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, + p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. + 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ε) -> nothing`. This syntax reduces the allocations and potentially the computational burden as well. The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for - the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). - - The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances - and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation - scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref). - Note that the constructor does not validate the observability of the resulting augmented - [`NonLinModel`](@ref). In such cases, it is the user's responsibility to ensure that it - is still observable. + the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). The estimation covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)`` gives an uncertainty on the state estimate at the beginning of the window ``k-N_k+p``, that is, in the past. From 4e03edcbeb649d9a8cec251c7f83365a64aa848b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 14:08:06 -0400 Subject: [PATCH 13/56] added: including `gc` in `mhe.con.i_g` field --- src/estimator/mhe/construct.jl | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index abbadddc2..da5a3654f 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -893,7 +893,8 @@ function setconstraint!( i_Ŵmin, i_Ŵmax = .!isinf.(con.Ŵmin), .!isinf.(con.Ŵmax) i_V̂min, i_V̂max = .!isinf.(con.V̂min), .!isinf.(con.V̂max) if notSolvedYet - con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mhe(model, + con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mhe( + model, con.nc, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, con.A_x̃min, con.A_x̃max, con.A_X̂min, con.A_X̂max, con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max @@ -930,7 +931,8 @@ function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel) end @doc raw""" - init_matconstraint_mhe(model::LinModel, + init_matconstraint_mhe( + model::LinModel, nc::Int, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args... ) -> i_b, i_g, A @@ -943,17 +945,19 @@ The linear and nonlinear inequality constraints are respectively defined as: \mathbf{g(Z̃)} &≤ \mathbf{0} \end{aligned} ``` -`i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are finite numbers. -`i_g` is a similar vector but for the indices of ``\mathbf{g}`` (empty if `model` is a -[`LinModel`](@ref)). The method also returns the ``\mathbf{A}`` matrix if `args` is -provided. In such a case, `args` needs to contain all the inequality constraint matrices: -`A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max`. +The argument `nc` is the number of custom nonlinear inequality constraints in +``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are +finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}`` (empty if +`model` is a [`LinModel`](@ref)). The method also returns the ``\mathbf{A}`` matrix if +`args` is provided. In such a case, `args` needs to contain all the inequality constraint +matrices: `A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max`. """ -function init_matconstraint_mhe(::LinModel{NT}, +function init_matconstraint_mhe( + ::LinModel{NT}, nc::Int, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args... ) where {NT<:Real} i_b = [i_x̃min; i_x̃max; i_X̂min; i_X̂max; i_Ŵmin; i_Ŵmax; i_V̂min; i_V̂max] - i_g = BitVector() + i_g = trues(nc) if isempty(args) A = zeros(NT, length(i_b), 0) else @@ -964,11 +968,12 @@ function init_matconstraint_mhe(::LinModel{NT}, end "Init `i_b, A` without state and sensor noise constraints if `model` is not a [`LinModel`](@ref)." -function init_matconstraint_mhe(::SimModel{NT}, +function init_matconstraint_mhe( + ::SimModel{NT}, nc::Int, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args... ) where {NT<:Real} i_b = [i_x̃min; i_x̃max; i_Ŵmin; i_Ŵmax] - i_g = [i_X̂min; i_X̂max; i_V̂min; i_V̂max] + i_g = [i_X̂min; i_X̂max; i_V̂min; i_V̂max; trues(nc)] if isempty(args) A = zeros(NT, length(i_b), 0) else @@ -1010,7 +1015,8 @@ function init_defaultcon_mhe( i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max) i_Ŵmin, i_Ŵmax = .!isinf.(Ŵmin), .!isinf.(Ŵmax) i_V̂min, i_V̂max = .!isinf.(V̂min), .!isinf.(V̂max) - i_b, i_g, A = init_matconstraint_mhe(model, + i_b, i_g, A = init_matconstraint_mhe( + model, nc, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max ) From 4942411d86cc607c1adc010e7001b0a1a8097575 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 14:18:54 -0400 Subject: [PATCH 14/56] changed: more precise error for `setconstraint!` of MHE --- src/estimator/mhe/construct.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index da5a3654f..9d06f2484 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -907,11 +907,12 @@ function setconstraint!( @constraint(optim, linconstraint, A*Z̃var .≤ b) reset_nonlincon!(estim, model) else - i_b, i_g = init_matconstraint_mhe(model, + i_b, i_g = init_matconstraint_mhe( + model, con.nc, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max ) if i_b ≠ con.i_b || i_g ≠ con.i_g - error("Cannot modify ±Inf constraints after calling updatestate!") + error("Cannot modify ±Inf constraints after first solve of estimation problem") end end return estim From e39e60121b2f35831826674781121f12bbf70cda Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 16:25:56 -0400 Subject: [PATCH 15/56] changed : minor improvement in `NonLinMPC` --- src/controller/nonlinmpc.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e47a5fdf6..e6a4b1f16 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -931,7 +931,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K::Vector{JNT} = zeros(JNT, nK) + K::Vector{JNT} = zeros(JNT, nK) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) @@ -1119,7 +1119,7 @@ end Evaluate the custom inequality constraint `gc` in-place and return it. """ function con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) - mpc.con.nc ≠ 0 && mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) + mpc.con.nc > 0 && mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) return gc end From 1377cc4f623f62bc01f4c9303c96d2c98967c92f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 18 May 2026 17:39:01 -0400 Subject: [PATCH 16/56] wip: custom nl constraint for MHE --- src/controller/execute.jl | 2 +- src/controller/nonlinmpc.jl | 2 +- src/estimator/mhe/construct.jl | 4 +- src/estimator/mhe/execute.jl | 75 +++++++++++++++++++++------------- 4 files changed, 51 insertions(+), 32 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 9440f93ba..b10927c5d 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -206,7 +206,7 @@ The argument `Z̃orΔŨ` can be the augmented decision vector ``\mathbf{Z̃}`` input increment vector ``\mathbf{ΔŨ}``, it works with both. """ function getϵ(mpc::PredictiveController, Z̃orΔŨ::AbstractVector{NT}) where NT<:Real - return mpc.nϵ ≠ 0 ? Z̃orΔŨ[end] : zero(NT) + return mpc.nϵ > 0 ? Z̃orΔŨ[end] : zero(NT) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e6a4b1f16..04756fa33 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1086,7 +1086,7 @@ function update_predictions!( ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) - ϵ = getϵ(mpc, Z̃) + ϵ = getϵ(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) geq = con_nonlinprogeq!(geq, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 9d06f2484..99b6b5fe6 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1634,13 +1634,15 @@ function get_nonlincon_oracle( He = estim.He i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(con.i_g), sum(con.i_g) + nc = con.nc nV̂, nX̂, nZ̃ = He*nym, He*nx̂, length(estim.Z̃) strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - g::Vector{JNT}, gi::Vector{JNT} = zeros(JNT, ng), zeros(JNT, ngi) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) # -------------- inequality constraint: nonlinear oracle ------------------------- function gi!(gi, Z̃, V̂, X̂0, û0, k, ŷ0, g) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c377fa471..3e24e858f 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -148,7 +148,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŵ] = estim.Ŵ[1:Nk*nŵ] info[:x̂arr] = x̂0arr + estim.x̂op info[:ε] = nε ≠ 0 ? estim.Z̃[begin] : zero(NT) - info[:J] = obj_nonlinprog!(x̄, estim, estim.model, V̂, estim.Z̃) + info[:J] = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Z̃) info[:X̂] = X̂0 .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂ @@ -204,7 +204,7 @@ function addinfo!( ) function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + return obj_nonlinprog(estim, model, x̄, V̂, Z̃) end if !isnothing(hess) prep_∇²J = prepare_hessian(J!, hess, estim.Z̃, J_cache...) @@ -282,7 +282,7 @@ addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info Get the slack `ε` from the decision vector `Z̃` if present, otherwise return 0. """ function getε(estim::MovingHorizonEstimator, Z̃::AbstractVector{NT}) where NT<:Real - return estim.nε ≠ 0 ? Z̃[begin] : zero(NT) + return estim.nε > 0 ? Z̃[begin] : zero(NT) end """ @@ -377,8 +377,8 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) # --- update H̃, q̃ and p vectors for quadratic optimization --- ẼZ̃ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]] FZ̃ = @views [estim.fx̄; estim.F[1:nYm]] - invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) - invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) + invQ̂_Nk = trunc_cov(invQ̂_He, estim.nx̂, Nk, estim.He) + invR̂_Nk = trunc_cov(invR̂_He, estim.nym, Nk, estim.He) M_Nk = [invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] Ñ_Nk = [fill(C, nε, nε) zeros(nε, nx̂+nŴ); zeros(nx̂, nε+nx̂+nŴ); zeros(nŴ, nε+nx̂) invQ̂_Nk] M_Nk_ẼZ̃ = M_Nk*ẼZ̃ @@ -576,7 +576,7 @@ function set_warmstart_mhe!(estim::MovingHorizonEstimator{NT}, Z̃var) where NT< # verify definiteness of objective function: V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s) - Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) + Js = obj_nonlinprog(estim, model, x̄, V̂, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] = 0 end @@ -680,43 +680,36 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) end """ - obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃) Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinModel`](@ref). It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective -function at specific `Z̃` and `V̂` values. +function at specific `Z̃`. """ -function obj_nonlinprog!( - _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃::AbstractVector{NT} -) where NT<:Real +function obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃) return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end """ - obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, model::SimModel, V̂, Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Z̃) Objective function of the MHE when `model` is not a [`LinModel`](@ref). -The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates -`x̄` vector arguments. +The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ -function obj_nonlinprog!( - x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃::AbstractVector{NT} +function obj_nonlinprog( + estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Z̃::AbstractVector{NT} ) where NT<:Real nε, Nk = estim.nε, estim.Nk[] nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.cov.invP̄ nx̃ = nε + nx̂ invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) - x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm] - x̄ .= estim.x̂0arr_old .- x̂0arr - Jε = nε ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT) - return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jε + Ŵ, V̂, ε = @views Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm], getε(estim, Z̃) + return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + estim.C*ε^2 end - - @doc raw""" predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 @@ -791,26 +784,50 @@ end """ - update_predictions!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + update_predictions!( + V̂, X̂0, û0, k, ŷ0, x̄, gc, g, + estim::MovingHorizonEstimator, Z̃ + ) -> nothing Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) +function update_prediction!(V̂, X̂0, û0, k, ŷ0, Ŵ, x̄, gc, g, estim::MovingHorizonEstimator, Z̃) model = estim.model + nx̃ = estim.nε + estim.nx̂ V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃) - ε = getε(estim, Z̃) - g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ε) + x̂0arr = @views Z̃[nx̃-nx̂+1:nx̃] + x̄ .= estim.x̂0arr_old .- x̂0arr + ε = getε(estim, Z̃) + gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) + g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, gc, ε) return nothing end + +""" + con_custom_mhe!(gc, estim::MovingHorizonEstimator, V̂, X̂0, Z̃, x̄, ε) -> gc + +Evaluate the custom inequality constraint `gc` in-place for [`MovingHorizonEstimator`](@ref). +""" +function con_custom_mhe!(gc, estim::MovingHorizonEstimator, V̂, X̂0, Z̃, x̄, ε) + if estim.con.nc > 0 + P̄ = estim.P̂arr_old + nx̂, nε, Nk = estim.nx̂, estim.nε, estim.Nk[] + nx̃ = nε + nx̂ + X̂ = [x̂0arr .+ estim.x̂op; X̂0 .+ estim.X̂op] + estim.con.gc!(gc, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ε) + end + return gc +end + """ - con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ε) -> g + con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, gc, ε) -> g Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref). """ -function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ε) +function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, gc, ε) nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[] nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] for i in eachindex(g) @@ -837,7 +854,7 @@ function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, end "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g +con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ , _ ) = g "Throw an error if P̂ != nothing." function setstate_cov!(::MovingHorizonEstimator, P̂) From 8ebe6396f579a134643d947b88c3ab0e63d5a426 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 19 May 2026 11:11:01 -0400 Subject: [PATCH 17/56] debug: doc building on windows now work --- docs/src/index.md | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 8a9a8a851..513f7022c 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -30,10 +30,10 @@ The documentation is divided in two parts: ```@contents Depth = 2 Pages = [ - "manual/installation.md", - "manual/linmpc.md", - "manual/nonlinmpc.md", - "manual/mtk.md" + joinpath("manual", "installation.md"), + joinpath("manual", "linmpc.md"), + joinpath("manual", "nonlinmpc.md"), + joinpath("manual", "mtk.md") ] ``` @@ -42,11 +42,11 @@ Pages = [ ```@contents Depth = 2 Pages = [ - "public/sim_model.md", - "public/state_estim.md", - "public/predictive_control.md", - "public/generic_func.md", - "public/plot_sim.md", + joinpath("public", "sim_model.md"), + joinpath("public", "state_estim.md"), + joinpath("public", "predictive_control.md"), + joinpath("public", "generic_func.md"), + joinpath("public", "plot_sim.md") ] ``` @@ -55,8 +55,8 @@ Pages = [ ```@contents Depth = 1 Pages = [ - "internals/sim_model.md", - "internals/state_estim.md", - "internals/predictive_control.md", + joinpath("internals", "sim_model.md"), + joinpath("internals", "state_estim.md"), + joinpath("internals", "predictive_control.md") ] ``` From f7a6a7242d4aa4cdbf500b9efb9729c030c8d3ed Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 19 May 2026 11:32:15 -0400 Subject: [PATCH 18/56] debug: solve method ambiguity --- src/controller/nonlinmpc.jl | 6 +++--- src/estimator/mhe/execute.jl | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 04756fa33..4a2c5e481 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -280,9 +280,9 @@ NonLinMPC controller with a sample time Ts = 10.0 s: | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | | :--------------- | :------------- | :----------- | :-----------| - | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k`` | ``k + H_p`` | + | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k`` | ``k + H_p`` | + | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k`` | ``k + H_p`` | More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3e24e858f..ea0432e81 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -687,7 +687,9 @@ Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinMo It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective function at specific `Z̃`. """ -function obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃) +function obj_nonlinprog( + estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃::AbstractVector{NT} +) where NT<:Real return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end From 3296c8696806d723c5867de624c48f47b5955443 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 19 May 2026 17:31:46 -0400 Subject: [PATCH 19/56] =?UTF-8?q?added:=20distinct=20`W=CC=82`=20variable?= =?UTF-8?q?=20in=20MHE=20(filled=20from=20`Z=CC=83`)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/construct.jl | 38 ++++++----- src/estimator/mhe/execute.jl | 117 +++++++++++++++++++++------------ 2 files changed, 97 insertions(+), 58 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 99b6b5fe6..de7d869c8 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1527,25 +1527,27 @@ function get_nonlinobj_op( ) where JNT<:Real model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian - nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk + nx̂, nym, nŷ, nu, nk, nc = estim.nx̂, estim.nym, model.ny, model.nu, model.nk, con.nc He = estim.He ng = length(con.i_g) - nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) + nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) strict = Val(true) myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) + Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) - function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + function J!(Z̃, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g) + update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call J_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), + Cache(x̄), Cache(gc), Cache(g), ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He @@ -1635,28 +1637,33 @@ function get_nonlincon_oracle( i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(con.i_g), sum(con.i_g) nc = con.nc - nV̂, nX̂, nZ̃ = He*nym, He*nx̂, length(estim.Z̃) + nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) + Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + x̄::Vector{JNT} = zeros(JNT, nx̂) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) - gi::Vector{JNT} = zeros(JNT, ngi) + gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, V̂, X̂0, û0, k, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + function gi!(gi, Z̃, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g) + update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, V̂, X̂0, û0, k, ŷ0, g, gi) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + function ℓ_gi(Z̃, λi, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, gi) + update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call - ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) + ∇gi_cache = ( + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), + Cache(x̄), Cache(gc), Cache(g), + ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) @@ -1665,7 +1672,8 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(gi) + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), + Cache(x̄), Cache(gc), Cache(g), Cache(gi) ) estim.Nk[] = He # see comment above ∇²gi_prep = prepare_hessian( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ea0432e81..59aa696c0 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -203,8 +203,8 @@ function addinfo!( Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), ) function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) - return obj_nonlinprog(estim, model, x̄, V̂, Z̃) + update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end if !isnothing(hess) prep_∇²J = prepare_hessian(J!, hess, estim.Z̃, J_cache...) @@ -276,6 +276,12 @@ end "Nothing to add in the `info` dict for [`LinModel`](@ref)." addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info +"Get the estimated state at arrival from the decision vector `Z̃`." +function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) + nx̃ = estim.nε + estim.nx̂ + return x̂0arr .= @views Z̃[nx̃-estim.nx̂+1:nx̃] +end + """ getε(estim::MovingHorizonEstimator, Z̃) -> ε @@ -528,10 +534,11 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real estim.Z̃ .= JuMP.value.(Z̃var) end # --------- update estimate ----------------------- - û0, ŷ0, k = buffer.û, buffer.ŷ, buffer.k + x̂0arr, û0, ŷ0, k = buffer.x̂, buffer.û, buffer.ŷ, buffer.k + V̂, X̂0 = buffer.V̂, buffer.X̂ estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) + getarrival!(x̂0arr, estim, estim.Z̃) + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) x̂0next = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂] estim.x̂0 .= x̂0next return estim.Z̃ @@ -575,10 +582,11 @@ function set_warmstart_mhe!(estim::MovingHorizonEstimator{NT}, Z̃var) where NT< Z̃s[nx̃+1:end] = estim.Ŵ # verify definiteness of objective function: V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s) - Js = obj_nonlinprog(estim, model, x̄, V̂, Z̃s) + x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.x̂0arr_old, estim.Ŵ, Z̃s) + Js = obj_nonlinprog(estim, model, x̄, V̂, estim.Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+1:end] = 0 + Z̃s[nx̃+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 @@ -680,40 +688,41 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) end """ - obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinModel`](@ref). It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective function at specific `Z̃`. """ -function obj_nonlinprog( - estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃::AbstractVector{NT} -) where NT<:Real +function obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end """ - obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Ŵ, _ ) Objective function of the MHE when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ -function obj_nonlinprog( - estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Z̃::AbstractVector{NT} -) where NT<:Real - nε, Nk = estim.nε, estim.Nk[] - nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.cov.invP̄ - nx̃ = nε + nx̂ +function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ, Z̃) + Nk = estim.Nk[] + invP̄ = estim.cov.invP̄ invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) - Ŵ, V̂, ε = @views Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm], getε(estim, Z̃) + if Nk < estim.He + nŴ, nYm = Nk*estim.nx̂, Nk*estim.nym + Ŵ, V̂ = Ŵ[1:nŴ], V̂[1:nYm] + end + ε = getε(estim, Z̃) return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + estim.C*ε^2 end @doc raw""" - predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 + predict_mhe!( + V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, _ , _ , Z̃ + ) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -727,17 +736,31 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k \end{aligned} ``` """ -function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) +function predict_mhe!( + V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃ +) nε, Nk = estim.nε, estim.Nk[] - nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk - nZ̃ = nε + estim.nx̂ + nŴ - V̂[1:nYm] .= @views estim.Ẽ[1:nYm, 1:nZ̃]*Z̃[1:nZ̃] + estim.F[1:nYm] - X̂0[1:nX̂] .= @views estim.con.Ẽx̂[1:nX̂, 1:nZ̃]*Z̃[1:nZ̃] + estim.con.Fx̂[1:nX̂] + if Nk < estim.He + nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk + nZ̃ = nε + estim.nx̂ + nŴ + Ẽ, F = estim.Ẽ[1:nYm, 1:nZ̃], estim.F[1:nYm] + Ẽx̂, Fx̂ = estim.con.Ẽx̂[1:nX̂, 1:nZ̃], estim.con.Fx̂[1:nX̂] + Z̃ = Z̃[1:nZ̃] + V̂_res, X̂0_res = @views V̂[1:nYm], X̂0[1:nX̂] + else + Ẽ, F = estim.Ẽ, estim.F + Ẽx̂, Fx̂ = estim.con.Ẽx̂, estim.con.Fx̂ + V̂_res, X̂0_res = V̂, X̂0 + end + V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F + X̂0_res .= mul!(X̂0_res, Ẽx̂, Z̃) .+ Fx̂ return V̂, X̂0 end @doc raw""" - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 + predict_mhe!( + V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, x̂0arr, Ŵ, _ + ) -> V̂, X̂0 Compute the vectors when `model` is *not* a [`LinModel`](@ref). @@ -745,17 +768,17 @@ The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The augm [`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated process noise ``\mathbf{ŵ}``. """ -function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) - nε, Nk = estim.nε, estim.Nk[] - nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym - nx̃ = nε + nx̂ - x̂0 = @views Z̃[nx̃-nx̂+1:nx̃] - if estim.direct # p = 0 +function predict_mhe!( + V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, x̂0arr, Ŵ, _ +) + nu, nd, nx̂, nŵ, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] + x̂0 = x̂0arr + if estim.direct # p = 0 ŷ0next = ŷ0 d0 = @views estim.D0[1:nd] for j=1:Nk u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] - ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] + ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) x̂0next .+= ŵ @@ -766,12 +789,12 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m V̂[(1 + nym*(j-1)):(nym*j)] .= y0nextm .- ŷ0nextm x̂0, d0 = x̂0next, d0next end - else # p = 1 + else # p = 1 for j=1:Nk y0m = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] d0 = @views estim.D0[ (1 + nd*j):(nd*(j+1))] # 1st one is d(k-Nk), not used - ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] + ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] ĥ!(ŷ0, estim, model, x̂0, d0) ŷ0m = @views ŷ0[estim.i_ym] V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m @@ -787,7 +810,7 @@ end """ update_predictions!( - V̂, X̂0, û0, k, ŷ0, x̄, gc, g, + Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -795,14 +818,20 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, k, ŷ0, Ŵ, x̄, gc, g, estim::MovingHorizonEstimator, Z̃) +function update_prediction!( + Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim::MovingHorizonEstimator, Z̃ +) + nŵ, nx̂, nε, Nk = estim.nx̂, estim.nx̂, estim.nε, estim.Nk[] model = estim.model - nx̃ = estim.nε + estim.nx̂ - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃) - x̂0arr = @views Z̃[nx̃-nx̂+1:nx̃] + nx̃ = nε + nx̂ + nŴ = nŵ*Nk + x̂0arr = x̄ + getarrival!(x̂0arr, estim, Z̃) x̄ .= estim.x̂0arr_old .- x̂0arr + Ŵ[1:nŴ] .= @views Z̃[(nx̃+1):(nx̃+nŴ)] + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) ε = getε(estim, Z̃) - gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) + #gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, gc, ε) return nothing end @@ -825,7 +854,9 @@ function con_custom_mhe!(gc, estim::MovingHorizonEstimator, V̂, X̂0, Z̃, x̄, end """ - con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, gc, ε) -> g + con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, gc, ε + ) -> g Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref). """ From af9df422c3e9681454853fb060471a01924903b0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 09:26:41 -0400 Subject: [PATCH 20/56] debug: ignore slack in MHE objective if `Cwt=Inf` --- src/estimator/mhe/execute.jl | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 59aa696c0..8f43c08c4 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -282,6 +282,12 @@ function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) return x̂0arr .= @views Z̃[nx̃-estim.nx̂+1:nx̃] end +"Get the estimated process noise over the horizon from the decision vector `Z̃`." +function getŴ!(Ŵ, estim::MovingHorizonEstimator, Z̃) + nx̃ = estim.nε + estim.nx̂ + return Ŵ .= @views Z̃[(nx̃ + 1):(nx̃ + estim.nx̂*estim.He)] +end + """ getε(estim::MovingHorizonEstimator, Z̃) -> ε @@ -700,7 +706,7 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z end """ - obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Ŵ, _ ) + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Ŵ, Z̃) Objective function of the MHE when `model` is not a [`LinModel`](@ref). @@ -715,8 +721,8 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ nŴ, nYm = Nk*estim.nx̂, Nk*estim.nym Ŵ, V̂ = Ŵ[1:nŴ], V̂[1:nYm] end - ε = getε(estim, Z̃) - return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + estim.C*ε^2 + Jε = estim.nε > 0 ? estim.C*Z̃[begin]^2 : 0 + return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jε end @doc raw""" @@ -741,6 +747,7 @@ function predict_mhe!( ) nε, Nk = estim.nε, estim.Nk[] if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk nZ̃ = nε + estim.nx̂ + nŴ Ẽ, F = estim.Ẽ[1:nYm, 1:nZ̃], estim.F[1:nYm] @@ -821,18 +828,15 @@ The method mutates all the arguments before `estim` argument. function update_prediction!( Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim::MovingHorizonEstimator, Z̃ ) - nŵ, nx̂, nε, Nk = estim.nx̂, estim.nx̂, estim.nε, estim.Nk[] model = estim.model - nx̃ = nε + nx̂ - nŴ = nŵ*Nk x̂0arr = x̄ getarrival!(x̂0arr, estim, Z̃) x̄ .= estim.x̂0arr_old .- x̂0arr - Ŵ[1:nŴ] .= @views Z̃[(nx̃+1):(nx̃+nŴ)] - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) - ε = getε(estim, Z̃) - #gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) - g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, gc, ε) + Ŵ = getŴ!(Ŵ, estim, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) + ε = getε(estim, Z̃) + # gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) + g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, gc, ε) return nothing end From 35b268341d53b1ce2cc89b9fb98372a430da881f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 11:47:40 -0400 Subject: [PATCH 21/56] =?UTF-8?q?added:=20`x=CC=820arr`=20argument=20in=20?= =?UTF-8?q?`update=5Fprediction!`=20for=20MHE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/construct.jl | 58 +++++++++++++++++----------------- src/estimator/mhe/execute.jl | 9 +++--- 2 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index de7d869c8..fdd730a4c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1532,22 +1532,22 @@ function get_nonlinobj_op( ng = length(con.i_g) nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) strict = Val(true) - myNaN = convert(JNT, NaN) - J::Vector{JNT} = zeros(JNT, 1) - Ŵ::Vector{JNT} = zeros(JNT, nŴ) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) - û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - x̄::Vector{JNT} = zeros(JNT, nx̂) - gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) - function J!(Z̃, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g) - update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + myNaN = convert(JNT, NaN) + J::Vector{JNT} = zeros(JNT, 1) + Ŵ::Vector{JNT} = zeros(JNT, nŴ) + V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + k::Vector{JNT} = zeros(JNT, nk) + û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call J_cache = ( - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), - Cache(x̄), Cache(gc), Cache(g), + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He @@ -1639,30 +1639,30 @@ function get_nonlincon_oracle( nc = con.nc nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) strict = Val(true) - myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) - Ŵ::Vector{JNT} = zeros(JNT, nŴ) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) - û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - x̄::Vector{JNT} = zeros(JNT, nx̂) - gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) - gi::Vector{JNT} = zeros(JNT, ngi) - λi::Vector{JNT} = rand(JNT, ngi) + myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) + Ŵ::Vector{JNT} = zeros(JNT, nŴ) + V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + k::Vector{JNT} = zeros(JNT, nk) + û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + gi::Vector{JNT} = zeros(JNT, ngi) + λi::Vector{JNT} = rand(JNT, ngi) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g) - update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, gi) - update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, gi) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇gi_cache = ( - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), - Cache(x̄), Cache(gc), Cache(g), + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He @@ -1672,8 +1672,8 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), - Cache(x̄), Cache(gc), Cache(g), Cache(gi) + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi) ) estim.Nk[] = He # see comment above ∇²gi_prep = prepare_hessian( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 8f43c08c4..f59908f5d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -817,7 +817,7 @@ end """ update_predictions!( - Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, + x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -826,12 +826,11 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_prediction!( - Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim::MovingHorizonEstimator, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ ) model = estim.model - x̂0arr = x̄ - getarrival!(x̂0arr, estim, Z̃) - x̄ .= estim.x̂0arr_old .- x̂0arr + x̂0arr = getarrival!(x̂0arr, estim, Z̃) + x̄ .= estim.x̂0arr_old .- x̂0arr Ŵ = getŴ!(Ŵ, estim, Z̃) V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) ε = getε(estim, Z̃) From 44f315917e2d9bfdd2b33db16a09eb59b5b16a0a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 11:54:08 -0400 Subject: [PATCH 22/56] changed: cosmetic modification --- src/estimator/mhe/construct.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fdd730a4c..c07c0f3fb 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1534,11 +1534,11 @@ function get_nonlinobj_op( strict = Val(true) myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) + x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) @@ -1640,11 +1640,11 @@ function get_nonlincon_oracle( nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) + x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) From a1a6e23d0ad2add61dd810396c908013dc4d8be0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 13:56:42 -0400 Subject: [PATCH 23/56] added: `getinfo!` back on tracks for MHE --- src/estimator/mhe/execute.jl | 51 ++++++++++++++++++++---------------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index f59908f5d..ba90ab29f 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -123,11 +123,11 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̂, nym, nŵ, nε = estim.nx̂, estim.nym, estim.nx̂, estim.nε nx̃ = nε + nx̂ info = Dict{Symbol, Any}() - V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) - û0, k, ŷ0 = buffer.û, buffer.k, buffer.ŷ - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) - x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] - x̄ = estim.x̂0arr_old - x̂0arr + V̂, X̂0 = buffer.V̂, buffer.X̂ + x̂0arr, û0, k, ŷ0 = buffer.x̂, buffer.û, buffer.k, buffer.ŷ + x̂0arr = getarrival!(x̂0arr, estim, estim.Z̃) + x̄ = estim.x̂0arr_old - x̂0arr + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) X̂0 = [x̂0arr; X̂0] Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*Nk] Ŷ0m, Ŷ0 = Vector{NT}(undef, nym*Nk), Vector{NT}(undef, ny*Nk) @@ -148,7 +148,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŵ] = estim.Ŵ[1:Nk*nŵ] info[:x̂arr] = x̂0arr + estim.x̂op info[:ε] = nε ≠ 0 ? estim.Z̃[begin] : zero(NT) - info[:J] = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Z̃) + info[:J] = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Ŵ, estim.Z̃) info[:X̂] = X̂0 .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂ @@ -189,21 +189,24 @@ function addinfo!( # --- objective derivatives --- optim, con = estim.optim, estim.con hess = estim.hessian - nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk + nx̂, nym, nŷ, nu, nk, nc = estim.nx̂, estim.nym, model.ny, model.nu, model.nk, con.nc He = estim.He i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(con.i_g), sum(con.i_g) - nV̂, nX̂ = He*nym, He*nx̂ - V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) - k = zeros(NT, nk) - û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) - g, gi = zeros(NT, ng), zeros(NT, ngi) - x̄ = zeros(NT, nx̂) + nV̂, nX̂, nŴ = He*nym, He*nx̂, He*nx̂ + x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) + V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) + Ŵ = zeros(NT, nŴ) + k = zeros(NT, nk) + û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) + gc, g = zeros(NT, nc), zeros(NT, ng) + gi = zeros(NT, ngi) J_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) - function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) - update_prediction!(Ŵ, V̂, X̂0, û0, k, ŷ0, x̄, gc, g, estim, Z̃) + function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end if !isnothing(hess) @@ -216,9 +219,12 @@ function addinfo!( ∇²J_opt, ∇²J_ncolors = nothing, nothing end # --- inequality constraint derivatives --- - ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) - function gi!(gi, Z̃, V̂, X̂0, û0, k, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + ∇g_cache = ( + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), + ) + function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return nothing end @@ -241,10 +247,11 @@ function addinfo!( end end ∇²g_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(gi) + Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi) ) - function ℓ_gi(Z̃, λi, V̂, X̂0, û0, k, ŷ0, g, gi) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, gi) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end From 35605b33eb4227ab3f03818d515e12affb82b078 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 14:32:23 -0400 Subject: [PATCH 24/56] debug: correct arg init `init_predmat_mhe` --- src/estimator/mhe/execute.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ba90ab29f..328f04943 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -928,11 +928,10 @@ function setmodel_estimator!( estim.f̂op .= f̂op estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- predictions matrices --- - p = estim.direct ? 0 : 1 E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( model, He, estim.i_ym, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, - estim.x̂op, estim.f̂op, p + estim.x̂op, estim.f̂op, estim.direct ) A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nε, con.C_x̂min, con.C_x̂max, Ex̂) A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nε, con.C_v̂min, con.C_v̂max, E) From 916c24df93ba8e64e15ecf4ecf6952948017c6a8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 20 May 2026 14:44:29 -0400 Subject: [PATCH 25/56] debug: `getinfo!` for MHE correct sizes --- 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 328f04943..e73ddf0ee 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -149,7 +149,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:x̂arr] = x̂0arr + estim.x̂op info[:ε] = nε ≠ 0 ? estim.Z̃[begin] : zero(NT) info[:J] = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Ŵ, estim.Z̃) - info[:X̂] = X̂0 .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]] + info[:X̂] = (X̂0 .+ @views [estim.x̂op; estim.X̂op])[1:nx̂*(Nk+1)] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂ info[:P̄] = estim.P̂arr_old From 10263e9261ec31ae4626accb736a979b40715448 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 10:42:47 -0400 Subject: [PATCH 26/56] doc: extended vectors for MHE custom NL constraint --- src/controller/nonlinmpc.jl | 8 +++++--- src/estimator/mhe/yo.md | 2 ++ 2 files changed, 7 insertions(+), 3 deletions(-) create mode 100644 src/estimator/mhe/yo.md diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 4a2c5e481..d8dd217bd 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -280,9 +280,11 @@ NonLinMPC controller with a sample time Ts = 10.0 s: | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | | :--------------- | :------------- | :----------- | :-----------| - | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k`` | ``k + H_p`` | - | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k`` | ``k + H_p`` | - | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k`` | ``k + H_p`` | + | ``\mathbf{U_e}`` | `((Hp+1)*nu,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{Ŷ_e}`` | `((Hp+1)*ny,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{D̂_e}`` | `((Hp+1)*nd,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{p}`` | var. | — | — | + | ``ϵ`` | `()` | — | — | More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that diff --git a/src/estimator/mhe/yo.md b/src/estimator/mhe/yo.md new file mode 100644 index 000000000..35d53f69d --- /dev/null +++ b/src/estimator/mhe/yo.md @@ -0,0 +1,2 @@ + The vectors will grows with time until ``N_k = H_e`` is reached. They are also + *artificially* aligned in terms of time steps to ease the user life. But note \ No newline at end of file From 0d40d693c6920e233ee59aa5d143cbeb06214c30 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 11:03:40 -0400 Subject: [PATCH 27/56] doc: idem --- src/estimator/mhe/construct.jl | 61 +++++++++++++++++++--------------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index c07c0f3fb..7db739ba9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -229,7 +229,7 @@ however, since it minimizes the following objective function at each discrete ti ``` subject to [`setconstraint!`](@ref) bounds and the custom nonlinear inequality constraints: ```math -\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0} +\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0} ``` and in which the arrival costs are evaluated from the states estimated at time ``k-N_k``: ```math @@ -253,11 +253,14 @@ N_k = \begin{cases} ``` The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` respectively encompass the estimated process noises ``\mathbf{ŵ}(k-j+p)`` from ``j=N_k`` to ``1`` and sensor noises ``\mathbf{v̂}(k-j+1)`` -from ``j=N_k`` to ``1``. The Extended Help defines the two vectors, the slack variable -``ε``, the other arguments of the ``\mathbf{g_c}`` function, and the estimation of the -covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument `direct=true` -(default value), the constant ``p=0`` in the equations above, and the MHE is in the current -form. Else ``p=1``, leading to the prediction form. +from ``j=N_k`` to ``1``. The arguments of ``\mathbf{g_c}`` include the extended vectors of +the estimated states ``\mathbf{X̂_e}``, estimated sensor noises ``\mathbf{V̂_e}``, estimated +process noises ``\mathbf{Ŵ_e}``, manipulated inputs ``\mathbf{U_e}``, measured outputs +``\mathbf{Y_e^m}``and measured disturbances ``\mathbf{D_e}``. The Extended Help details all +these vectors, the slack variable ``ε`` and the estimation of the covariance at arrival +``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument `direct=true` (default value), the +constant ``p=0`` in the equations above, and the MHE is in the current form. Else ``p=1``, +leading to the prediction form. See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory @@ -380,30 +383,36 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). It is disabled thus always zero by default for the MHE (from `Cwt=Inf`) but it should be activated for problems with two or more types of bounds, to ensure feasibility (e.g. on - ``\mathbf{x̂}`` and ``\mathbf{v̂}``). The following table details the other arguments of - ``\mathbf{g_c}``, including the time steps of the first and last sample in them. Note - that the vectors will grows with time until ``N_k = H_e`` is reached, and the windows - don't start at the same time step (a side-effect of the current form). - - | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | - | :--------------- | :-------------- | :-------------- | :-------------- | - | ``\mathbf{X̂}`` | `(nx̂*(Nk+1),)` | ``k - N_k + p`` | ``k + p`` | - | ``\mathbf{V̂}`` | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | - | ``\mathbf{Ŵ}`` | `(nx̂*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | - | ``\mathbf{U}`` | `(nu*Nk,)` | ``k - N_k + p`` | ``k - 1 + p`` | - | ``\mathbf{Y^m}`` | `(nym*Nk,)` | ``k - N_k + 1`` | ``k`` | - | ``\mathbf{D}`` | `(nd*(Nk+1),)` | ``k - N_k`` | ``k`` | - | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | - | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | + ``\mathbf{x̂}`` and ``\mathbf{v̂}``). The following table details the arguments of + ``\mathbf{g_c}``, including the time steps of the first and last sample in them. + + !!! warning + The vectors will grows with time until ``N_k = H_e`` is reached. The time steps are + also *artificially aligned* to ease the user life, but some data at boundaries are + unavailable e.g.: ``\mathbf{u}(k)`` with ``p=0``. They are filled with `NaN` values. + The exact time steps of the `NaN`s are detailed in the last column below. + + | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | MISSING SAMPLES (NAN) | + | :--------------- | :-------------- | :-------------- | :-------------- | :------------------------- | + | ``\mathbf{X̂_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | — | + | ``\mathbf{V̂_e}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k, k + 1`` | + | ``\mathbf{Ŵ_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k + p - 1, k + p`` | + | ``\mathbf{U_e}`` | `((Nk+1)*nu,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` | + | ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | + | ``\mathbf{D_e}`` | `((Nk+1)*nd,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | + | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | — | + | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | — | + | ``\mathbf{p}`` | var. | — | — | — | + | ``ε`` | `()` | — | — | — | If `LHS` represents the result of the left-hand side in the inequality - ``\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, P̄, x̄, p}, ε) ≤ \mathbf{0}``, + ``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0}``, the function `gc` can be implemented in two possible ways: - 1. **Non-mutating function** (out-of-place): define it as `gc(X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, - p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. - 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, - p, ε) -> nothing`. This syntax reduces the allocations and potentially the + 1. **Non-mutating function** (out-of-place): define it as `gc(X̂e, V̂e, Ŵe, Ue, Yem, De, + P̄, x̄, p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. + 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yme, De, P̄, + x̄, p, ε) -> nothing`. This syntax reduces the allocations and potentially the computational burden as well. The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for From f6dbcfe19d7d0480be551c039c1e64859b051643 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 14:27:32 -0400 Subject: [PATCH 28/56] added: filling the extended windows of MHE --- src/estimator/mhe/construct.jl | 24 ++++++---------- src/estimator/mhe/execute.jl | 51 +++++++++++++++++++++++++++------- 2 files changed, 50 insertions(+), 25 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 7db739ba9..64e9cc0fc 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -116,16 +116,13 @@ struct MovingHorizonEstimator{ r::Vector{NT} C::NT X̂op ::Vector{NT} - Uop ::Vector{NT} - Yopm::Vector{NT} - Dop ::Vector{NT} X̂0 ::Vector{NT} Y0m::Vector{NT} - Ym ::Vector{NT} + Yem::Vector{NT} U0 ::Vector{NT} - U ::Vector{NT} + Ue ::Vector{NT} D0 ::Vector{NT} - D ::Vector{NT} + De ::Vector{NT} Ŵ ::Vector{NT} x̂0arr_old::Vector{NT} P̂arr_old ::Hermitian{NT, Matrix{NT}} @@ -174,13 +171,10 @@ struct MovingHorizonEstimator{ H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) X̂op = repeat(x̂op, He) - Uop = repeat(model.uop, He) - Yopm = repeat(model.yop[i_ym], He) - Dop = repeat(model.dop, He+1) X̂0 = zeros(NT, nx̂*He) - Y0m, Ym = zeros(NT, nym*He), zeros(NT, nym*He) - U0, U = zeros(NT, nu*He), zeros(NT, nu*He) - D0, D = zeros(NT, nd*(He+1)), zeros(NT, nd*(He+1)) + Y0m, Yem = zeros(NT, nym*He), fill(NT(NaN), nym*(He+1)) + U0, Ue = zeros(NT, nu*He), fill(NT(NaN), nu*(He+1)) + D0, De = zeros(NT, nd*(He+1)), fill(NT(NaN), nd*(He+1)) Ŵ = zeros(NT, nx̂*He) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) x̂0arr_old = zeros(NT, nx̂) @@ -201,8 +195,8 @@ struct MovingHorizonEstimator{ Ẽ, F, G, J, B, ẽx̄, fx̄, H̃, q̃, r, Cwt, - X̂op, Uop, Yopm, Dop, - X̂0, Y0m, Ym, U0, U, D0, D, Ŵ, + X̂op, + X̂0, Y0m, Yem, U0, Ue, D0, De, Ŵ, x̂0arr_old, P̂arr_old, Nk, direct, corrected, buffer @@ -387,7 +381,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: ``\mathbf{g_c}``, including the time steps of the first and last sample in them. !!! warning - The vectors will grows with time until ``N_k = H_e`` is reached. The time steps are + The vectors will grows with time until ``N_k = H_e`` is reached. The time series are also *artificially aligned* to ease the user life, but some data at boundaries are unavailable e.g.: ``\mathbf{u}(k)`` with ``p=0``. They are filled with `NaN` values. The exact time steps of the `NaN`s are detailed in the last column below. diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index e73ddf0ee..c941b675b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -1,17 +1,29 @@ "Reset the data windows and time-varying variables for the moving horizon estimator." -function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) +function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) + model = estim.model estim.Z̃ .= 0 estim.X̂0 .= 0 estim.Y0m .= 0 + estim.Yem .= NaN estim.U0 .= 0 + estim.Ue .= NaN estim.D0 .= 0 + estim.De .= NaN estim.Ŵ .= 0 estim.Nk .= 0 estim.H̃ .= 0 estim.q̃ .= 0 estim.r .= 0 - estim.direct && (estim.U0[1:estim.model.nu] .= u0) # add u0(-1) to the data windows - estim.D0[1:estim.model.nd] .= d0 # add d0(-1) to the data windows + if estim.direct + # add y0m(-1) to the extended data window (custom NL constraints): + estim.Yem[1:model.ny] .= y0m .+ @views model.yop[estim.i_ym] + # add u0(-1) to the two data windows: + estim.U0[1:model.nu] .= u0 + estim.Ue[1:model.nu] .= u0 .+ model.uop + # add d0(-1) to the extended data window (custom NL constraints): + model.nd > 0 && (estim.De[1:model.nd] .= d0 .+ model.dop) + end + model.nd > 0 && (estim.D0[1:model.nd] .= d0) # add d0(-1) to the data window estim.lastu0 .= u0 # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) invert_cov!(estim, estim.cov.P̂_0) @@ -312,17 +324,18 @@ Add data to the observation windows of the moving horizon estimator and clamp `e If ``k ≥ H_e``, the observation windows are moving in time and `estim.Nk` is clamped to `estim.He`. It returns `true` if the observation windows are moving, `false` otherwise. If no `u0` argument is provided, the manipulated input of the last time step is added to its -window (the correct value if `estim.direct == true`). +window (the correct value if `estim.direct`). """ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0) model = estim.model nx̂, nym, nd, nu, nŵ = estim.nx̂, estim.nym, model.nd, model.nu, estim.nx̂ Nk = estim.Nk[] - p = estim.direct ? 0 : 1 - x̂0, ŵ = estim.x̂0, 0 # ŵ(k-1+p) = 0 for warm-start + p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k) + x̂0, ŵ = estim.x̂0, 0 # ŵ(k-1+p) = 0 for warm-start estim.Nk .+= 1 Nk = estim.Nk[] ismoving = (Nk > estim.He) + # --- data windows for the predictions --- if ismoving estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] estim.Y0m[end-nym+1:end] .= y0m @@ -345,10 +358,28 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ end estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] - # data windows including operating points, needed for custom NL constraints: - estim.U .= estim.U0 .+ estim.Uop - estim.Ym .= estim.Y0m .+ estim.Yopm - estim.D .= estim.D0 .+ estim.Dop + # --- extended data windows for custom NL constraints --- + # see MovingHorzionEstimator extended help for the exact time steps in each data window + yopm = @views model.yop[estim.i_ym] + # TODO: move above + if ismoving + estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] + estim.Yem[(end-nym+1 - p*nym):(end - p*nym)] .= y0m .+ yopm + if nd > 0 + estim.De[1:end-nd] .= @views estim.De[nd+1:end] + estim.De[(end-nd+1 - p*nd):(end - p*nd)] .= d0 .+ model.dop + end + estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] + estim.Ue[(end-nu+1 - nu):(end - nu)] = u0 .+ model.uop + else + estim.Yem[(1 + nym*(Nk-p)):(nym*(Nk-p+1))] .= y0m .+ yopm + nd > 0 && (estim.De[(1 + nd*(Nk-p)):(nd*(Nk-p+1))] .= d0 .+ model.dop) + estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop + end + @show estim.Nk + @show estim.Yem + @show estim.De + @show estim.Ue return ismoving end From 9eeb35ca1538d02633568eb346a648cab8417852 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 14:33:08 -0400 Subject: [PATCH 29/56] change: code cleaning in `add_data_windows!` --- src/estimator/mhe/execute.jl | 51 +++++++++++++++--------------------- 1 file changed, 21 insertions(+), 30 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c941b675b..1d65eadeb 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -329,6 +329,7 @@ window (the correct value if `estim.direct`). function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0) model = estim.model nx̂, nym, nd, nu, nŵ = estim.nx̂, estim.nym, model.nd, model.nu, estim.nx̂ + yopm = @views model.yop[estim.i_ym] Nk = estim.Nk[] p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k) x̂0, ŵ = estim.x̂0, 0 # ŵ(k-1+p) = 0 for warm-start @@ -336,50 +337,40 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last Nk = estim.Nk[] ismoving = (Nk > estim.He) # --- data windows for the predictions --- + # see MovingHorzionEstimator extended help for the exact time steps in each data window if ismoving estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] - estim.Y0m[end-nym+1:end] .= y0m + estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] + estim.Y0m[end-nym+1:end] .= y0m + estim.Yem[(end-nym+1 - p*nym):(end - p*nym)] .= y0m .+ yopm if nd > 0 estim.D0[1:end-nd] .= @views estim.D0[nd+1:end] - estim.D0[end-nd+1:end] .= d0 + estim.De[1:end-nd] .= @views estim.De[nd+1:end] + estim.D0[end-nd+1:end] .= d0 + estim.De[(end-nd+1 - p*nd):(end - p*nd)] .= d0 .+ model.dop end estim.U0[1:end-nu] .= @views estim.U0[nu+1:end] - estim.U0[end-nu+1:end] .= u0 + estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] + estim.U0[end-nu+1:end] .= u0 + estim.Ue[(end-nu+1 - nu):(end - nu)] .= u0 .+ model.uop estim.X̂0[1:end-nx̂] .= @views estim.X̂0[nx̂+1:end] estim.X̂0[end-nx̂+1:end] .= x̂0 estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] estim.Ŵ[end-nŵ+1:end] .= ŵ estim.Nk .= estim.He else - estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m - nd > 0 && (estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0) - estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 - estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0 - estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ - end - estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] - # --- extended data windows for custom NL constraints --- - # see MovingHorzionEstimator extended help for the exact time steps in each data window - yopm = @views model.yop[estim.i_ym] - # TODO: move above - if ismoving - estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] - estim.Yem[(end-nym+1 - p*nym):(end - p*nym)] .= y0m .+ yopm - if nd > 0 - estim.De[1:end-nd] .= @views estim.De[nd+1:end] - estim.De[(end-nd+1 - p*nd):(end - p*nd)] .= d0 .+ model.dop + estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m + estim.Yem[(1 + nym*(Nk-p)):(nym*(Nk-p+1))] .= y0m .+ yopm + if nd > 0 + estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0 + estim.De[(1 + nd*(Nk-p)):(nd*(Nk-p+1))] .= d0 .+ model.dop end - estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] - estim.Ue[(end-nu+1 - nu):(end - nu)] = u0 .+ model.uop - else - estim.Yem[(1 + nym*(Nk-p)):(nym*(Nk-p+1))] .= y0m .+ yopm - nd > 0 && (estim.De[(1 + nd*(Nk-p)):(nd*(Nk-p+1))] .= d0 .+ model.dop) - estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop + estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 + estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop + estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0 + estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ end - @show estim.Nk - @show estim.Yem - @show estim.De - @show estim.Ue + estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] return ismoving end From 82726f1b2cbc604c8ecaeb242182a2e45c14fdf6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 15:37:03 -0400 Subject: [PATCH 30/56] changed: fill all windows with `NaN` when no data This is more explicit this way. It is also safer since If I do computation with missing data by mistake, the `NaN` will propagate, which is a good thing to catch bugs. --- src/estimator/mhe/construct.jl | 10 ++--- src/estimator/mhe/execute.jl | 71 +++++++++++++++++++++------------- 2 files changed, 50 insertions(+), 31 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 64e9cc0fc..796597c4a 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -171,11 +171,11 @@ struct MovingHorizonEstimator{ H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) X̂op = repeat(x̂op, He) - X̂0 = zeros(NT, nx̂*He) - Y0m, Yem = zeros(NT, nym*He), fill(NT(NaN), nym*(He+1)) - U0, Ue = zeros(NT, nu*He), fill(NT(NaN), nu*(He+1)) - D0, De = zeros(NT, nd*(He+1)), fill(NT(NaN), nd*(He+1)) - Ŵ = zeros(NT, nx̂*He) + X̂0 = fill(NT(NaN), nx̂*He) + Y0m, Yem = fill(NT(NaN), nym*He), fill(NT(NaN), nym*(He+1)) + U0, Ue = fill(NT(NaN), nu*He), fill(NT(NaN), nu*(He+1)) + D0, De = fill(NT(NaN), nd*(He+1)), fill(NT(NaN), nd*(He+1)) + Ŵ = fill(NT(NaN), nx̂*He) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(cov.P̂_0) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1d65eadeb..4011309a7 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -2,14 +2,14 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model estim.Z̃ .= 0 - estim.X̂0 .= 0 - estim.Y0m .= 0 + estim.X̂0 .= NaN + estim.Y0m .= NaN estim.Yem .= NaN - estim.U0 .= 0 + estim.U0 .= NaN estim.Ue .= NaN - estim.D0 .= 0 + estim.D0 .= NaN estim.De .= NaN - estim.Ŵ .= 0 + estim.Ŵ .= NaN estim.Nk .= 0 estim.H̃ .= 0 estim.q̃ .= 0 @@ -406,33 +406,52 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He F, C, optim = estim.F, estim.C, estim.optim nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nYm, nŴ = nym*Nk, nŵ*Nk + nU, nYm, nD, nŴ = model.nu*Nk, estim.nym*Nk, model.nd*Nk, nŵ*Nk nZ̃ = nε + nx̂ + nŴ - # --- update F and fx̄ vectors for MHE predictions --- - F .= estim.Y0m .+ estim.B - mul!(F, estim.G, estim.U0, 1, 1) - if model.nd > 0 - mul!(F, estim.J, estim.D0, 1, 1) + # --- truncate vector and matrices if necessary --- + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + Y0m, B = estim.Y0m[1:nYm], estim.B[1:nYm] + G, U0 = estim.G[1:nYm, 1:nU], estim.U0[1:nU] + J, D0 = estim.J[1:nYm, 1:nD], estim.D0[1:nD] + Ẽ, ẽx̄ = estim.Ẽ[1:nYm, 1:nZ̃], estim.ẽx̄[:, 1:nZ̃] + F, q̃ = @views estim.F[1:nYm], estim.q̃[1:nZ̃] + H̃_data = @views estim.H̃.data[1:nZ̃, 1:nZ̃] + H̃ = @views estim.H̃[1:nZ̃, 1:nZ̃] + Z̃var = @views optim[:Z̃var][1:nZ̃] + else + Y0m, B = estim.Y0m, estim.B + G, U0 = estim.G, estim.U0 + J, D0 = estim.J, estim.D0 + Ẽ, ẽx̄ = estim.Ẽ, estim.ẽx̄ + F, q̃ = estim.F, estim.q̃ + H̃_data = estim.H̃.data + H̃ = estim.H̃ + Z̃var = optim[:Z̃var] end - estim.fx̄ .= estim.x̂0arr_old + invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He) + invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He) + fx̄ = estim.fx̄ + r = estim.r + # --- update F and fx̄ vectors for MHE predictions --- + F .= Y0m .+ B + mul!(F, G, U0, 1, 1) + (model.nd > 0) && mul!(F, J, D0, 1, 1) + fx̄ .= estim.x̂0arr_old # --- update H̃, q̃ and p vectors for quadratic optimization --- - ẼZ̃ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]] - FZ̃ = @views [estim.fx̄; estim.F[1:nYm]] - invQ̂_Nk = trunc_cov(invQ̂_He, estim.nx̂, Nk, estim.He) - invR̂_Nk = trunc_cov(invR̂_He, estim.nym, Nk, estim.He) + ẼZ̃ = [ẽx̄; Ẽ] + FZ̃ = [fx̄; F] M_Nk = [invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] Ñ_Nk = [fill(C, nε, nε) zeros(nε, nx̂+nŴ); zeros(nx̂, nε+nx̂+nŴ); zeros(nŴ, nε+nx̂) invQ̂_Nk] M_Nk_ẼZ̃ = M_Nk*ẼZ̃ - @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃) - @views lmul!(2, estim.q̃[1:nZ̃]) - estim.r .= dot(FZ̃, M_Nk, FZ̃) - estim.H̃.data[1:nZ̃, 1:nZ̃] .= Ñ_Nk - @views mul!(estim.H̃.data[1:nZ̃, 1:nZ̃], ẼZ̃', M_Nk_ẼZ̃, 1, 1) - @views lmul!(2, estim.H̃.data[1:nZ̃, 1:nZ̃]) - Z̃var_Nk::Vector{JuMP.VariableRef} = @views optim[:Z̃var][1:nZ̃] - H̃_Nk = @views estim.H̃[1:nZ̃,1:nZ̃] - q̃_Nk = @views estim.q̃[1:nZ̃] - JuMP.set_objective_function(optim, obj_quadprog(Z̃var_Nk, H̃_Nk, q̃_Nk)) + mul!(q̃, M_Nk_ẼZ̃', FZ̃) + lmul!(2, q̃) + r .= dot(FZ̃, M_Nk, FZ̃) + H̃_data .= Ñ_Nk + mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) + lmul!(2, H̃_data) + println(q̃) + JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end "Does nothing if `model` is not a [`LinModel`](@ref)." From 1dc619a28202a9d46a479ec19701140677fed051 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 17:28:46 -0400 Subject: [PATCH 31/56] wip: debug `init_pred` for `LinModel` in MHE --- src/estimator/mhe/construct.jl | 14 ++-- src/estimator/mhe/execute.jl | 122 +++++++++++++++++++-------------- 2 files changed, 78 insertions(+), 58 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 796597c4a..1dc9a13f8 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -115,8 +115,7 @@ struct MovingHorizonEstimator{ q̃::Vector{NT} r::Vector{NT} C::NT - X̂op ::Vector{NT} - X̂0 ::Vector{NT} + X̂op::Vector{NT} Y0m::Vector{NT} Yem::Vector{NT} U0 ::Vector{NT} @@ -124,6 +123,7 @@ struct MovingHorizonEstimator{ D0 ::Vector{NT} De ::Vector{NT} Ŵ ::Vector{NT} + X̂0_old ::Vector{NT} x̂0arr_old::Vector{NT} P̂arr_old ::Hermitian{NT, Matrix{NT}} Nk::Vector{Int} @@ -171,16 +171,16 @@ struct MovingHorizonEstimator{ H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) X̂op = repeat(x̂op, He) - X̂0 = fill(NT(NaN), nx̂*He) Y0m, Yem = fill(NT(NaN), nym*He), fill(NT(NaN), nym*(He+1)) U0, Ue = fill(NT(NaN), nu*He), fill(NT(NaN), nu*(He+1)) D0, De = fill(NT(NaN), nd*(He+1)), fill(NT(NaN), nd*(He+1)) - Ŵ = fill(NT(NaN), nx̂*He) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) + Ŵ = fill(NT(NaN), nx̂*He) + X̂0_old = fill(NT(NaN), nx̂*He) x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(cov.P̂_0) Nk = [0] corrected = [false] + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) estim = new{NT, SM, KC, JM, GB, JB, HB, GCfunc, CE}( model, cov, @@ -196,8 +196,8 @@ struct MovingHorizonEstimator{ H̃, q̃, r, Cwt, X̂op, - X̂0, Y0m, Yem, U0, Ue, D0, De, Ŵ, - x̂0arr_old, P̂arr_old, Nk, + Y0m, Yem, U0, Ue, D0, De, Ŵ, + X̂0_old, x̂0arr_old, P̂arr_old, Nk, direct, corrected, buffer ) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 4011309a7..c515fa9dd 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -2,7 +2,6 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model estim.Z̃ .= 0 - estim.X̂0 .= NaN estim.Y0m .= NaN estim.Yem .= NaN estim.U0 .= NaN @@ -10,10 +9,13 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.D0 .= NaN estim.De .= NaN estim.Ŵ .= NaN + estim.X̂0_old .= NaN estim.Nk .= 0 + estim.F .= 0 estim.H̃ .= 0 estim.q̃ .= 0 estim.r .= 0 + estim.con.Fx̂ .= 0 if estim.direct # add y0m(-1) to the extended data window (custom NL constraints): estim.Yem[1:model.ny] .= y0m .+ @views model.yop[estim.i_ym] @@ -332,15 +334,16 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last yopm = @views model.yop[estim.i_ym] Nk = estim.Nk[] p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k) - x̂0, ŵ = estim.x̂0, 0 # ŵ(k-1+p) = 0 for warm-start + x̂0_old = estim.x̂0 # x̂0_old is x̂0(k-1|k-1) if estim.direct, else x̂0(k|k-1) + ŵ = 0 # ŵ(k-1+p) = 0 for warm-start estim.Nk .+= 1 Nk = estim.Nk[] ismoving = (Nk > estim.He) # --- data windows for the predictions --- # see MovingHorzionEstimator extended help for the exact time steps in each data window if ismoving - estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] - estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] + estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] + estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] estim.Y0m[end-nym+1:end] .= y0m estim.Yem[(end-nym+1 - p*nym):(end - p*nym)] .= y0m .+ yopm if nd > 0 @@ -349,14 +352,14 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.D0[end-nd+1:end] .= d0 estim.De[(end-nd+1 - p*nd):(end - p*nd)] .= d0 .+ model.dop end - estim.U0[1:end-nu] .= @views estim.U0[nu+1:end] - estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] + estim.U0[1:end-nu] .= @views estim.U0[nu+1:end] + estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] estim.U0[end-nu+1:end] .= u0 estim.Ue[(end-nu+1 - nu):(end - nu)] .= u0 .+ model.uop - estim.X̂0[1:end-nx̂] .= @views estim.X̂0[nx̂+1:end] - estim.X̂0[end-nx̂+1:end] .= x̂0 - estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] - estim.Ŵ[end-nŵ+1:end] .= ŵ + estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] + estim.Ŵ[end-nŵ+1:end] .= ŵ + estim.X̂0_old[1:end-nx̂] .= @views estim.X̂0_old[nx̂+1:end] + estim.X̂0_old[end-nx̂+1:end] .= x̂0_old estim.Nk .= estim.He else estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m @@ -367,10 +370,12 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last end estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop - estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0 estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ + estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0_old end - estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] + estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂] + @show estim.X̂0_old + @show estim.x̂0arr_old return ismoving end @@ -450,7 +455,6 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) - println(q̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end @@ -466,33 +470,47 @@ Also init ``\mathbf{F_x̂ = G_x̂ U_0 + J_x̂ D_0 + B_x̂}`` vector for the stat [`init_predmat_mhe`](@ref). """ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel) - Fx̂ = estim.con.Fx̂ - Fx̂ .= estim.con.Bx̂ - mul!(Fx̂, estim.con.Gx̂, estim.U0, 1, 1) - if model.nd > 0 - mul!(Fx̂, estim.con.Jx̂, estim.D0, 1, 1) + nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] + nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*Nk + # --- truncate vector and matrices if necessary --- + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + Bx̂ = estim.con.Bx̂[1:nX̂] + Gx̂, U0 = estim.con.Gx̂[1:nX̂, 1:nU], estim.U0[1:nU] + Jx̂, D0 = estim.con.Jx̂[1:nX̂, 1:nD], estim.D0[1:nD] + Fx̂ = @views estim.con.Fx̂[1:nX̂] + else + Bx̂ = estim.con.Bx̂ + Gx̂, U0 = estim.con.Gx̂, estim.U0 + Jx̂, D0 = estim.con.Jx̂, estim.D0 + Fx̂ = estim.con.Fx̂ end - X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, estim.nx̂) - Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) - V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, estim.nym) - nX̂, nŴ, nV̂ = length(X̂0min), length(Ŵmin), length(V̂min) + X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂) + Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, nŵ) + V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, nym) + # --- update Fx̂ vectors for MHE state constraints --- + Fx̂ .= Bx̂ + mul!(Fx̂, Gx̂, U0, 1, 1) + model.nd > 0 && mul!(Fx̂, Jx̂, D0, 1, 1) + # --- update b vector for linear inequality constraints --- + nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min) nx̃ = length(estim.con.x̃0min) n = 0 estim.con.b[(n+1):(n+nx̃)] .= @. -estim.con.x̃0min n += nx̃ estim.con.b[(n+1):(n+nx̃)] .= @. +estim.con.x̃0max n += nx̃ - estim.con.b[(n+1):(n+nX̂)] .= @. -X̂0min + Fx̂ - n += nX̂ - estim.con.b[(n+1):(n+nX̂)] .= @. +X̂0max - Fx̂ - n += nX̂ - estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin - n += nŴ - estim.con.b[(n+1):(n+nŴ)] .= @. +Ŵmax - n += nŴ - estim.con.b[(n+1):(n+nV̂)] .= @. -V̂min + estim.F - n += nV̂ - estim.con.b[(n+1):(n+nV̂)] .= @. +V̂max - estim.F + estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax + n += nŴ_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. -V̂min + estim.F + n += nV̂_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. +V̂max - estim.F if any(estim.con.i_b) lincon = estim.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) @@ -502,16 +520,18 @@ end "Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)." function linconstraint!(estim::MovingHorizonEstimator, ::SimModel) + # --- truncate vector and matrices if necessary --- Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) - nx̃, nŴ = length(estim.con.x̃0min), length(Ŵmin) + # --- update b vector for linear inequality constraints --- + nx̃, nŴ_He = length(estim.con.x̃0min), length(Ŵmin) n = 0 estim.con.b[(n+1):(n+nx̃)] .= @. -estim.con.x̃0min n += nx̃ estim.con.b[(n+1):(n+nx̃)] .= @. +estim.con.x̃0max n += nx̃ - estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin - n += nŴ - estim.con.b[(n+1):(n+nŴ)] .= @. +Ŵmax + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax if any(estim.con.i_b) lincon = estim.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) @@ -591,10 +611,10 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real x̂0arr, û0, ŷ0, k = buffer.x̂, buffer.û, buffer.ŷ, buffer.k V̂, X̂0 = buffer.V̂, buffer.X̂ estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - getarrival!(x̂0arr, estim, estim.Z̃) + getarrival!(x̂0arr, estim, estim.Z̃) predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) - x̂0next = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂] - estim.x̂0 .= x̂0next + x̂0corrORnext = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂] + estim.x̂0 .= x̂0corrORnext return estim.Z̃ end @@ -1021,23 +1041,23 @@ function setmodel_estimator!( JuMP.unregister(estim.optim, :linconstraint) @constraint(estim.optim, linconstraint, A*Z̃var .≤ b) # --- data windows --- - for i in 1:He - # convert x̂0 to x̂ with the old operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op_old + for i in 1:He # convert y0m to ym with the old operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] # convert u0 to u with the old operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old + estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old # convert d0 to d with the old operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old - # convert x̂ to x̂0 with the new operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op + estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old + # convert x̂0 to x̂ with the old operating point: + estim.X̂0_old[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op_old # convert ym to y0m with the new operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views model.yop[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views model.yop[estim.i_ym] # convert u to u0 with the new operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .-= model.uop + estim.U0[(1+nu*(i-1)):(nu*i)] .-= model.uop # convert d to d0 with the new operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop + estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop + # convert x̂ to x̂0 with the new operating point: + estim.X̂0_old[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op end estim.lastu0 .+= uop_old estim.Z̃[nε+1:nε+nx̂] .+= x̂op_old From 3a30eb57b6cc3b14f2179e7fbc27ef6a5aa450c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 21 May 2026 22:38:44 -0400 Subject: [PATCH 32/56] removed: useless file --- src/estimator/mhe/execute.jl | 6 ++++-- src/estimator/mhe/yo.md | 2 -- 2 files changed, 4 insertions(+), 4 deletions(-) delete mode 100644 src/estimator/mhe/yo.md diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c515fa9dd..68404428a 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -374,8 +374,8 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0_old end estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂] - @show estim.X̂0_old - @show estim.x̂0arr_old + #@show estim.X̂0_old + #@show estim.x̂0arr_old return ismoving end @@ -455,6 +455,8 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) + @show estim.q̃ + display(estim.H̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end diff --git a/src/estimator/mhe/yo.md b/src/estimator/mhe/yo.md deleted file mode 100644 index 35d53f69d..000000000 --- a/src/estimator/mhe/yo.md +++ /dev/null @@ -1,2 +0,0 @@ - The vectors will grows with time until ``N_k = H_e`` is reached. They are also - *artificially* aligned in terms of time steps to ease the user life. But note \ No newline at end of file From a3ab09c64c5f66c9b91a2ffa05b31d252b7dbcf6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 11:00:06 -0400 Subject: [PATCH 33/56] wip: idem --- 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 68404428a..d2a9db872 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -455,7 +455,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) - @show estim.q̃ + display(estim.q̃) display(estim.H̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing From 1c3cf3d721553e599aa4c37452eae85d88ec22ee Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 11:39:01 -0400 Subject: [PATCH 34/56] debug: `D0` window 1 add. sample in `initpred!` --- src/estimator/mhe/execute.jl | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d2a9db872..55d29cacf 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -374,8 +374,6 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0_old end estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂] - #@show estim.X̂0_old - #@show estim.x̂0arr_old return ismoving end @@ -411,7 +409,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He F, C, optim = estim.F, estim.C, estim.optim nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nU, nYm, nD, nŴ = model.nu*Nk, estim.nym*Nk, model.nd*Nk, nŵ*Nk + nU, nYm, nŴ, nD = model.nu*Nk, estim.nym*Nk, nŵ*Nk, model.nd*(Nk+1) nZ̃ = nε + nx̂ + nŴ # --- truncate vector and matrices if necessary --- if Nk < estim.He @@ -455,8 +453,6 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) - display(estim.q̃) - display(estim.H̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end From 42ebcbc4e2d2aeb866c3a93c3a37cd5f8e027561 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 15:19:57 -0400 Subject: [PATCH 35/56] added: custom NL con in MHE now works! --- src/controller/nonlinmpc.jl | 15 ++--- src/controller/transcription.jl | 2 +- src/estimator/mhe/construct.jl | 116 ++++++++++++++++++-------------- src/estimator/mhe/execute.jl | 88 ++++++++++++++++++------ src/general.jl | 13 ++++ 5 files changed, 151 insertions(+), 83 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d8dd217bd..f46baa8d5 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -736,11 +736,11 @@ function init_optimization!( mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real # --- variables and linear constraints --- - con, transcription = mpc.con, mpc.transcription + con = mpc.con nZ̃ = length(mpc.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(mpc.optim, mpc.estim.model.Ts) + limit_solve_time(mpc.optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = con.A[con.i_b, :] b = con.b[con.i_b] @@ -749,15 +749,8 @@ function init_optimization!( beq = con.beq @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- nonlinear optimization init --- - if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt" - C = mpc.weights.Ñ_Hc[end] - try - JuMP.get_attribute(optim, "nlp_scaling_max_gradient") - catch - # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: - JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) - end - end + C = mpc.nϵ > 0 ? mpc.weights.Cwt : Inf + set_scaling_gradient!(optim, C) J_op = get_nonlinobj_op(mpc, optim) g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim) @objective(optim, Min, J_op(Z̃var...)) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fb5b61f9b..66607ae30 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1330,7 +1330,7 @@ The method mutates the `g` vectors in argument and returns it. Only the custom c `gc` are include in the `g` vector. """ function con_nonlinprog!( - g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ + g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _ ) for i in eachindex(g) g[i] = gc[i] diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 1dc9a13f8..490e88dac 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -66,7 +66,8 @@ struct MovingHorizonEstimator{ JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, - HB<:Union{AbstractADType, Nothing}, + HB<:Union{AbstractADType, Nothing}, + PT<:Any, GCfunc<:Function, CE<:KalmanEstimator, } <: StateEstimator{NT} @@ -92,6 +93,7 @@ struct MovingHorizonEstimator{ nym::Int nyu::Int nxs::Int + p::PT As ::Matrix{NT} Cs_u::Matrix{NT} Cs_y::Matrix{NT} @@ -133,7 +135,7 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, cov::KC, Cwt, - gc!::GCfunc, nc, p, + gc!::GCfunc, nc, p::PT, optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; direct=true ) where { @@ -144,6 +146,7 @@ struct MovingHorizonEstimator{ GB<:AbstractADType, JB<:AbstractADType, HB<:Union{AbstractADType, Nothing}, + PT<:Any, GCfunc<:Function, CE<:KalmanEstimator{NT} } @@ -181,7 +184,7 @@ struct MovingHorizonEstimator{ Nk = [0] corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) - estim = new{NT, SM, KC, JM, GB, JB, HB, GCfunc, CE}( + estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}( model, cov, optim, con, @@ -190,6 +193,7 @@ struct MovingHorizonEstimator{ Z̃, lastu0, x̂op, f̂op, x̂0, He, nε, i_ym, nx̂, nym, nyu, nxs, + p, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Ẽ, F, G, J, B, ẽx̄, fx̄, @@ -386,18 +390,18 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: unavailable e.g.: ``\mathbf{u}(k)`` with ``p=0``. They are filled with `NaN` values. The exact time steps of the `NaN`s are detailed in the last column below. - | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | MISSING SAMPLES (NAN) | - | :--------------- | :-------------- | :-------------- | :-------------- | :------------------------- | - | ``\mathbf{X̂_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | — | - | ``\mathbf{V̂_e}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k, k + 1`` | - | ``\mathbf{Ŵ_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k + p - 1, k + p`` | - | ``\mathbf{U_e}`` | `((Nk+1)*nu,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` | - | ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | - | ``\mathbf{D_e}`` | `((Nk+1)*nd,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | - | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | — | - | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | — | - | ``\mathbf{p}`` | var. | — | — | — | - | ``ε`` | `()` | — | — | — | + | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | MISSING SAMPLES | + | :--------------- | :-------------- | :-------------- | :-------------- | :----------------- | + | ``\mathbf{X̂_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | — | + | ``\mathbf{V̂_e}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k, k + 1`` | + | ``\mathbf{Ŵ_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` | + | ``\mathbf{U_e}`` | `((Nk+1)*nu,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` | + | ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | + | ``\mathbf{D_e}`` | `((Nk+1)*nd,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` | + | ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | — | + | ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | — | + | ``\mathbf{p}`` | var. | — | — | — | + | ``ε`` | `()` | — | — | — | If `LHS` represents the result of the left-hand side in the inequality ``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0}``, @@ -405,7 +409,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: 1. **Non-mutating function** (out-of-place): define it as `gc(X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory. - 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yme, De, P̄, + 2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) -> nothing`. This syntax reduces the allocations and potentially the computational burden as well. @@ -921,7 +925,7 @@ function setconstraint!( return estim end -"By default, no nonlinear constraints, return nothing." +"By default, no nonlinear constraints or only custom ones, do and return nothing." reset_nonlincon!(::MovingHorizonEstimator, ::SimModel) = nothing """ @@ -931,7 +935,7 @@ Re-construct nonlinear constraints and add them to `estim.optim`. """ function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel) g_oracle = get_nonlincon_oracle(estim, estim.optim) - set_nonlincon!(estim, model, estim.optim, g_oracle) + set_nonlincon!(estim, estim.optim, g_oracle) end @doc raw""" @@ -1462,15 +1466,21 @@ Init the quadratic optimization of [`MovingHorizonEstimator`](@ref). function init_optimization!( estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel, ) + C, con = estim.C, estim.con nZ̃ = length(estim.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) - A = estim.con.A[estim.con.i_b, :] - b = estim.con.b[estim.con.i_b] + A = con.A[con.i_b, :] + b = con.b[con.i_b] @constraint(optim, linconstraint, A*Z̃var .≤ b) @objective(optim, Min, obj_quadprog(Z̃var, estim.H̃, estim.q̃)) + if con.nc > 0 + set_scaling_gradient!(optim, C) + g_oracle = get_nonlincon_oracle(estim, optim) + set_nonlincon!(estim, optim, g_oracle) + end return nothing end @@ -1491,23 +1501,16 @@ function init_optimization!( JuMP.set_silent(optim) limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) - A = estim.con.A[con.i_b, :] - b = estim.con.b[con.i_b] + A = con.A[con.i_b, :] + b = con.b[con.i_b] @constraint(optim, linconstraint, A*Z̃var .≤ b) # --- nonlinear optimization init --- - if !isinf(C) && JuMP.solver_name(optim) == "Ipopt" - try - JuMP.get_attribute(optim, "nlp_scaling_max_gradient") - catch - # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: - JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) - end - end + set_scaling_gradient!(optim, C) # constraints with vector nonlinear oracle, objective function with splatting: J_op = get_nonlinobj_op(estim, optim) g_oracle = get_nonlincon_oracle(estim, optim) @objective(optim, Min, J_op(Z̃var...)) - set_nonlincon!(estim, model, optim, g_oracle) + set_nonlincon!(estim, optim, g_oracle) return nothing end @@ -1534,23 +1537,28 @@ function get_nonlinobj_op( He = estim.He ng = length(con.i_g) nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) + nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) - x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) + x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) Ŵ::Vector{JNT} = zeros(JNT, nŴ) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + Ŵe::Vector{JNT} = zeros(JNT, nŴe) + V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) k::Vector{JNT} = zeros(JNT, nk) - û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call J_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), - Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), + Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He @@ -1641,30 +1649,35 @@ function get_nonlincon_oracle( ng, ngi = length(con.i_g), sum(con.i_g) nc = con.nc nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) + nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) Ŵ::Vector{JNT} = zeros(JNT, nŴ) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + Ŵe::Vector{JNT} = zeros(JNT, nŴe) + V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, gi) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, gi) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇gi_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) # temporarily "fill" the estimation window for the preparation of the gradient: @@ -1675,7 +1688,9 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi) ) estim.Nk[] = He # see comment above @@ -1722,16 +1737,13 @@ function get_nonlincon_oracle( return g_oracle end -"By default, there is no nonlinear constraint, thus do nothing." -set_nonlincon!(::MovingHorizonEstimator, ::SimModel, _ , _ ) = nothing - """ - set_nonlincon!(estim::MovingHorizonEstimator, ::NonLinModel, optim, g_oracle) + set_nonlincon!(estim::MovingHorizonEstimator, optim, g_oracle) -Set the nonlinear inequality constraints for `NonLinModel`, if any. +Set the nonlinear inequality constraints of `estim`, if any. """ function set_nonlincon!( - estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT}, g_oracle + estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT}, g_oracle ) where JNT<:Real Z̃var = optim[:Z̃var] nonlin_constraints = JuMP.all_constraints( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 55d29cacf..f7c682509 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -883,7 +883,7 @@ end """ update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -892,32 +892,67 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_prediction!( - x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ ) - model = estim.model - x̂0arr = getarrival!(x̂0arr, estim, Z̃) - x̄ .= estim.x̂0arr_old .- x̂0arr - Ŵ = getŴ!(Ŵ, estim, Z̃) - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) - ε = getε(estim, Z̃) - # gc = con_custom_mhe!(gc, estim, V̂, X̂0, Z̃, x̄, ε) - g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, gc, ε) + x̂0arr = getarrival!(x̂0arr, estim, Z̃) + x̄ .= estim.x̂0arr_old .- x̂0arr + Ŵ = getŴ!(Ŵ, estim, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, estim.model, x̂0arr, Ŵ, Z̃) + Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) + ε = getε(estim, Z̃) + gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) + g = con_nonlinprog_mhe!(g, estim, estim.model, X̂0, V̂, gc, ε) return nothing end +""" + extended_vectors!( + Ŵe, V̂e, X̂e, estim::MovingHorizonEstimator, Ŵ, V̂, X̂0, x̂0arr + ) -> Ŵe, V̂e, X̂e + +Compute the extended `Ŵe, V̂e` and `X̂e` vectors for NLP using the `Ŵ, V̂` and `X̂0` vectors. +See [`MovingHorizonEstimator`](@ref) for the definition of the vectors, the exact time +steps of the samples in them and the missing values with `NaN`s. The method mutates all +the arguments before `estim` argument. """ - con_custom_mhe!(gc, estim::MovingHorizonEstimator, V̂, X̂0, Z̃, x̄, ε) -> gc +function extended_vectors!(Ŵe, V̂e, X̂e, estim::MovingHorizonEstimator, Ŵ, V̂, X̂0, x̂0arr) + nym, nŵ, nx̂ = estim.nym, estim.nx̂, estim.nx̂ + Ŵe[1:end-nŵ] .= Ŵ + Ŵe[end-nŵ+1:end] .= NaN + X̂e[1:nx̂] .= x̂0arr .+ estim.x̂op + X̂e[nx̂+1:end] .= X̂0 .+ estim.X̂op + if estim.direct + V̂e[1:nym] .= NaN + V̂e[1+nym:end] .= V̂ + else + V̂e[1:end-nym] .= V̂ + V̂e[end-nym+1:end] .= NaN + end + return Ŵe, V̂e, X̂e +end + + +""" + con_custom_mhe!(gc, estim::MovingHorizonEstimator, X̂e, V̂e, Ŵe, x̄, ε) -> gc Evaluate the custom inequality constraint `gc` in-place for [`MovingHorizonEstimator`](@ref). """ -function con_custom_mhe!(gc, estim::MovingHorizonEstimator, V̂, X̂0, Z̃, x̄, ε) +function con_custom_mhe!(gc, estim::MovingHorizonEstimator, X̂e, V̂e, Ŵe, x̄, ε) if estim.con.nc > 0 P̄ = estim.P̂arr_old - nx̂, nε, Nk = estim.nx̂, estim.nε, estim.Nk[] - nx̃ = nε + nx̂ - X̂ = [x̂0arr .+ estim.x̂op; X̂0 .+ estim.X̂op] - estim.con.gc!(gc, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ε) + Nk = estim.Nk[] + Ue, Yem, De = estim.Ue, estim.Yem, estim.De + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + nX̂e, nŴe, nYem = (Nk+1)*estim.nx̂, (Nk+1)*estim.nx̂, (Nk+1)*estim.nym + nUe, nDe = (Nk+1)*estim.model.nu, (Nk+1)*estim.model.nd + Ue, Yem, De = estim.Ue[1:nUe], estim.Yem[1:nYem], estim.De[1:nDe] + X̂e, V̂e, Ŵe = X̂e[1:nX̂e], V̂e[1:nYem], Ŵe[1:nŴe] + else + Ue, Yem, De = estim.Ue, estim.Yem, estim.De + end + estim.con.gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, estim.p, ε) end return gc end @@ -946,17 +981,32 @@ function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, j = i - 2nX̂con jcon = nV̂con-nV̂+j g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] - else + elseif i ≤ 2nX̂con + 2nV̂con j = i - 2nX̂con - nV̂con jcon = nV̂con-nV̂+j g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] + else + j = i - 2nX̂con - 2nV̂con + g[i] = gc[j] end end return g end -"No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ , _ ) = g +""" + con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , gc, _ ) + +Compute the same but for [`LinModel`](@ref). + +The nonlinear custom inequality constraints in `gc` are the only nonlinear constraints +for this case. +""" +function con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , gc , _ ) + for i in eachindex(g) + g[i] = gc[i] + end + return g +end "Throw an error if P̂ != nothing." function setstate_cov!(::MovingHorizonEstimator, P̂) diff --git a/src/general.jl b/src/general.jl index 6329ec80b..451c8fb40 100644 --- a/src/general.jl +++ b/src/general.jl @@ -86,6 +86,19 @@ function limit_solve_time(optim::GenericModel, Ts) end end +"Set the maximum gradient scaling in `optim` to `10.0/C` if optimizer is `Ipopt`." +function set_scaling_gradient!(optim::JuMP.GenericModel, C) + if !isinf(C) && JuMP.solver_name(optim) == "Ipopt" + try + JuMP.get_attribute(optim, "nlp_scaling_max_gradient") + catch + # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: + JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) + end + end + return nothing +end + "Init a differentiation result matrix as dense or sparse matrix, as required by `backend`." init_diffmat(T, ::AbstractADType, _ , nx, ny) = zeros(T, ny, nx) function init_diffmat(T, ::AutoSparse, prep , _ , _ ) From 4a79e8b52a25135e79a474503edb2fe2346d5219 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 15:44:16 -0400 Subject: [PATCH 36/56] =?UTF-8?q?debug:=20`NonLinMPC`=20slack=20weight=20i?= =?UTF-8?q?s=20in=20`N=CC=83=5FHc`=20field?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/nonlinmpc.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f46baa8d5..c2e07b646 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -749,7 +749,7 @@ function init_optimization!( beq = con.beq @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- nonlinear optimization init --- - C = mpc.nϵ > 0 ? mpc.weights.Cwt : Inf + C = mpc.nϵ > 0 ? mpc.weights.Ñ_Hc[end, end] : Inf set_scaling_gradient!(optim, C) J_op = get_nonlinobj_op(mpc, optim) g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim) From 4dd600fe51a05612022862e15fd5a37309a35d35 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 15:45:06 -0400 Subject: [PATCH 37/56] added: simple test of the custom constraint function for MHE --- src/estimator/mhe/construct.jl | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 490e88dac..bd4501910 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -183,6 +183,7 @@ struct MovingHorizonEstimator{ P̂arr_old = copy(cov.P̂_0) Nk = [0] corrected = [false] + test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}( model, @@ -647,19 +648,23 @@ function get_mutating_gc_mhe(NT, gc) end """ - test_custom_function_mhe(NT, model::SimModel, gc!, nc, X̂op, Ymop, Uop, Dop, p) -> nothing + test_custom_function_mhe(NT, model::SimModel, i_ym, He, gc!, nc, x̂op, p) -> nothing -Test the custom functions `gc!` at the operating points +Test the custom functions `gc!` at the operating points. This function is called at the end of `MovingHorizonEstimator` construction. It warns the -user if the custom constraint `gc!` functions crash at `model` operating points. This +user if the custom constraint `gc!` function crashes at `model` operating points. This should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. """ -function test_custom_function_mhe(NT, model::SimModel, gc!, nc, X̂op, Ymop, Uop, Dop, p) +function test_custom_function_mhe(NT, model::SimModel, i_ym, He, gc!, nc, x̂op, p) + nŵ, nym = length(x̂op), length(i_ym) uop, dop, yop = model.uop, model.dop, model.yop + yopm = yop[i_ym] + X̂e, V̂e, Ŵe = repeat(x̂op, He+1), zeros(NT, (He+1)*nym), zeros(NT, (He+1)*nŵ) + Ue, Yem, De = repeat(uop, He+1), repeat(yopm, He+1), repeat(dop, He+1) gc = Vector{NT}(undef, nc) try - gc!(gc, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, 0.0) + gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, I, x̂op, p, zero(NT)) catch err @warn( """ From ef2c67fe3b764eaadd401f9b78bd6bbf252e7230 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 16:14:49 -0400 Subject: [PATCH 38/56] changed: better testing function for MHE --- src/estimator/mhe/construct.jl | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index bd4501910..efb2ab554 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -653,24 +653,35 @@ end Test the custom functions `gc!` at the operating points. This function is called at the end of `MovingHorizonEstimator` construction. It warns the -user if the custom constraint `gc!` function crashes at `model` operating points. This -should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. +user if the custom constraint `gc!` function crashes at `model` operating points. It +will also verify the custom function work with the growing windows. It should ease +troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. """ function test_custom_function_mhe(NT, model::SimModel, i_ym, He, gc!, nc, x̂op, p) - nŵ, nym = length(x̂op), length(i_ym) + nx̂, nŵ, nym = length(x̂op), length(x̂op), length(i_ym) + nu, nd = model.nu, model.nd uop, dop, yop = model.uop, model.dop, model.yop yopm = yop[i_ym] - X̂e, V̂e, Ŵe = repeat(x̂op, He+1), zeros(NT, (He+1)*nym), zeros(NT, (He+1)*nŵ) - Ue, Yem, De = repeat(uop, He+1), repeat(yopm, He+1), repeat(dop, He+1) + X̂e_He, V̂e_He, Ŵe_He = repeat(x̂op, He+1), zeros(NT, (He+1)*nym), zeros(NT, (He+1)*nŵ) + Ue_He, Yem_He, De_He = repeat(uop, He+1), repeat(yopm, He+1), repeat(dop, He+1) + x̄ = zeros(NT, nx̂) + P̄ = Hermitian(Matrix{NT}(I, 4, 4), :L) + ε = zero(NT) gc = Vector{NT}(undef, nc) try - gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, I, x̂op, p, zero(NT)) + for i in 2:He+1 + X̂e, V̂e, Ŵe = X̂e_He[1:(i*nx̂)], V̂e_He[1:(i*nym)], Ŵe_He[1:(i*nŵ)] + Ue, Yem, De = Ue_He[1:(i*nu)], Yem_He[1:(i*nym)], De_He[1:(i*nd)] + gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + end catch err @warn( """ - Calling the gc function with Ue, Ŷe, D̂e, ϵ arguments fixed at uop=$uop, - yop=$yop, dop=$dop, ϵ=0 failed with the following stacktrace. Did you - forget to set the keyword argument p or nc? + Calling the gc function with X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, ε arguments + fixed at x̂op=$x̂op, uop=$uop, yop=$yop, dop=$dop, + P̄=I, x̄=0, p=$p, ϵ=0 failed with the following stacktrace. + Did you forget to set the keyword argument p or nc? + Did you handle the growing data windows in your function? """, exception=(err, catch_backtrace()) ) From ee2d68180612d6047b85be9e28f3364cf2906360 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 17:17:16 -0400 Subject: [PATCH 39/56] debug: `getinfo` for MHE with `NonLinModel` --- src/estimator/mhe/execute.jl | 41 ++++++++++++++++++++++-------------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index f7c682509..795f9cf95 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -208,19 +208,24 @@ function addinfo!( i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(con.i_g), sum(con.i_g) nV̂, nX̂, nŴ = He*nym, He*nx̂, He*nx̂ - x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) - V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) - Ŵ = zeros(NT, nŴ) - k = zeros(NT, nk) - û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) - gc, g = zeros(NT, nc), zeros(NT, ng) - gi = zeros(NT, ngi) + nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym + x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) + Ŵ = zeros(NT, nŴ) + V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) + Ŵe = zeros(NT, nŴe) + V̂e, X̂e = zeros(NT, nV̂e), zeros(NT, nX̂e) + k = zeros(NT, nk) + û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) + gc, g = zeros(NT, nc), zeros(NT, ng) + gi = zeros(NT, ngi) J_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end if !isnothing(hess) @@ -234,11 +239,13 @@ function addinfo!( end # --- inequality constraint derivatives --- ∇g_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), ) - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return nothing end @@ -261,11 +268,13 @@ function addinfo!( end end ∇²g_cache = ( - Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(x̂0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), + Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi) ) - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, gi) - update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃) + function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, gi) + update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end From 815f8245d63e1513aa0a21491eb638d04b158450 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 21:29:24 -0400 Subject: [PATCH 40/56] test: correct field --- test/2_test_state_estim.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b1dbabf00..46a80e30f 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -848,7 +848,7 @@ end mhe7 = MovingHorizonEstimator(linmodel, He=10) @test mhe7.He == 10 - @test length(mhe7.X̂0) == mhe7.He*6 + @test length(mhe7.X̂0_old) == mhe7.He*6 @test length(mhe7.Y0m) == mhe7.He*2 @test length(mhe7.U0) == mhe7.He*2 @test length(mhe7.D0) == (mhe7.He+mhe7.direct)*1 @@ -1402,7 +1402,7 @@ end setmodel!(mhe, newlinmodel) @test mhe.x̂0 ≈ [3.0 - 8.0] @test mhe.Z̃[1] ≈ 3.0 - 8.0 - @test mhe.X̂0 ≈ repeat([3.0 - 8.0], He) + @test mhe.X̂0_old ≈ repeat([3.0 - 8.0], He) @test mhe.x̂0arr_old ≈ [3.0 - 8.0] @test mhe.con.X̂0min ≈ repeat([-1000 - 8.0], He) @test mhe.con.X̂0max ≈ repeat([+1000 - 8.0], He) From a8d8ad1281038696f0dcd642f14dd67ee3965aba Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 21:35:17 -0400 Subject: [PATCH 41/56] debug: avoid deprecated function in `FastGaussQuadrature` --- src/controller/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 66607ae30..34c94fcef 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -190,11 +190,11 @@ struct OrthogonalCollocation <: CollocationMethod throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end if roots==:gaussradau - x, _ = FastGaussQuadrature.gaussradau(no, COLLOCATION_NODE_TYPE) + x, _ = FastGaussQuadrature.gaussradau(COLLOCATION_NODE_TYPE, no) # we reverse the nodes to include the τ=1.0 node: τ = (reverse(-x) .+ 1) ./ 2 elseif roots==:gausslegendre - x, _ = FastGaussQuadrature.gausslegendre(no) + x, _ = FastGaussQuadrature.gausslegendre(COLLOCATION_NODE_TYPE, no) # converting [-1, 1] to [0, 1] (see # https://en.wikipedia.org/wiki/Gaussian_quadrature#Change_of_interval): τ = (x .+ 1) ./ 2 From 7c0a70aa7c6209a9af29a56f701c80216c14a4c5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 22:20:58 -0400 Subject: [PATCH 42/56] Revert "debug: avoid deprecated function in `FastGaussQuadrature`" This reverts commit a8d8ad1281038696f0dcd642f14dd67ee3965aba. --- src/controller/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 34c94fcef..66607ae30 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -190,11 +190,11 @@ struct OrthogonalCollocation <: CollocationMethod throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end if roots==:gaussradau - x, _ = FastGaussQuadrature.gaussradau(COLLOCATION_NODE_TYPE, no) + x, _ = FastGaussQuadrature.gaussradau(no, COLLOCATION_NODE_TYPE) # we reverse the nodes to include the τ=1.0 node: τ = (reverse(-x) .+ 1) ./ 2 elseif roots==:gausslegendre - x, _ = FastGaussQuadrature.gausslegendre(COLLOCATION_NODE_TYPE, no) + x, _ = FastGaussQuadrature.gausslegendre(no) # converting [-1, 1] to [0, 1] (see # https://en.wikipedia.org/wiki/Gaussian_quadrature#Change_of_interval): τ = (x .+ 1) ./ 2 From c4c967264a080f23dfbe257f935cea289e57cfbc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 23:20:07 -0400 Subject: [PATCH 43/56] debug: correctly initialize `mhe.D0` window --- src/estimator/mhe/construct.jl | 1 + src/estimator/mhe/execute.jl | 11 ++++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index efb2ab554..99ec178bc 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -179,6 +179,7 @@ struct MovingHorizonEstimator{ D0, De = fill(NT(NaN), nd*(He+1)), fill(NT(NaN), nd*(He+1)) Ŵ = fill(NT(NaN), nx̂*He) X̂0_old = fill(NT(NaN), nx̂*He) + D0[1:nd] .= 0 # D0 start with d0(-1) and it should not be NaN x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(cov.P̂_0) Nk = [0] diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 795f9cf95..81b7986f6 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -18,14 +18,14 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.con.Fx̂ .= 0 if estim.direct # add y0m(-1) to the extended data window (custom NL constraints): - estim.Yem[1:model.ny] .= y0m .+ @views model.yop[estim.i_ym] + estim.Yem[1:ny] .= y0m .+ @views yop[estim.i_ym] # add u0(-1) to the two data windows: - estim.U0[1:model.nu] .= u0 - estim.Ue[1:model.nu] .= u0 .+ model.uop + estim.U0[1:nu] .= u0 + estim.Ue[1:nu] .= u0 .+ uop # add d0(-1) to the extended data window (custom NL constraints): - model.nd > 0 && (estim.De[1:model.nd] .= d0 .+ model.dop) + nd > 0 && (estim.De[1:nd] .= d0 .+ dop) end - model.nd > 0 && (estim.D0[1:model.nd] .= d0) # add d0(-1) to the data window + nd > 0 && (estim.D0[1:nd] .= d0) # add d0(-1) to the data window estim.lastu0 .= u0 # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) invert_cov!(estim, estim.cov.P̂_0) @@ -462,6 +462,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) + println(q̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end From abbbaa86110dfbe7d3b4a707b0fc74e968c5efc0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 23:31:27 -0400 Subject: [PATCH 44/56] debug: extract fields --- src/estimator/mhe/execute.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 81b7986f6..d4cc7eb61 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -1,6 +1,7 @@ "Reset the data windows and time-varying variables for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model + nu, ny, nd = model.nu, model.ny, model.nd estim.Z̃ .= 0 estim.Y0m .= NaN estim.Yem .= NaN From c9e4c5691b126b64a58f0b5f9e946f10838b1537 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 22 May 2026 23:48:37 -0400 Subject: [PATCH 45/56] debug: idem --- src/estimator/mhe/execute.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d4cc7eb61..1f785b464 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -2,6 +2,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) model = estim.model nu, ny, nd = model.nu, model.ny, model.nd + uop, yop, dop = model.uop, model.yop, model.dop estim.Z̃ .= 0 estim.Y0m .= NaN estim.Yem .= NaN From 5e3a996f088d71d45d9429493956dfdaee4fa17e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 00:03:07 -0400 Subject: [PATCH 46/56] test: debug --- test/2_test_state_estim.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 46a80e30f..ccb01b045 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1402,7 +1402,7 @@ end setmodel!(mhe, newlinmodel) @test mhe.x̂0 ≈ [3.0 - 8.0] @test mhe.Z̃[1] ≈ 3.0 - 8.0 - @test mhe.X̂0_old ≈ repeat([3.0 - 8.0], He) + @test mhe.X̂0_old[1] ≈ 3.0 - 8.0 @test mhe.x̂0arr_old ≈ [3.0 - 8.0] @test mhe.con.X̂0min ≈ repeat([-1000 - 8.0], He) @test mhe.con.X̂0max ≈ repeat([+1000 - 8.0], He) From 17fd2f7767a7f33c9ac0149b24694d7b14c99f14 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 09:39:33 -0400 Subject: [PATCH 47/56] test: check op pts only beginning of windows --- test/2_test_state_estim.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index ccb01b045..1ce97a80e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1394,8 +1394,8 @@ end @test mhe. ≈ [0.2] @test evaloutput(mhe) ≈ [55.0] @test mhe.lastu0 ≈ [2.0 - 3.0] - @test mhe.U0 ≈ repeat([2.0 - 3.0], He) - @test mhe.Y0m ≈ repeat([50.0 - 55.0], He) + @test mhe.U0[1] ≈ 2.0 - 3.0 + @test mhe.Y0m[1] ≈ 50.0 - 55.0 x̂ = preparestate!(mhe, [55.0]) @test x̂ ≈ [3.0] newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0]) From b92be5919a6954314043c826bb8ef1105ed57806 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 10:48:50 -0400 Subject: [PATCH 48/56] debug: `mhe.D0` contains one add. sample --- 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 1f785b464..c504b6fa8 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -464,7 +464,6 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) H̃_data .= Ñ_Nk mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) lmul!(2, H̃_data) - println(q̃) JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end @@ -481,7 +480,7 @@ Also init ``\mathbf{F_x̂ = G_x̂ U_0 + J_x̂ D_0 + B_x̂}`` vector for the stat """ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel) nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] - nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*Nk + nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*(Nk+1) # --- truncate vector and matrices if necessary --- if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul!: @@ -623,7 +622,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start getarrival!(x̂0arr, estim, estim.Z̃) predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) - x̂0corrORnext = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂] + x̂0corrORnext = @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] estim.x̂0 .= x̂0corrORnext return estim.Z̃ end From ff923d280ffc50be895fef243242effba8ce8541 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 12:25:59 -0400 Subject: [PATCH 49/56] debug: use `gc!` instead of `gc` in MHE construction --- src/estimator/mhe/construct.jl | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 99ec178bc..9193bd9e2 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -570,7 +570,7 @@ function MovingHorizonEstimator( return MovingHorizonEstimator{NT}( model, He, i_ym, nint_u, nint_ym, cov, Cwt, - gc, nc, p, + gc!, nc, p, optim, gradient, jacobian, hessian, covestim; direct ) @@ -604,30 +604,30 @@ function validate_gc_mhe(NT, gc) ismutating = hasmethod( gc, Tuple{ - # LHS, , X̂ V̂ , Ŵ, + # LHS, , X̂e , V̂e , Ŵe Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, - # U , Ym , D , P̄ , x̄ , p , ε + # Ue , Yem , De , P̄ , x̄ , p , ε Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT } ) isnonmutating = hasmethod( gc, Tuple{ - # X̂ V̂ , Ŵ, + # X̂e , V̂e , Ŵe Vector{NT}, Vector{NT}, Vector{NT}, - # U , Ym , D , P̄ , x̄ , p , ε + # Ue , Yem , De , P̄ , x̄ , p , ε Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT } ) if !(ismutating || isnonmutating) error( "the custom constraint function has no method with type signature "* - "gc(X̂::Vector{$(NT)}, V̂::Vector{$(NT)}, Ŵ::Vector{$(NT)}, "* - "U::Vector{$(NT)}, Ym::Vector{$(NT)}, D::Vector{$(NT)}, "* + "gc(X̂e::Vector{$(NT)}, V̂e::Vector{$(NT)}, Ŵe::Vector{$(NT)}, "* + "Ue::Vector{$(NT)}, Yem::Vector{$(NT)}, De::Vector{$(NT)}, "* "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT)) "* "or mutating form gc!(LHS::Vector{$(NT)}, "* - "X̂::Vector{$(NT)}, V̂::Vector{$(NT)}, Ŵ::Vector{$(NT)}, "* - "U::Vector{$(NT)}, Ym::Vector{$(NT)}, D::Vector{$(NT)}, "* + "X̂e::Vector{$(NT)}, V̂e::Vector{$(NT)}, Ŵe::Vector{$(NT)}, "* + "Ue::Vector{$(NT)}, Yem::Vector{$(NT)}, De::Vector{$(NT)}, "* "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT))" ) end @@ -637,11 +637,13 @@ end "Get mutating custom constraint function `gc!` from the provided function in argument." function get_mutating_gc_mhe(NT, gc) ismutating_gc = validate_gc_mhe(NT, gc) + @show ismutating_gc gc! = if ismutating_gc gc else - function gc!(LHS, X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ϵ) - LHS .= gc(X̂, V̂, Ŵ, U, Ym, D, P̄, x̄, p, ϵ) + function gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ) + println("ASDS") + LHS .= gc(X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ) return nothing end end From 339ca24210eae483ee07066ef8055c6e66fc53d0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 12:35:30 -0400 Subject: [PATCH 50/56] removed: useless print --- src/estimator/mhe/construct.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 9193bd9e2..d81228d9c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -637,12 +637,10 @@ end "Get mutating custom constraint function `gc!` from the provided function in argument." function get_mutating_gc_mhe(NT, gc) ismutating_gc = validate_gc_mhe(NT, gc) - @show ismutating_gc gc! = if ismutating_gc gc else function gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ) - println("ASDS") LHS .= gc(X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ) return nothing end From e18ab072ffcb6c9ea63d0b78081a3eb2a724ed65 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 23 May 2026 12:46:06 -0400 Subject: [PATCH 51/56] test: wip new tests for MHE custom constraints --- test/2_test_state_estim.jl | 136 +++++++++++++++++++++++++++++++++++++ 1 file changed, 136 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 1ce97a80e..8cf92a61b 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1566,6 +1566,142 @@ end @test X̂_lin ≈ X̂_nonlin atol=1e-3 rtol=1e-3 end + +@testitem "MovingHorizonEstimator construction with custom constraint (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint: simple bound on states using only X̂e and ε + function gc_lin(X̂e, _ , _ , _ , _ , _ , _ , _ ,nX̂e , ε) + gc = X̂e .- 100 .- ε # all states must be <= 100 + return [gc; zeros(nX̂e .- length(X̂e))] + end + He = 3 + nx̂ = length(linmodel.A) + 2 # number of states (with augmentation) + nc = nx̂ * (He+1) # Approximate constraint count for He=3 + nX̂e = nx̂ * (He+1) + mhe = MovingHorizonEstimator(linmodel, Cwt=1e5, He=He, gc=gc_lin, nc=nc, p=nX̂e) + + @test mhe.con.nc == nc + @test mhe.nε > 0 + @test mhe.He == 3 +end + +@testitem "MovingHorizonEstimator custom constraint violation (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using only X̂e and ε + # This constraint enforces that the first state must be >= 0.5 + function gc_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 4 # number of states (with augmentation) + # Extract and constrain first state at each time step + for i in 1:div(length(X̂e), nx̂) + LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 + end + return nothing + end + + nc = 3 # One constraint per time step (He=1 initially, but grows) + mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_constraint!, nc=nc, Cwt=1e3, nint_ym=0) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (first state should be >= 0.5) + @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint violation (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using only X̂e and ε + # This constraint enforces that the first state must be >= 0.5 + function gc_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 6 # number of states (with augmentation) + # Extract and constrain first state at each time step + for i in 1:div(length(X̂e), nx̂) + LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 + end + return nothing + end + + nc = 3 # One constraint per time step + optim = Model(Ipopt.Optimizer) + mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (first state should be >= 0.5) + @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint on noise (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using Ŵe and ε + # This constraint enforces that process noise must be <= 0 + function gc_noise_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 4 # number of states (with augmentation) + # Constraint: all process noise components must be <= 0 + for i in 1:length(Ŵe) + LHS[i] = Ŵe[i] # Ŵe <= 0 + end + return nothing + end + + nc = 4 * 2 # Two time steps with 4 states each + mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_noise_constraint!, nc=nc, Cwt=1e3, nint_ym=0) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (process noise should be <= 0) + @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint on noise (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using Ŵe and ε + # This constraint enforces that process noise must be <= 0 + function gc_noise_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 6 # number of states (with augmentation) + # Constraint: all process noise components must be <= 0 + for i in 1:length(Ŵe) + LHS[i] = Ŵe[i] # Ŵe <= 0 + end + return nothing + end + + nc = 6 * 2 # Two time steps with 6 states each + optim = Model(Ipopt.Optimizer) + mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_noise_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (process noise should be <= 0) + @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance +end + @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) From f26344ff6109782fe73d22137060926577c2d999 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 24 May 2026 16:04:52 -0400 Subject: [PATCH 52/56] debug: fill MHE predictios with 0s when Nk < He --- src/estimator/mhe/execute.jl | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c504b6fa8..b67cc026d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -857,6 +857,10 @@ function predict_mhe!( ) nu, nd, nx̂, nŵ, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] x̂0 = x̂0arr + if Nk < estim.He + V̂ .= 0 # fill unused values a 0 for tracer sparsity detection + X̂0 .= 0 + end if estim.direct # p = 0 ŷ0next = ŷ0 d0 = @views estim.D0[1:nd] From 9df75ed8ccd523fa700c4d810a212d40e530b131 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 24 May 2026 16:05:42 -0400 Subject: [PATCH 53/56] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 9acd22334..742ac0fad 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.3.2" +version = "2.4.0" authors = ["Francis Gagnon"] [deps] From d7f752e63ea55b3260f03808588facce49c48ee4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 24 May 2026 17:12:19 -0400 Subject: [PATCH 54/56] test: cleanup and rework MHE custom constraints --- test/2_test_state_estim.jl | 206 ++++++++++++------------------------- 1 file changed, 68 insertions(+), 138 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 8cf92a61b..d0e5ef426 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -868,6 +868,18 @@ end mhe13 = MovingHorizonEstimator(linmodel2, He=5) @test isa(mhe13, MovingHorizonEstimator{Float32}) + function gcl(X̂e, _ , _ , _ , _ , _ , _ , _ , nx, ε) + gc = X̂e .- 100 .- ε + return [gc; zeros(nc .- length(X̂e))] + end + He = 3 + nx̂ = linmodel.nx + nc = nx̂*(He+1) + mhe = MovingHorizonEstimator(linmodel, nint_ym=0, Cwt=1e5, He=He, gc=gcl, nc=nc, p=nc) + @test mhe.con.nc == nc + @test mhe.nε > 0 + @test mhe.He == 3 + @test_throws ArgumentError MovingHorizonEstimator(linmodel) @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) @@ -1324,8 +1336,8 @@ end info = getinfo(mhe) @test info[:V̂] ≈ [-1,-1] atol=5e-2 - f(x,u,_,model) = model.A*x + model.Bu*u - h(x,_,model) = model.C*x + f = (x,u,_,model) -> model.A*x + model.Bu*u + h = (x,_,model) -> model.C*x nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) @@ -1374,6 +1386,60 @@ end info = getinfo(mhe2) @test info[:V̂] ≈ [-1,-1] atol=5e-2 + linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5]) + function gclv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _ ) + for i in 1:div(length(X̂e), nx̂) + LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 + end + return nothing + end + nx̂ = linmodel2.nx + nc = 2 + mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gclv!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test x̂[1] ≈ 0.5 atol = 5e-2 + + function gcln!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _) + LHS .= Ŵe[1:nx̂] + return nothing + end + nx̂ = linmodel2.nx + nc = linmodel2.nx + mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gcln!, nc=nc, nint_ym=0, direct=false, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2 + + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel2 = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2) + nonlinmodel2 = setop!(nonlinmodel2, uop=[10,50], yop=[50,30], dop=[5]) + function gcnlv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _) + for i in 1:div(length(X̂e), nx̂) + LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 + end + return nothing + end + nc = 2 + nx̂ = nonlinmodel2.nx + mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gcnlv!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test x̂[1] ≈ 0.5 atol = 5e-2 + + function gclnl!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _) + LHS .= Ŵe[1:nx̂] + return nothing + end + nx̂ = nonlinmodel2.nx + nc = nonlinmodel2.nx + mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gclnl!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2 + end @testitem "MovingHorizonEstimator set model" setup=[SetupMPCtests] begin @@ -1566,142 +1632,6 @@ end @test X̂_lin ≈ X̂_nonlin atol=1e-3 rtol=1e-3 end - -@testitem "MovingHorizonEstimator construction with custom constraint (LinModel)" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) - linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) - - # Custom constraint: simple bound on states using only X̂e and ε - function gc_lin(X̂e, _ , _ , _ , _ , _ , _ , _ ,nX̂e , ε) - gc = X̂e .- 100 .- ε # all states must be <= 100 - return [gc; zeros(nX̂e .- length(X̂e))] - end - He = 3 - nx̂ = length(linmodel.A) + 2 # number of states (with augmentation) - nc = nx̂ * (He+1) # Approximate constraint count for He=3 - nX̂e = nx̂ * (He+1) - mhe = MovingHorizonEstimator(linmodel, Cwt=1e5, He=He, gc=gc_lin, nc=nc, p=nX̂e) - - @test mhe.con.nc == nc - @test mhe.nε > 0 - @test mhe.He == 3 -end - -@testitem "MovingHorizonEstimator custom constraint violation (LinModel)" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) - linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) - - # Custom constraint function using only X̂e and ε - # This constraint enforces that the first state must be >= 0.5 - function gc_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) - nx̂ = 4 # number of states (with augmentation) - # Extract and constrain first state at each time step - for i in 1:div(length(X̂e), nx̂) - LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 - end - return nothing - end - - nc = 3 # One constraint per time step (He=1 initially, but grows) - mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_constraint!, nc=nc, Cwt=1e3, nint_ym=0) - - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - # The constraint should be satisfied (first state should be >= 0.5) - @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance -end - -@testitem "MovingHorizonEstimator custom constraint violation (NonLinModel)" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt - linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) - linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) - - f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d - h = (x,d,model) -> model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) - nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) - - # Custom constraint function using only X̂e and ε - # This constraint enforces that the first state must be >= 0.5 - function gc_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) - nx̂ = 6 # number of states (with augmentation) - # Extract and constrain first state at each time step - for i in 1:div(length(X̂e), nx̂) - LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 - end - return nothing - end - - nc = 3 # One constraint per time step - optim = Model(Ipopt.Optimizer) - mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) - - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - # The constraint should be satisfied (first state should be >= 0.5) - @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance -end - -@testitem "MovingHorizonEstimator custom constraint on noise (LinModel)" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) - linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) - - # Custom constraint function using Ŵe and ε - # This constraint enforces that process noise must be <= 0 - function gc_noise_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) - nx̂ = 4 # number of states (with augmentation) - # Constraint: all process noise components must be <= 0 - for i in 1:length(Ŵe) - LHS[i] = Ŵe[i] # Ŵe <= 0 - end - return nothing - end - - nc = 4 * 2 # Two time steps with 4 states each - mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_noise_constraint!, nc=nc, Cwt=1e3, nint_ym=0) - - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - # The constraint should be satisfied (process noise should be <= 0) - @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance -end - -@testitem "MovingHorizonEstimator custom constraint on noise (NonLinModel)" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt - linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) - linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) - - f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d - h = (x,d,model) -> model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) - nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) - - # Custom constraint function using Ŵe and ε - # This constraint enforces that process noise must be <= 0 - function gc_noise_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) - nx̂ = 6 # number of states (with augmentation) - # Constraint: all process noise components must be <= 0 - for i in 1:length(Ŵe) - LHS[i] = Ŵe[i] # Ŵe <= 0 - end - return nothing - end - - nc = 6 * 2 # Two time steps with 6 states each - optim = Model(Ipopt.Optimizer) - mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_noise_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) - - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - # The constraint should be satisfied (process noise should be <= 0) - @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance -end - @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) From 0c979c905f3b35ebc34b7992d7e4734a9b039d48 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 24 May 2026 17:16:29 -0400 Subject: [PATCH 55/56] test: shorter title --- test/2_test_state_estim.jl | 64 +++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index d0e5ef426..2403898a4 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1,4 +1,4 @@ -@testitem "SteadyKalmanFilter construction" setup=[SetupMPCtests] begin +@testitem "SKF construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) kalmanfilter1 = SteadyKalmanFilter(linmodel) @@ -61,7 +61,7 @@ @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_u=[1,1], nint_ym=[1,1]) end -@testitem "SteadyKalmanFilter estimator methods" setup=[SetupMPCtests] begin +@testitem "SKF estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) kalmanfilter1 = SteadyKalmanFilter(linmodel, nint_ym=[1, 1]) @@ -114,7 +114,7 @@ end @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50]) end -@testitem "SteadyKalmanFilter set model" setup=[SetupMPCtests] begin +@testitem "SKF set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -125,7 +125,7 @@ end @test_throws ErrorException setmodel!(skalmanfilter, linmodel, R̂=[0.01]) end -@testitem "SteadyKalmanFilter real-time simulations" setup=[SetupMPCtests] begin +@testitem "SKF real-time simulations" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(tf(2, [10, 1]), 0.25) kalmanfilter1 = SteadyKalmanFilter(linmodel) @@ -139,7 +139,7 @@ end @test all(isapprox.(diff(times1[2:end]), 0.25, atol=0.01)) end -@testitem "KalmanFilter construction" setup=[SetupMPCtests] begin +@testitem "KF construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) kalmanfilter1 = KalmanFilter(linmodel) @@ -191,7 +191,7 @@ end @test_throws DimensionMismatch KalmanFilter(linmodel, nint_ym=0, σP_0=[1]) end -@testitem "KalmanFilter estimator methods" setup=[SetupMPCtests] begin +@testitem "KF estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) kalmanfilter1 = KalmanFilter(linmodel) @@ -240,7 +240,7 @@ end @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50]) end -@testitem "KalmanFilter set model" setup=[SetupMPCtests] begin +@testitem "KF set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -268,7 +268,7 @@ end @test kalmanfilter.cov.R̂ ≈ [1e-6] end -@testitem "Luenberger construction" setup=[SetupMPCtests] begin +@testitem "Luenb. construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) lo1 = Luenberger(linmodel) @@ -309,7 +309,7 @@ end @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), poles=[0.5,0.6]) end -@testitem "Luenberger estimator methods" setup=[SetupMPCtests] begin +@testitem "Luenb. estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) lo1 = Luenberger(linmodel, nint_ym=[1, 1]) @@ -357,7 +357,7 @@ end @test_throws ErrorException setstate!(lo1, [1,2,3,4], diagm(.1:.1:.4)) end -@testitem "Luenberger set model" setup=[SetupMPCtests] begin +@testitem "Luenb. set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -366,7 +366,7 @@ end @test_throws ErrorException setmodel!(lo, deepcopy(linmodel)) end -@testitem "InternalModel construction" setup=[SetupMPCtests] begin +@testitem "IM construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) internalmodel1 = InternalModel(linmodel) @@ -436,7 +436,7 @@ end @test_throws ErrorException InternalModel(linmodel, stoch_ym=ss(1,1,1,0,Ts).*I(2)) end -@testitem "InternalModel estimator methods" setup=[SetupMPCtests] begin +@testitem "IM estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]) , uop=[10,50], yop=[50,30]) internalmodel1 = InternalModel(linmodel) @@ -476,7 +476,7 @@ end @test_throws ErrorException setstate!(internalmodel1, [1,2,3,4], diagm(.1:.1:.4)) end -@testitem "InternalModel set model" setup=[SetupMPCtests] begin +@testitem "IM set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -501,7 +501,7 @@ end @test internalmodel.x̂0 ≈ [3.0 - 8.0] end -@testitem "UnscentedKalmanFilter construction" setup=[SetupMPCtests] begin +@testitem "UKF construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_d=[3]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d @@ -559,7 +559,7 @@ end @test isa(ukf10, UnscentedKalmanFilter{Float32}) end -@testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin +@testitem "UKF estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) function f!(xnext, x,u,_,model) @@ -618,7 +618,7 @@ end @test isa(x̂, Vector{Float32}) end -@testitem "UnscentedKalmanFilter set model" setup=[SetupMPCtests] begin +@testitem "UKF set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -654,7 +654,7 @@ end @test_throws ErrorException setmodel!(ukf2, deepcopy(nonlinmodel)) end -@testitem "ExtendedKalmanFilter construction" setup=[SetupMPCtests] begin +@testitem "EKF construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface import FiniteDiff @@ -713,7 +713,7 @@ end @test isa(ekf8, ExtendedKalmanFilter{Float32}) end -@testitem "ExtendedKalmanFilter estimator methods" setup=[SetupMPCtests] begin +@testitem "EKF estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface import FiniteDiff @@ -779,7 +779,7 @@ end @test evaloutput(ekf4) ≈ ekf4() ≈ [50, 30] end -@testitem "ExtendedKalmanFilter set model" setup=[SetupMPCtests] begin +@testitem "EKF set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -815,7 +815,7 @@ end @test_throws ErrorException setmodel!(ekf2, deepcopy(nonlinmodel)) end -@testitem "MovingHorizonEstimator construction (LinModel)" setup=[SetupMPCtests] begin +@testitem "MHE construction (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra import FiniteDiff linmodel = LinModel(sys,Ts,i_d=[3]) @@ -885,7 +885,7 @@ end @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) end -@testitem "MovingHorizonEstimator construction (NonLinModel)" setup=[SetupMPCtests] begin +@testitem "MHE construction (NonLinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using JuMP, Ipopt, DifferentiationInterface import FiniteDiff @@ -936,7 +936,7 @@ end ) end -@testitem "MovingHorizonEstimator estimation and getinfo (LinModel)" setup=[SetupMPCtests] begin +@testitem "MHE estimation and getinfo (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, DAQP linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) @@ -1006,7 +1006,7 @@ end @test isa(x̂, Vector{Float32}) end -@testitem "MovingHorizonEstimator estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin +@testitem "MHE estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, Ipopt linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) @@ -1115,7 +1115,7 @@ end end -@testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin +@testitem "MHE estimation with unfilled window" setup=[SetupMPCtests] begin f(x,u,_,_) = 0.5x + u h(x,_,_) = x model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing) @@ -1140,7 +1140,7 @@ end @test mhe2() ≈ model() atol = 1e-6 end -@testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin +@testitem "MHE fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d @@ -1186,7 +1186,7 @@ end @test mhe.cov.invP̄ ≈ invP̄_copy end -@testitem "MovingHorizonEstimator set constraints" setup=[SetupMPCtests] begin +@testitem "MHE set constraints" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe1 = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, Cwt=1e3) @@ -1287,7 +1287,7 @@ end @test_throws ArgumentError setconstraint!(mhe4, c_v̂max=[1,1]) end -@testitem "MovingHorizonEstimator constraint violation" setup=[SetupMPCtests] begin +@testitem "MHE constraint violation" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0) @@ -1442,7 +1442,7 @@ end end -@testitem "MovingHorizonEstimator set model" setup=[SetupMPCtests] begin +@testitem "MHE set model" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) @@ -1493,7 +1493,7 @@ end @test_throws ErrorException setmodel!(mhe, R̂=diagm([-0.1])) end -@testitem "MovingHorizonEstimator v.s. Kalman filters" setup=[SetupMPCtests] begin +@testitem "MHE v.s. Kalman filters" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) kf = KalmanFilter(linmodel, nint_ym=0, direct=false) @@ -1582,7 +1582,7 @@ end @test X̂_mhe ≈ X̂_ekf atol=1e-6 rtol=1e-6 end -@testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin +@testitem "MHE LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d @@ -1632,7 +1632,7 @@ end @test X̂_lin ≈ X̂_nonlin atol=1e-3 rtol=1e-3 end -@testitem "ManualEstimator construction" setup=[SetupMPCtests] begin +@testitem "Manual construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d @@ -1680,7 +1680,7 @@ end @test manual7.nx̂ == 6 end -@testitem "ManualEstimator estimator methods" setup=[SetupMPCtests] begin +@testitem "Manual estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d From 08b7d9b6734e33595f6d6bb363b7a300a952491e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 24 May 2026 17:23:44 -0400 Subject: [PATCH 56/56] test: improve coverage --- test/2_test_state_estim.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 2403898a4..259ea33dc 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1395,8 +1395,8 @@ end return nothing end nx̂ = linmodel2.nx - nc = 2 - mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gclv!, nc=nc, nint_ym=0, p=nx̂) + nc = 6 + mhe = MovingHorizonEstimator(linmodel2, He=5, gc=gclv!, nc=nc, nint_ym=0, p=nx̂) preparestate!(mhe, [50, 30], [5]) x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) @test x̂[1] ≈ 0.5 atol = 5e-2