From d7f0dc12336c412eb55eff39ba2e76efb4ecb9d6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 10:09:28 -0400 Subject: [PATCH 01/52] added: error if `ShootingMethods` with DAEs --- src/estimator/mhe/construct.jl | 2 +- src/model/nonlinmodeldae.jl | 5 +++++ test/2_test_state_estim.jl | 28 ++++++++++++++++++++++++++++ 3 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 914ed89f9..91e2d6f5b 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -173,11 +173,11 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator{NT} } nu, ny, nd = model.nu, model.ny, model.nd + validate_transcription(model, transcription) nk̄, na = get_nk̄(model, transcription), get_na(model) He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) - validate_transcription(model, transcription) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 04e091481..28ef89968 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -355,6 +355,11 @@ function validate_h_dae(NT, h) return ismutating end +function validate_transcription(::NonLinModelDAE, ::ShootingMethod) + throw(ArgumentError("Transcription with shooting methods is not supported with NonLinModelDAE")) + return nothing +end + """ validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) -> iszero_Ha diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index f4100d87c..9c8e99378 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1035,6 +1035,34 @@ end ) end +@testitem "MHE construction (NonLinModelDAE)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = 2 * x[] + a[] + 0.1 * d[] + end + Ts, p = 100.0, [0.01] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) + + transcription = TrapezoidalCollocation() + mhe = MovingHorizonEstimator(dae; He=3, transcription) + @test mhe.transcription isa TrapezoidalCollocation + + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + mhe2 = MovingHorizonEstimator(dae; He=3, transcription, direct=false) + @test mhe2.direct == false + + @test_throws ArgumentError MovingHorizonEstimator(dae, He=3, transcription=SingleShooting()) + @test_throws ArgumentError MovingHorizonEstimator(dae, He=3, transcription=MultipleShooting()) +end + @testitem "MHE estim. & getinfo (LinModel, SS)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, DAQP From cb853a590cc340622e7761e95d0b99bdbc6de02d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 10:10:06 -0400 Subject: [PATCH 02/52] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 7b4a641d2..8a146db26 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.13.2" +version = "2.14.0" authors = ["Francis Gagnon"] [deps] From b7b79d86723fc51c9738ce8ff012bca68bc7dd14 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 14:04:19 -0400 Subject: [PATCH 03/52] added: pretty-print `na` in `MovingHorizonEstimator` --- src/estimator/mhe.jl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 5bfbe7b68..be1843d28 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -3,7 +3,7 @@ include("mhe/execute.jl") include("mhe/transcription.jl") "Return estimation horizon He and slack variables length nε for `MovingHorizonEstimator`." -get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε) +get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε, get_na(estim.model)) "Print optimizer and other information for `MovingHorizonEstimator`." function print_details(io::IO, estim::MovingHorizonEstimator) @@ -27,11 +27,13 @@ print_backends(::IO, ::MovingHorizonEstimator, ::LinModel) = nothing function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n; firstchars=nothing) nu, nd = estim.model.nu, estim.model.nd nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu + na = get_na(estim.model) He, nε = estim.He, estim.nε niu, niym = sum(estim.nint_u), sum(estim.nint_ym) println(io, " │ ├$(lpad(He, n)) estimation steps He") println(io, " │ ├$(lpad(nu, n)) manipulated inputs u ($niu integrating states)") println(io, " │ ├$(lpad(nx̂, n)) estimated states x̂") + println(io, " │ ├$(lpad(na, n)) algebraic variables a") println(io, " │ ├$(lpad(nym, n)) measured outputs ym ($niym integrating states)") println(io, " │ ├$(lpad(nyu, n)) unmeasured outputs yu") print(io, " │ └$(lpad(nd, n)) measured disturbances d") From 1bb4a6250163a4cfb305053381421bcfd72777e9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 14:05:24 -0400 Subject: [PATCH 04/52] changed: new notation for `TrapezoidalCollocation` in MHE+DAE --- src/estimator/mhe/transcription.jl | 71 ++++++++++++++---------------- src/transcription.jl | 10 +++-- 2 files changed, 40 insertions(+), 41 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 2a7969a11..fa5fc7351 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1671,7 +1671,7 @@ Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalColloc By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the deterministic state defects are computed with: ```math -\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇_1}(ℓ+j) + \mathbf{k̇_2}(ℓ+j)] +\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇_0}(ℓ+j) + \mathbf{k̇_1}(ℓ+j)] - \mathbf{x̂_d}(ℓ+j+1) + \mathbf{ŵ_d}(ℓ+j) ``` for ``j = 0, 1, ... , N_k-1``, and in which ``\mathbf{x̂_d}`` and ``\mathbf{ŵ_d}`` are the @@ -1680,8 +1680,8 @@ variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continu function [`fq_dae!`](@ref) and: ```math \begin{aligned} -\mathbf{k̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ -\mathbf{k̇_2}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) +\mathbf{k̇_0}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` @@ -1689,13 +1689,13 @@ is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`] also computed from ``j = 0, 1, ... , N_k-1`` and: ```math \begin{aligned} -\mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ -\mathbf{q_2}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) +\mathbf{q_0}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` -and also one more residual at the arrival: +and also one final residual at `k+p`: ```math -\mathbf{q_0}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{a_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) +\mathbf{q_0}(k+p) = \mathbf{q}\Big(\mathbf{x̂_d}(k+p), \mathbf{a_0}(k+p), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) ``` """ function con_nonlinprogeq_mhe!( @@ -1714,21 +1714,15 @@ function con_nonlinprogeq_mhe!( nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He - nX, nA = nx*He, na*He + nŜk̄, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)] - A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] - Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nA)] + A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] # skipping a0arr components + Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - Ŝk̄ = @views geq[1:nX] - q0arr = @views geq[(nX + 1):(nX + na)] - Q0 = @views geq[(nX + na + 1):(nX + na + nA)] - Q̄ = @views geq[(nX + na + nA + 1):end] - if na > 0 - k̇0, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] - û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + i_d0arr):(nd + i_d0arr)] - fq_dae!(k̇0, q0arr, model, x̂darr, a0arr, û0arr, d0arr) - end + Ŝk̄ = @views geq[1:nŜk̄] + Q0 = @views geq[(nŜk̄ + 1):(nŜk̄ + nA + na)] + Q̄ = @views geq[(nŜk̄ + nA + na + 1):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1740,38 +1734,39 @@ function con_nonlinprogeq_mhe!( d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + a1 = @views Ā_Z̃[(1 + na*(j-1)):(na*j)] + q0 = @views Q0[(1 + na*(j-1)):(na*j)] q1 = @views Q̄[(1 + na*(j-1)):(na*j)] - q2 = @views Q0[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ā = @views Ā_Z̃[(1 + na*(j-1)):(na*(j-1) + na)] ŝk = @views Ŝk̄[(1 + nx*(j-1)):(nx*j)] - k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2nx] + k̇0, k̇1 = @views k̄[1:nx], k̄[nx+1:2nx] if estim.direct || j < Nk - d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] - else - d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + else # special case, d0(k+1)≈d0(k), since d0(k+1) is not available at time k: + d1 = d0 end if f_threads || h < 1 || j < 2 - # we need to recompute k1 with multi-threading, even with h==1, since the - # last iteration (j-1) may not be executed (iterations are re-orderable) - fq_dae!(k̇1, q1, model, x̂d_Z̃, ā, û0, d0) + # we need to recompute k̇0 with multi-threading, even with h==1, since the + # previous iteration (j-1) may not be executed (iterations are re-orderable) + fq_dae!(k̇0, q0, model, x̂d_Z̃, a0, û0, d0) else - k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇2 of the last iter. j-1 - q1 .= @views Q0[(1 + na*(j-1)-na):(na*(j-1))] # q2 of the last iter. j-1 + k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1 of the prev. iter. j-1 + q0 .= @views Q0[(1 + na*(j-1)-na):(na*(j-1))] # q1 of the prev. iter. j-1 end - if h < 1 - fq_dae!(k̇2, q2, model, x̂dnext, a0, û0, d0next) - else - # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k - û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - fq_dae!(k̇2, q2, model, x̂dnext, a0, û0next, d0next) + + if h > 0 && j < Nk + û1 = @views Û0[(1 + nu*j):(nu*(j+1))] + else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k: + û1 = û0 end - ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇1 + k̇2) + ŵd + fq_dae!(k̇1, q1, model, x̂dnext, a1, û1, d1) + ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇0 + k̇1) + ŵd end + Q0[(na*Nk + 1):(na*Nk + na)] .= @views Q̄[(1 + na*(Nk-1)):(na*Nk)] # final q1 value if Nk < He Ŝk̄[(nx*Nk + 1):end] .= 0 - Q0[(na*Nk + 1):end] .= 0 + Q0[(na*Nk + na + 1):end] .= 0 Q̄[(na*Nk + 1):end] .= 0 end return geq diff --git a/src/transcription.jl b/src/transcription.jl index 6875610af..18820d14a 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -218,10 +218,10 @@ transcription method. \mathbf{a_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a}(k-N_k+p+0) \\ - \mathbf{a}(k-N_k+p+1) \\ + \mathbf{a_1}(k-N_k+p+0) \\ + \mathbf{a_1}(k-N_k+p+1) \\ \vdots \\ - \mathbf{a}(k+p-1) \end{bmatrix} + \mathbf{a_1}(k+p-1) \end{bmatrix} ``` See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision @@ -229,6 +229,10 @@ transcription method. the algebraic variables in ``\mathbf{A_0}``, while the values in ``\mathbf{Ā}`` are strictly reserved for the the `fq!` function. + It's important to understand that the ``\mathbf{A_0}`` vector must be explicitly + included in the decision variables because of the output function ``\mathbf{h}``. As a + corollary, the optimal values #TODO: continue here or delete this par if no longer required + Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods require continuous-time dynamics. Because of this, and also to From 30ba429df6c7fdd666c13527a1f6ecb576290f46 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 15:00:12 -0400 Subject: [PATCH 05/52] doc: details on why `A0` is needed in `Z` for `OC` --- src/estimator/mhe/transcription.jl | 16 +++++++-------- src/transcription.jl | 31 ++++++++++++++---------------- test/2_test_state_estim.jl | 12 ++++++------ 3 files changed, 28 insertions(+), 31 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index fa5fc7351..424a60316 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1719,10 +1719,10 @@ function con_nonlinprogeq_mhe!( X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)] A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] # skipping a0arr components Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nĀ)] - Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - Ŝk̄ = @views geq[1:nŜk̄] - Q0 = @views geq[(nŜk̄ + 1):(nŜk̄ + nA + na)] - Q̄ = @views geq[(nŜk̄ + nA + na + 1):end] + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + Ŝk̄ = @views geq[1:nŜk̄] + Q0 = @views geq[(nŜk̄ + 1):(nŜk̄ + nA + na)] + Q̄ = @views geq[(nŜk̄ + nA + na + 1):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1763,11 +1763,11 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇1, q1, model, x̂dnext, a1, û1, d1) ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇0 + k̇1) + ŵd end - Q0[(na*Nk + 1):(na*Nk + na)] .= @views Q̄[(1 + na*(Nk-1)):(na*Nk)] # final q1 value + Q0[(1 + na*Nk):(na*(Nk+1))] .= @views Q̄[(1 + na*(Nk-1)):(na*Nk)] # final q1 value if Nk < He - Ŝk̄[(nx*Nk + 1):end] .= 0 - Q0[(na*Nk + na + 1):end] .= 0 - Q̄[(na*Nk + 1):end] .= 0 + Ŝk̄[(1 + nx*Nk):end] .= 0 + Q0[(1 + na*Nk + na):end] .= 0 + Q̄[(1 + na*Nk):end] .= 0 end return geq end diff --git a/src/transcription.jl b/src/transcription.jl index 18820d14a..156bcd87f 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -161,8 +161,7 @@ moderately stiff systems and is A-stable. See Extended Help for more details. Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a `TrapezoidalCollocation` transcription, the built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to - estimate the plant states, not the trapezoidal rule (see `supersample` option of - [`RungeKutta`](@ref) for stiff systems). + estimate the plant states, not the trapezoidal rule. Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this transcription method. @@ -227,11 +226,8 @@ transcription method. two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at the beginning (``N_k < H_e``). The predicted outputs are computed from the algebraic variables in ``\mathbf{A_0}``, while the values in ``\mathbf{Ā}`` are - strictly reserved for the the `fq!` function. - - It's important to understand that the ``\mathbf{A_0}`` vector must be explicitly - included in the decision variables because of the output function ``\mathbf{h}``. As a - corollary, the optimal values #TODO: continue here or delete this par if no longer required + strictly reserved for the the `fq!` function. The ``\mathbf{a_0}`` vector is at the left + endpoint of the trapezoid, while the ``\mathbf{a_1}`` is at the right endpoint. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -287,7 +283,7 @@ where ``\mathbf{K̄}`` encompasses all the intermediate stages of the determinis ``` The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for info -on `f_threads` and `h_threads` keywords. This transcription computes thecpredictions by +on `f_threads` and `h_threads` keywords. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocationc points. It is efficient for highly stiff systems, but generally more expensive than the other methods for non-stiff systems. See Extended Help for details and the transcription of @@ -297,8 +293,7 @@ non-stiff systems. See Extended Help for details and the transcription of Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a `OrthogonalCollocation` transcription, the built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to estimate - the plant states, not orthogonal collocation (see `supersample` option of - [`RungeKutta`](@ref) for stiff systems). + the plant states, not orthogonal collocation. Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for this transcription method (sparser formulation than [`MultipleShooting`](@ref)). @@ -396,14 +391,16 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at the beginning in the [`MovingHorizonEstimator`](@ref) (``N_k < H_e``). The predicted outputs are computed from the algebraic variables in ``\mathbf{A_0}``, while the values - in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. + in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. The ``\mathbf{A_0}`` + vector must be explicitly included in the decision variables since the output function + `h!` is evaluated at different locations than the collocation points, in general. - The collocation points are located at the roots of orthogonal polynomials, which is - "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. - The method then enforces the system dynamics at these points. The Gauss-Legendre scheme - is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) - for more details. + More precisely, the outputs are at the sampling times, while the collocation points are + at the roots of orthogonal polynomials, which is "optimal" for approximating the state + trajectories with polynomials of degree ``n_o``. The method then enforces the system + dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau + but only A-stable, while the latter being L-stable. See [`init_orthocolloc`](@ref), + [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic states are left out of the ``\mathbf{K̄}`` vector to reduce the dimensions, and also diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 9c8e99378..d9b42144e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1432,12 +1432,12 @@ end mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) preparestate!(mhe, [0.0], [0.0]) x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) - @test x̂ ≈ zeros(mhe.nx̂) atol=1e-6 - @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-6 + @test x̂ ≈ zeros(mhe.nx̂) atol=1e-5 + @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-5 preparestate!(mhe, [0], [0]) info = getinfo(mhe) - @test info[:x̂] ≈ x̂ atol=1e-6 - @test info[:Ŷ][end] ≈ 0 atol=1e-6 + @test info[:x̂] ≈ x̂ atol=1e-5 + @test info[:Ŷ][end] ≈ 0 atol=1e-5 for i in 1:40 preparestate!(mhe, [0], [0]) updatestate!(mhe, [3.0], [0], [0]) @@ -1455,8 +1455,8 @@ end mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) preparestate!(mhe2, [0.0], [0.0]) x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) - @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 - @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-5 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-5 initstate!(mhe2, [0], [0], [0]) @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] From 2716b2aa32ff0515dcc5e833773bcad3a009b161 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 15:52:25 -0400 Subject: [PATCH 06/52] debug: explicitly compute final `q0` for MHE and TC --- src/estimator/mhe/transcription.jl | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 424a60316..cf435f9b1 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1763,7 +1763,14 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇1, q1, model, x̂dnext, a1, û1, d1) ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇0 + k̇1) + ŵd end - Q0[(1 + na*Nk):(na*(Nk+1))] .= @views Q̄[(1 + na*(Nk-1)):(na*Nk)] # final q1 value + if na > 0 # final residual at k+p: + x̂d = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + a0 = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] + û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] + d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] + q0 = @views Q0[(1 + na*Nk):(na*Nk + na)] + @views fq_dae!(K̄[1:nx], q0, model, x̂d, a0, û0, d0) + end if Nk < He Ŝk̄[(1 + nx*Nk):end] .= 0 Q0[(1 + na*Nk + na):end] .= 0 From d59c0ebbc954b6f278b235d2f10c461dbc7ad8cc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 15:53:52 -0400 Subject: [PATCH 07/52] test: tighter tolerances in MHE+DAE+TC tests --- test/2_test_state_estim.jl | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index d9b42144e..2605b7593 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1432,12 +1432,12 @@ end mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) preparestate!(mhe, [0.0], [0.0]) x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) - @test x̂ ≈ zeros(mhe.nx̂) atol=1e-5 - @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-5 + @test x̂ ≈ zeros(mhe.nx̂) atol=1e-6 + @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-6 preparestate!(mhe, [0], [0]) info = getinfo(mhe) - @test info[:x̂] ≈ x̂ atol=1e-5 - @test info[:Ŷ][end] ≈ 0 atol=1e-5 + @test info[:x̂] ≈ x̂ atol=1e-6 + @test info[:Ŷ][end] ≈ 0 atol=1e-6 for i in 1:40 preparestate!(mhe, [0], [0]) updatestate!(mhe, [3.0], [0], [0]) @@ -1455,8 +1455,9 @@ end mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) preparestate!(mhe2, [0.0], [0.0]) x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) - @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-5 - @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-5 + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 + @show x̂ initstate!(mhe2, [0], [0], [0]) @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] From 8b69587561f3746833d16ada17557f5716559a74 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 15:56:14 -0400 Subject: [PATCH 08/52] doc: update `jldoctest` --- src/estimator/mhe/construct.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 91e2d6f5b..17122380f 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -380,6 +380,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 5.0 s: │ ├ 5 estimation steps He │ ├ 1 manipulated inputs u (0 integrating states) │ ├ 2 estimated states x̂ + │ ├ 0 algebraic variables a │ ├ 1 measured outputs ym (1 integrating states) │ ├ 0 unmeasured outputs yu │ └ 0 measured disturbances d From 04cc1265fd615bbe12d1b91c3b45222d626a3754 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 16:04:55 -0400 Subject: [PATCH 09/52] doc: update `jldoctest` --- src/estimator/mhe/construct.jl | 1 + src/estimator/mhe/transcription.jl | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 17122380f..3887b665a 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -844,6 +844,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 1.0 s: │ ├ 3 estimation steps He │ ├ 1 manipulated inputs u (0 integrating states) │ ├ 2 estimated states x̂ + │ ├ 0 algebraic variables a │ ├ 1 measured outputs ym (1 integrating states) │ ├ 0 unmeasured outputs yu │ └ 0 measured disturbances d diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index cf435f9b1..e404d1fde 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1766,8 +1766,8 @@ function con_nonlinprogeq_mhe!( if na > 0 # final residual at k+p: x̂d = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] a0 = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] - û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] - d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] + û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) + d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) q0 = @views Q0[(1 + na*Nk):(na*Nk + na)] @views fq_dae!(K̄[1:nx], q0, model, x̂d, a0, û0, d0) end From e36716c11d75c79d8095732fea6493cd859ee889 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 20 Sep 2026 18:39:01 -0400 Subject: [PATCH 10/52] added: default to `model.transcription` for MHE + DAE --- src/controller/linmpc.jl | 6 ++---- src/controller/nonlinmpc.jl | 11 +++++------ src/estimator/mhe/construct.jl | 10 +++++----- src/model/nonlinmodeldae.jl | 4 ++++ 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index ebb3c8102..135041760 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -1,5 +1,3 @@ -const DEFAULT_LINMPC_TRANSCRIPTION = SingleShooting() - struct LinMPC{ NT<:Real, SE<:StateEstimator, @@ -241,7 +239,7 @@ function LinMPC( Wd = nothing, Wr = nothing, Cwt = DEFAULT_CWT, - transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, + transcription::ShootingMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_QP_OPTIMIZER, add_bridges=true), kwargs... ) @@ -300,7 +298,7 @@ function LinMPC( Wd = nothing, Wr = nothing, Cwt = DEFAULT_CWT, - transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, + transcription::ShootingMethod = SingleShooting(), optim::JM = JuMP.Model(DEFAULT_QP_OPTIMIZER, add_bridges=true) ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 18294ac07..f7f2749c2 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,4 +1,3 @@ -const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() const DEFAULT_NONLINMPC_HESSIAN = AutoSparse( AutoForwardDiff(); sparsity_detector=TracerSparsityDetector(), @@ -365,7 +364,7 @@ function NonLinMPC( gc ::Function = gc!, nc::Int = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, + transcription::TranscriptionMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -381,9 +380,6 @@ function NonLinMPC( ) end -default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) -default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) - """ NonLinMPC(estim::StateEstimator; ) @@ -441,7 +437,7 @@ function NonLinMPC( gc ::Function = gc!, nc = 0, p = estim.model.p, - transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, + transcription::TranscriptionMethod = SingleShooting(), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -467,6 +463,9 @@ function NonLinMPC( ) end +default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) +default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) + """ validate_JE(NT, JE) -> nothing diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 3887b665a..fec3f6b73 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1,4 +1,3 @@ -const DEFAULT_MHE_TRANSCRIPTION = SingleShooting() const DEFAULT_NONLINMHE_HESSIAN = AutoSparse( AutoForwardDiff(); sparsity_detector=TracerSparsityDetector(), @@ -346,7 +345,8 @@ at each time step for the optimization. (details in Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). -- `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. +- `transcription=default_transcription(model)` : a [`TranscriptionMethod`](@ref) object, + default to [`SingleShooting`](@ref) or `model.transcription` for [`NonLinModelDAE`](@ref). - `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)). @@ -558,7 +558,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc ::Int = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_MHE_TRANSCRIPTION, + transcription::TranscriptionMethod = default_transcription(model), optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -591,7 +591,7 @@ end gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing, gc=gc!, nc=0, - transcription=SingleShooting(), + transcription=default_transcription(model), optim=default_optim_mhe(model, nc), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), @@ -612,7 +612,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc = 0, p = model.p, - transcription::TranscriptionMethod = DEFAULT_MHE_TRANSCRIPTION, + transcription::TranscriptionMethod = default_transcription(model), optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 28ef89968..bef3e69e0 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -405,6 +405,10 @@ function validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) return iszero_Ha end +"Default to `SingleShooting()`, or `model.transcription` if `model` is a `NonLinModelDAE`." +default_transcription(::SimModel) = SingleShooting() +default_transcription(model::NonLinModelDAE) = model.transcription + "Get the number of algebraic variable `na` in `model`." get_na(model::NonLinModelDAE) = model.na get_na(::SimModel) = 0 From da2af1f2729833a9153a749487024c1b334af682 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 02:35:44 -0400 Subject: [PATCH 11/52] changed: `a1` notation in `NonLinModelDAE` and `TrapezoidalCollocation` --- src/model/nonlinmodeldae.jl | 6 +++--- src/transcription.jl | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index bef3e69e0..80ecc4ea9 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -690,11 +690,11 @@ function con_nonlinprogeq!( ) nx, na = model.nx, model.na Ts = model.Ts - x0next_Z, a0_Z, a0next_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] - sknext, q0, q1 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] + x0next_Z, a0_Z, a1_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] + sknext, q0, q1 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] k̇0, k̇1 = @views k̄[1:nx], k̄[(nx+1):(2nx)] model.fq!(k̇0, q0, x0, a0_Z, u0, d0, model.p) - model.fq!(k̇1, q1, x0next_Z, a0next_Z, u0, d0, model.p) + model.fq!(k̇1, q1, x0next_Z, a1_Z, u0, d0, model.p) sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end diff --git a/src/transcription.jl b/src/transcription.jl index 156bcd87f..f4baecf41 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -174,7 +174,7 @@ transcription method. \mathbf{Z} = \begin{bmatrix} \mathbf{x_0}(k+1) \\ \mathbf{a_0}(k+0) \\ - \mathbf{a_0}(k+1) \end{bmatrix} + \mathbf{a_1}(k+0) \end{bmatrix} ``` For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: ```math @@ -191,10 +191,10 @@ transcription method. \mathbf{a_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a}(k+0) \\ - \mathbf{a}(k+1) \\ + \mathbf{a_1}(k+0) \\ + \mathbf{a_1}(k+1) \\ \vdots \\ - \mathbf{a}(k+H_p-1) \end{bmatrix} + \mathbf{a_1}(k+H_p-1) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math From aa88bf5cd48383712c111dbf9656fd9edb0616dc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 08:29:33 -0400 Subject: [PATCH 12/52] doc: under construction admonition --- docs/src/manual/nonlinmpc2.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/src/manual/nonlinmpc2.md b/docs/src/manual/nonlinmpc2.md index 068ec43f3..63a724903 100644 --- a/docs/src/manual/nonlinmpc2.md +++ b/docs/src/manual/nonlinmpc2.md @@ -4,6 +4,10 @@ Pages = ["nonlinmpc2.md"] ``` +!!! todo "Under Construction" + This tutorial is currently under construction. Only the modeling part is written for + now. + ## Nonlinear Model (DAE) In this example, the goal is to control the pH of a solution in a continuously stirred From 534d2028f79b5acc96c33ccd67a08dae069293b2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 09:44:43 -0400 Subject: [PATCH 13/52] doc: DAE models in README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0b5a96b4d..706043774 100644 --- a/README.md +++ b/README.md @@ -82,7 +82,7 @@ for more detailed examples. ### 🎯 Model Predictive Control Features -- 🏭️ **Plant Model**: Linear or nonlinear models exploiting multiple dispatch. +- 🏭️ **Plant Model**: Linear, nonlinear ODE and nonlinear DAE models exploiting multiple dispatch. - ⛳️ **Objectives**: Tracking for inputs/outputs, move suppression, terminal costs, and economic costs. - ⏳️ **Horizons**: Distinct prediction/control horizons with custom move blocking. - 📸 **Linearization**: Auto-differentiation for exact Jacobians. From 176e0630886c4fe5396fa5555de4c759f9ccc90e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 10:24:34 -0400 Subject: [PATCH 14/52] changed: clean-up in various state-space functions --- docs/src/internals/sim_model.md | 2 ++ docs/src/internals/state_estim.md | 2 -- src/estimator/execute.jl | 25 ++++--------------------- src/estimator/mhe/transcription.jl | 27 +++++++++++++-------------- src/model/nonlinmodeldae.jl | 25 +++++++++++++++++++++++++ src/transcription.jl | 2 +- 6 files changed, 45 insertions(+), 38 deletions(-) diff --git a/docs/src/internals/sim_model.md b/docs/src/internals/sim_model.md index 820d9d870..31bf5549b 100644 --- a/docs/src/internals/sim_model.md +++ b/docs/src/internals/sim_model.md @@ -22,6 +22,8 @@ ModelPredictiveControl.init_defectmat_dae ```@docs ModelPredictiveControl.f! ModelPredictiveControl.h! +ModelPredictiveControl.fq_dae! +ModelPredictiveControl.h_dae! ``` ## Init State diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 9313eb8c0..359299bfe 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -67,8 +67,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! -ModelPredictiveControl.fq_dae! -ModelPredictiveControl.ĥ_dae! ``` ## Remove Operating Points diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index ce21c5f21..5d546fa95 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -142,35 +142,18 @@ function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) end """ - fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) + ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x0, a0, d0) -Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). +Same than [`ĥ!`](@ref) for [`SimModel`](@ref), but call [`h_dae!`](@ref) internally. """ -function fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) - return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) -end -fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0)= model.f!(ẋ0, x0, u0, d0, model.p) - -""" - ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x0, a0, d0) - -Similar than [`ĥ!`](@ref) but with a algebraic variable `a0` for [`NonLinModelDAE`](@ref) . -""" -function ĥ_dae!(ŷ0, estim::StateEstimator, model::NonLinModelDAE, x̂0, a0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, a0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] - model.h!(ŷ0, xd, a0, d0, model.p) + h_dae!(ŷ0, model, xd, a0, d0) mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end -""" - ĥ_dae!(ŷ0, estim::StateEstimator, model::SimModelODE, x0, _ , d0) - -Ignore the algebraic variable argument for other [`SimModelODE`](@ref) types. -""" -ĥ_dae!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, _, d0) = ĥ!(ŷ0, estim, model, x̂0, d0) - """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index e404d1fde..d1ce6a2f4 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1499,7 +1499,7 @@ function predict_mhe!( ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] v̂ = @views V̂[(1 + nym*(j-1)):(nym*j)] y0m = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] - ĥ_dae!(ŷ0, estim, model, x̂0, a0, d0) + ĥ!(ŷ0, estim, model, x̂0, a0, d0) ŷ0m = @views ŷ0[estim.i_ym] if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] @@ -1714,15 +1714,15 @@ function con_nonlinprogeq_mhe!( nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He - nŜk̄, nA, nĀ = nx*He, na*He, na*He + nŜk, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)] A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] # skipping a0arr components - Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nĀ)] + A1_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - Ŝk̄ = @views geq[1:nŜk̄] - Q0 = @views geq[(nŜk̄ + 1):(nŜk̄ + nA + na)] - Q̄ = @views geq[(nŜk̄ + nA + na + 1):end] + Ŝk̄ = @views geq[1:nŜk] + Q0 = @views geq[(nŜk + 1):(nŜk + nA + na)] + Q̄ = @views geq[(nŜk + nA + na + 1):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1734,7 +1734,7 @@ function con_nonlinprogeq_mhe!( d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a1 = @views Ā_Z̃[(1 + na*(j-1)):(na*j)] + a1 = @views A1_Z̃[(1 + na*(j-1)):(na*j)] q0 = @views Q0[(1 + na*(j-1)):(na*j)] q1 = @views Q̄[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] @@ -1806,8 +1806,8 @@ are computed by: ``` for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{x̂_d}(ℓ+j)`` vectors are extracted from the decision variables in `Z̃`. The -``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as -described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the +``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function [`fq_dae!`](@ref), +as described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). The estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ @@ -1823,7 +1823,6 @@ function con_nonlinprogeq_mhe!( Mo, no, τ = estim.Mo, transcription.no, transcription.τ nk̄ = get_nk̄(model, transcription) nx̃ = estim.nε + nx̂ - p = estim.direct ? 0 : 1 i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃, K_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)], Z̃[(nx̃+nx̂*He+1):(nx̃+nx̂*He+nk̄*He)] Dtemp = estim.buffer.D @@ -1834,11 +1833,11 @@ function con_nonlinprogeq_mhe!( else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end - d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] - ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] + ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] if estim.direct || j < Nk d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 80ecc4ea9..f79bdf69d 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -798,6 +798,31 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) return nothing end +""" + fq_dae!(ẋ0, q0, model, x0, a0, u0, d0) + +Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`](@ref). + +Both the algebraic variable `a0` and the residual `q0` arguments are ignored if `model` is a +[`NonLinModel`](@ref). +""" +function fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) + return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +end +fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0) = model.f!(ẋ0, x0, u0, d0, model.p) + +""" + h_dae!(y0, model, x0, a0, d0) + +Call `model.h!` with the `a0` argument if [`NonLinModelDAE`](@ref), else without. + +See also [`fq_dae!`](@ref). +""" +function h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) + return model.h!(y0, x0, a0, d0, model.p) +end +h_dae!(y0, model::NonLinModel, x0, _ , d0) = model.h!(y0, x0, d0, model.p) + function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) model.beq .= @. -model.Fs diff --git a/src/transcription.jl b/src/transcription.jl index f4baecf41..df9935d52 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -493,7 +493,7 @@ knowing that the ``\mathbf{k}_i(k)`` vectors are directly extracted from the dec variables in `Z̃`. The ``\mathbf{x̂_d}(k)`` vector is the estimated deterministic state at the beginning of the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivatives for the ``i``th collocation point are computed from the continuous-time function -`model.f!` and: +[`fq_dae!`](@ref) and: ```math \mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) ``` From f53773577d7772877219000a3aeaf3894968e773 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 12:14:28 -0400 Subject: [PATCH 15/52] debug: MHE+TC+DAE with `h=1` now works We can reuse the computation of the last iteration w/o multi-threading and with piecewise linear input `u`, but we need a special constraint to enforce that `a0` is equal to `a1` of the last iteration. --- src/estimator/mhe/transcription.jl | 47 ++++++++++++++---------------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index d1ce6a2f4..7c85e2773 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1710,36 +1710,34 @@ function con_nonlinprogeq_mhe!( Ts = model.Ts na = get_na(model) nk̄ = get_nk̄(model, transcription) - nw = nŵ - nxs - nx̃ = estim.nε + nx̂ - nx̃_nX̂ = nx̃ + nx̂*estim.He - nx̃_nX̂_na_nA = nx̃_nX̂ + na + na*estim.He + nw, nx̃ = nŵ - nxs, estim.nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃_nX̂)] - A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂_na_nA)] # skipping a0arr components - A1_Z̃ = @views Z̃[(1 + nx̃_nX̂_na_nA):(nx̃_nX̂_na_nA + nĀ)] + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] + A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components + A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) - Ŝk̄ = @views geq[1:nŜk] - Q0 = @views geq[(nŜk + 1):(nŜk + nA + na)] - Q̄ = @views geq[(nŜk + nA + na + 1):end] + Ŝk = @views geq[1:nŜk] + Q0 = @views geq[(1 + nŜk):(nŜk + nA + na)] + Q̄ = @views geq[(1 + nŜk + nA + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0 = @views a0arr[1:na] + a0_Z̃ = @views a0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + a0_Z̃ = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a1 = @views A1_Z̃[(1 + na*(j-1)):(na*j)] + a1_Z̃ = @views A1_Z̃[(1 + na*(j-1)):(na*j)] q0 = @views Q0[(1 + na*(j-1)):(na*j)] q1 = @views Q̄[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] - x̂dnext = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ŝk = @views Ŝk̄[(1 + nx*(j-1)):(nx*j)] + x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] k̇0, k̇1 = @views k̄[1:nx], k̄[nx+1:2nx] if estim.direct || j < Nk d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] @@ -1749,30 +1747,29 @@ function con_nonlinprogeq_mhe!( if f_threads || h < 1 || j < 2 # we need to recompute k̇0 with multi-threading, even with h==1, since the # previous iteration (j-1) may not be executed (iterations are re-orderable) - fq_dae!(k̇0, q0, model, x̂d_Z̃, a0, û0, d0) + fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) else - k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1 of the prev. iter. j-1 - q0 .= @views Q0[(1 + na*(j-1)-na):(na*(j-1))] # q1 of the prev. iter. j-1 + k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1 of the prev. iter. (j-1) + q0 .= @views a0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0 = a1 of prev. iter. end - if h > 0 && j < Nk û1 = @views Û0[(1 + nu*j):(nu*(j+1))] else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k: û1 = û0 end - fq_dae!(k̇1, q1, model, x̂dnext, a1, û1, d1) - ŝk .= @. x̂d_Z̃ - x̂dnext + 0.5*Ts*(k̇0 + k̇1) + ŵd + fq_dae!(k̇1, q1, model, x̂dnext_Z̃, a1_Z̃, û1, d1) + ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end if na > 0 # final residual at k+p: - x̂d = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] - a0 = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] + x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + a0_Z̃ = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) q0 = @views Q0[(1 + na*Nk):(na*Nk + na)] - @views fq_dae!(K̄[1:nx], q0, model, x̂d, a0, û0, d0) + @views fq_dae!(K̄[1:nx], q0, model, x̂d_Z̃, a0_Z̃, û0, d0) end if Nk < He - Ŝk̄[(1 + nx*Nk):end] .= 0 + Ŝk[(1 + nx*Nk):end] .= 0 Q0[(1 + na*Nk + na):end] .= 0 Q̄[(1 + na*Nk):end] .= 0 end From 2fe95672bdedcfff6833d70d5a122983f0deae8b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 13:06:42 -0400 Subject: [PATCH 16/52] debug: skip `a0arr` for `A0` in `predict_mhe!` --- src/estimator/mhe/transcription.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 7c85e2773..a4939d655 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1485,8 +1485,8 @@ function predict_mhe!( nx̃ = nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads - X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] - A0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂):(nx̃_nX̂ + na*Nk)] + X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] # skip x0arr + A0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + na*Nk)] # skip a0arr @threadsif h_threads for j=1:Nk if estim.direct x̂0 = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] @@ -1509,6 +1509,7 @@ function predict_mhe!( if Nk < estim.He # fill unused values with 0s for tracer sparsity detection: V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 + A0[na*Nk+1:end] .= 0 end return V̂, X̂0, A0 end From 6e9f3320e6580285ffc87e2cbb21c6a5557cac26 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 16:33:13 -0400 Subject: [PATCH 17/52] changed: simpler `geq` filling when `Nk < He` --- src/estimator/mhe/transcription.jl | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index a4939d655..75eb33773 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1715,13 +1715,14 @@ function con_nonlinprogeq_mhe!( nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x0arr components A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + (Nk < He) && (geq .= 0) Ŝk = @views geq[1:nŜk] Q0 = @views geq[(1 + nŜk):(nŜk + nA + na)] - Q̄ = @views geq[(1 + nŜk + nA + na):end] + Q̄ = @views geq[(1 + nŜk + nA + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1750,8 +1751,8 @@ function con_nonlinprogeq_mhe!( # previous iteration (j-1) may not be executed (iterations are re-orderable) fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) else - k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1 of the prev. iter. (j-1) - q0 .= @views a0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0 = a1 of prev. iter. + k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) + q0 .= @views a0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] end if h > 0 && j < Nk û1 = @views Û0[(1 + nu*j):(nu*(j+1))] @@ -1761,18 +1762,14 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇1, q1, model, x̂dnext_Z̃, a1_Z̃, û1, d1) ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end - if na > 0 # final residual at k+p: + if na > 0 # final residual at k+p: x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] a0_Z̃ = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) q0 = @views Q0[(1 + na*Nk):(na*Nk + na)] - @views fq_dae!(K̄[1:nx], q0, model, x̂d_Z̃, a0_Z̃, û0, d0) - end - if Nk < He - Ŝk[(1 + nx*Nk):end] .= 0 - Q0[(1 + na*Nk + na):end] .= 0 - Q̄[(1 + na*Nk):end] .= 0 + k̇0 = @views K̄[(end-nx+1):end] + fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) end return geq end From e7b8bfdc9aa94790cf8c72ffa0161d5aadef2d9f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 16:40:26 -0400 Subject: [PATCH 18/52] test: cover TC with `h=1` bugfix with smarter tests --- test/2_test_state_estim.jl | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 2605b7593..204ee82af 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1350,7 +1350,7 @@ end h! = (y,x,_,_) -> y .= x nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) - transcription = TrapezoidalCollocation(f_threads=true, h_threads=true) + transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) mhe6 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=false, transcription ) @@ -1361,7 +1361,8 @@ end preparestate!(mhe6, [13]) @test mhe6() ≈ [13] atol=5e-3 - transcription = TrapezoidalCollocation(1) + ##### # h=1 & f_threads=false options test the branch with the reuse of k̇1 from prev. iter: + transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) mhe7 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=true, transcription ) @@ -1428,7 +1429,7 @@ end as_0, xs_0 = [-1.0], [1.0] dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) - transcription = TrapezoidalCollocation() + transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) preparestate!(mhe, [0.0], [0.0]) x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) @@ -1451,7 +1452,8 @@ end preparestate!(mhe, [7.0], [0]) @test mhe([0]) ≈ [7.0] atol=1e-3 - transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + # h=1 & f_threads=false options test the branch with reuse of k̇1 & q1 from prev. iter: + transcription = TrapezoidalCollocation(1, f_threads=false, h_threads=true) mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) preparestate!(mhe2, [0.0], [0.0]) x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) From 17993df44bd0a12f3352ee4cbdc279fbc4108622 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 16:41:04 -0400 Subject: [PATCH 19/52] doc: residuals in `init_orthocolloc` docstring --- src/transcription.jl | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index df9935d52..6c5aa8919 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -495,7 +495,14 @@ beginning of the interval ``τ_0=0``, and is also extracted from `Z̃`. The ``\m derivatives for the ``i``th collocation point are computed from the continuous-time function [`fq_dae!`](@ref) and: ```math -\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) +\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{a}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) +``` +The residuals of [`NonLinModelDAE`](@ref) at the collocation points are nonlinear equality +constraints: +```math +\begin{aligned} +\mathbf{q}_i(k) &= \mathbf{q}\Big(\mathbf{k}_i(k), \mathbf{a}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) \\ + &= \mathbf{0} ``` Based on the normalized time ``τ_i`` and the hold order `transcription.h`, the inputs and disturbances are either piecewise constant or linear: From 029bbb9ed305002d9a2a5e30a59e857a1cece217 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 16:42:02 -0400 Subject: [PATCH 20/52] test: minor correction --- 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 204ee82af..233a64bd4 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1362,7 +1362,7 @@ end @test mhe6() ≈ [13] atol=5e-3 ##### # h=1 & f_threads=false options test the branch with the reuse of k̇1 from prev. iter: - transcription = TrapezoidalCollocation(1, f_threads=true, h_threads=true) + transcription = TrapezoidalCollocation(1, f_threads=false, h_threads=true) mhe7 = MovingHorizonEstimator( nonlinmodel_c; He=3, direct=true, transcription ) From f95f23ae7caed2bf68e9f399c3e1e9b29b484fec Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 18:22:28 -0400 Subject: [PATCH 21/52] debug: weird race condition seems to be solved now --- src/estimator/mhe/transcription.jl | 65 +++++++++++++++++++----------- 1 file changed, 42 insertions(+), 23 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 75eb33773..0e611e926 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1763,13 +1763,13 @@ function con_nonlinprogeq_mhe!( ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end if na > 0 # final residual at k+p: - x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] - a0_Z̃ = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] - û0 = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) - d0 = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) - q0 = @views Q0[(1 + na*Nk):(na*Nk + na)] - k̇0 = @views K̄[(end-nx+1):end] - fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) + x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + a0end = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] + û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) + d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) + q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + k̇0end = @views K̄[(end-nx+1):end] + fq_dae!(k̇0end, q0end, model, x̂dend, a0end, û0end, d0end) end return geq end @@ -1816,28 +1816,48 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads Mo, no, τ = estim.Mo, transcription.no, transcription.τ - nk̄ = get_nk̄(model, transcription) + na = get_na(model) + nā, nk̄ = na*no, get_nk̄(model, transcription) nx̃ = estim.nε + nx̂ + nx̃_nX̂ = nx̃ + nx̂*estim.He + nŜk̄, nA, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃, K_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)], Z̃[(nx̃+nx̂*He+1):(nx̃+nx̂*He+nk̄*He)] + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] + A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components + K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nK̄)] + Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA + nK̄):(nx̃_nX̂ + na + nA + nK̄ + nĀ)] Dtemp = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + Ŝk̄ = @views geq[1:nŜk̄] + Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + nA + na)] + Q̄ = @views geq[(1 + nŜk̄ + nA + na):()] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] + a0 = @views a0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end - d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] - ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K̄_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] + ā_Z̃ = @views Ā_Z̃[(1 + nā*(j-1)):(nā*j)] + q0 = @views Q0[(1 + na*(j-1)):(na*j)] + q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] + ŝk̄ = @views Ŝk̄[(1 + nk̄*(j-1)):(nk̄*j)] if estim.direct || j < Nk d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available end + # ----------------- residual at sampling times (τ=0) --------------------------- + if na > 0 && (f_threads || h < 1 || j < 2) + @views fq_dae!(k̄[1:nx], q0, model, x̂d_Z̃, a0, û0, d0) + else + q0 .= @views Q0[(1 + na*(j-2)):(na*(j-1))] # q0 of the prev. iter (j-1) + end # ----------------- collocation constraint defects ----------------------------- Δk = k̄ for i=1:no @@ -1849,17 +1869,16 @@ function con_nonlinprogeq_mhe!( ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i = @views k̄[(1 + (i-1)*nx):(i*nx)] - ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] + k̇i, ki_Z̃ = @views k̄[(1 + (i-1)*nx):(i*nx)], k̄_Z̃[(1 + (i-1)*nx):(i*nx)] + qi, ai_Z̃ = @views q̄[(1 + (i-1)*na):(i*na)], ā_Z̃[(1 + (i-1)*na):(i*na)] di .= (1-τ[i]).*d0 .+ τ[i].*d0next - if h < 1 - model.f!(k̇i, ki_Z̃, û0, di, model.p) - else - # special case: û0(k+p)≈û0(k+p-1), since û0(k+p) is not available at time k - û0next = @views j ≥ Nk ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next - model.f!(k̇i, ki_Z̃, ûi, di, model.p) + if h > 0 && j < Nk + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next + else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) not available at time k: + ûi = û0 end + fq_dae!(k̇i, qi, model, ki_Z̃, ai_Z̃, ûi, di) end ŝk̄ .-= k̄ end From 0335df2d3d333f487edb9add9dfe8dc1483c02ac Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 18:24:08 -0400 Subject: [PATCH 22/52] debug: comment for posterity --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 0e611e926..59a29e161 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1762,7 +1762,7 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇1, q1, model, x̂dnext_Z̃, a1_Z̃, û1, d1) ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end - if na > 0 # final residual at k+p: + if na > 0 # final residual at k+p (keep "end" in vars, solve a weird race cond.): x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] a0end = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) From d7f3c35913329c4ecd3e4eb943e1493e7fe4ba9a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 21 Sep 2026 18:30:24 -0400 Subject: [PATCH 23/52] test: remove useless `@show` --- test/2_test_state_estim.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 233a64bd4..fa8acb17e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1459,7 +1459,6 @@ end x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 - @show x̂ initstate!(mhe2, [0], [0], [0]) @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] From e46280ad48e1ddfc3e7f0bdb695153cbcc1f3f5c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 10:01:14 -0400 Subject: [PATCH 24/52] added: do not call `fq!` if `h>0` for final residual `q0end` --- src/estimator/mhe/transcription.jl | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 59a29e161..0ddcaaa70 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1750,7 +1750,7 @@ function con_nonlinprogeq_mhe!( # we need to recompute k̇0 with multi-threading, even with h==1, since the # previous iteration (j-1) may not be executed (iterations are re-orderable) fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) - else + else # piecewise linear inputs u and disturbances d: k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) q0 .= @views a0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] end @@ -1762,14 +1762,17 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇1, q1, model, x̂dnext_Z̃, a1_Z̃, û1, d1) ŝk .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇0 + k̇1) + ŵd end - if na > 0 # final residual at k+p (keep "end" in vars, solve a weird race cond.): + # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 + q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + a0end = @views A0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + if na > 0 && h < 1 # compute the final residual at k+p: x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] - a0end = @views A0_Z̃[(1 + na*(Nk-1)):((na*(Nk-1) + na))] - û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p)≈û0(k+p-1) - d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1)≈d0(k) - q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) + d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1) ≈ d0(k) k̇0end = @views K̄[(end-nx+1):end] fq_dae!(k̇0end, q0end, model, x̂dend, a0end, û0end, d0end) + else # piecewise linear inputs u and disturvances d: + q0end .= a0end .- @views A1_Z̃[(1 + na*(Nk-1)):(na*Nk)] end return geq end From 386619fb279bcd16ea8860c73bf54fe116c6f791 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 11:23:36 -0400 Subject: [PATCH 25/52] =?UTF-8?q?added:=20MHE=20+=20MHE=20+=20OC=20now=20w?= =?UTF-8?q?ork!=20=F0=9F=8D=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/transcription.jl | 67 +++++++++++++++++++----------- src/model/nonlinmodeldae.jl | 2 +- 2 files changed, 43 insertions(+), 26 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 0ddcaaa70..0fe584c0c 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1733,22 +1733,23 @@ function con_nonlinprogeq_mhe!( end d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] a1_Z̃ = @views A1_Z̃[(1 + na*(j-1)):(na*j)] q0 = @views Q0[(1 + na*(j-1)):(na*j)] q1 = @views Q̄[(1 + na*(j-1)):(na*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] - k̇0, k̇1 = @views k̄[1:nx], k̄[nx+1:2nx] + k̇0, k̇1 = @views k̄dot[1:nx], k̄dot[nx+1:2nx] if estim.direct || j < Nk d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else # special case, d0(k+1)≈d0(k), since d0(k+1) is not available at time k: d1 = d0 end - if f_threads || h < 1 || j < 2 - # we need to recompute k̇0 with multi-threading, even with h==1, since the - # previous iteration (j-1) may not be executed (iterations are re-orderable) + # we need to recompute k̇0 & q0 with multi-threading, even with h>0, since + # the previous iteration (j-1) may not be executed (iterations are re-orderable) + computeDynamicsAtBegin = f_threads || h < 1 || j < 2 + if computeDynamicsAtBegin fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) else # piecewise linear inputs u and disturbances d: k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) @@ -1819,50 +1820,55 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads Mo, no, τ = estim.Mo, transcription.no, transcription.τ + τendIsNotOne = (τ[end] < 1) na = get_na(model) nā, nk̄ = na*no, get_nk̄(model, transcription) nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk̄, nA, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x0arr components A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nK̄)] Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA + nK̄):(nx̃_nX̂ + na + nA + nK̄ + nĀ)] Dtemp = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + (Nk < He) && (geq .= 0) Ŝk̄ = @views geq[1:nŜk̄] Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + nA + na)] - Q̄ = @views geq[(1 + nŜk̄ + nA + na):()] + Q̄ = @views geq[(1 + nŜk̄ + nA + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0 = @views a0arr[1:na] + a0_Z̃ = @views a0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - a0 = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + a0_Z̃ = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] k̄_Z̃ = @views K̄_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] ā_Z̃ = @views Ā_Z̃[(1 + nā*(j-1)):(nā*j)] q0 = @views Q0[(1 + na*(j-1)):(na*j)] q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] ŝk̄ = @views Ŝk̄[(1 + nk̄*(j-1)):(nk̄*j)] if estim.direct || j < Nk - d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else - d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available + d1 = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available at time k: end # ----------------- residual at sampling times (τ=0) --------------------------- - if na > 0 && (f_threads || h < 1 || j < 2) - @views fq_dae!(k̄[1:nx], q0, model, x̂d_Z̃, a0, û0, d0) + # we need to recompute q0 with multi-threading, even with h>0 and τ[end]≈1, since + # the previous iteration (j-1) may not be executed (iterations are re-orderable) + computeDynamicsAtBegin = τendIsNotOne || f_threads || h < 1 || j < 2 + if na > 0 && computeDynamicsAtBegin + @views fq_dae!(k̄dot[1:nx] ,q0 , model, x̂d_Z̃, a0_Z̃, û0, d0) else - q0 .= @views Q0[(1 + na*(j-2)):(na*(j-1))] # q0 of the prev. iter (j-1) + q0 .= @views a0_Z̃ .- Ā_Z̃[(1 + nā*(j-1) - na):(nā*(j-1))] # a0[j] = a_no[j-1] end # ----------------- collocation constraint defects ----------------------------- - Δk = k̄ + Δk = k̄dot for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end @@ -1872,20 +1878,31 @@ function con_nonlinprogeq_mhe!( ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i, ki_Z̃ = @views k̄[(1 + (i-1)*nx):(i*nx)], k̄_Z̃[(1 + (i-1)*nx):(i*nx)] - qi, ai_Z̃ = @views q̄[(1 + (i-1)*na):(i*na)], ā_Z̃[(1 + (i-1)*na):(i*na)] - di .= (1-τ[i]).*d0 .+ τ[i].*d0next + k̇i, ki_Z̃ = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄_Z̃[(1 + (i-1)*nx):(i*nx)] + qi, ai_Z̃ = @views q̄[(1 + (i-1)*na):(i*na)], ā_Z̃[(1 + (i-1)*na):(i*na)] + di .= (1-τ[i]).*d0 .+ τ[i].*d1 if h > 0 && j < Nk - û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next - else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) not available at time k: - ûi = û0 + û1 = @views Û0[(1 + nu*j):(nu*(j+1))] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û1 + else # special case, û0(k+p)≈û0(k+p-1), since û0(k+p) is not available: + ûi = û0 end fq_dae!(k̇i, qi, model, ki_Z̃, ai_Z̃, ûi, di) end - ŝk̄ .-= k̄ + ŝk̄ .-= k̄dot + end + # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 + q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] + a0end = @views A0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + if na > 0 && h < 1 # compute the final residual at k+p: + x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] + û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) + d0end = @views estim.D0[(1 + nd*Nk):(nd*Nk + nd)] # d0(k+1) ≈ d0(k) + k̇0end = @views K̄[(end-nx+1):end] + fq_dae!(k̇0end, q0end, model, x̂dend, a0end, û0end, d0end) + else # piecewise linear inputs u and disturvances d: + q0end .= @views a0end .- Ā_Z̃[(1 + nā*Nk - na):(nā*Nk)] # a0[j] = a_no[j-1] end - Nk < He && (geq[nk̄*Nk+1:end] .= 0) return geq end diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index f79bdf69d..accefc48c 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -140,7 +140,7 @@ end Construct a nonlinear DAE model from state-space functions `fq`/`fq!` and `h`/`h!`. -It supports continuous differential and algebraic equations (DAE). The functions are +It supports continuous-time differential and algebraic equations (DAE). The functions are provided in the semi-explicit form: ```math \begin{aligned} From 3b8c92e0d2f873345b5d1a3b68e27ef19a66b0fe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 11:33:53 -0400 Subject: [PATCH 26/52] =?UTF-8?q?added:=20precompute=20`=CF=84endIsNotOne`?= =?UTF-8?q?=20at=20construction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/transcription.jl | 4 ++-- src/transcription.jl | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 0fe584c0c..fcb36655d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1819,8 +1819,8 @@ function con_nonlinprogeq_mhe!( nx̂, He = estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads - Mo, no, τ = estim.Mo, transcription.no, transcription.τ - τendIsNotOne = (τ[end] < 1) + no, τ, τendIsNotOne = transcription.no, transcription.τ, transcription.τendIsNotOne + Mo = estim.Mo na = get_na(model) nā, nk̄ = na*no, get_nk̄(model, transcription) nx̃ = estim.nε + nx̂ diff --git a/src/transcription.jl b/src/transcription.jl index 6c5aa8919..28407aaa2 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -413,6 +413,7 @@ struct OrthogonalCollocation <: CollocationMethod f_threads::Bool h_threads::Bool τ::Vector{COLLOCATION_NODE_TYPE} + τendIsNotOne::Bool function OrthogonalCollocation( h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) @@ -431,7 +432,8 @@ struct OrthogonalCollocation <: CollocationMethod else throw(ArgumentError("roots argument must be :gaussradau or :gausslegendre.")) end - return new(h, no, f_threads, h_threads, τ) + τendIsNotOne = (τ[end] < 1) + return new(h, no, f_threads, h_threads, τ, τendIsNotOne) end end From b2f68c856bc67a5d8f971338df346e8e9756254a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 11:50:19 -0400 Subject: [PATCH 27/52] added: reduce allocation in `MovingHorizonEstimator` --- src/estimator/construct.jl | 6 ++++-- src/estimator/mhe/execute.jl | 16 ++++++---------- src/estimator/mhe/transcription.jl | 13 ++++++------- test/5_test_extensions.jl | 1 - 4 files changed, 16 insertions(+), 20 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 400383ac1..af128edc1 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -10,6 +10,7 @@ struct StateEstimatorBuffer{NT<:Real} X̂ ::Vector{NT} A ::Vector{NT} Ŷ ::Vector{NT} + U ::Vector{NT} D ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} @@ -38,7 +39,7 @@ function StateEstimatorBuffer{NT}( transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) - nV̂, nŴ, nX̂, nA, nŶ, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nd*(He+1) + nV̂, nŴ, nX̂, nA, nŶ, nU, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nu*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) @@ -50,6 +51,7 @@ function StateEstimatorBuffer{NT}( X̂ = Vector{NT}(undef, nX̂) A = Vector{NT}(undef, nA) Ŷ = Vector{NT}(undef, nŶ) + U = Vector{NT}(undef, nU) D = Vector{NT}(undef, nD) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -60,7 +62,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d57d03eaa..2cd7cf852 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -54,9 +54,7 @@ function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) end return nothing end -function reset_warmstart!( - estim::MovingHorizonEstimator, ::TrapezoidalCollocation -) +function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocation) model = estim.model nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) nx̃ = nε + nx̂ @@ -74,9 +72,7 @@ function reset_warmstart!( end return nothing end -function reset_warmstart!( - estim::MovingHorizonEstimator, transcription::OrthogonalCollocation -) +function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation) model = estim.model nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) nx̃ = nε + nx̂ @@ -729,15 +725,15 @@ otherwise the state is for the next time step. """ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real model, buffer = estim.model, estim.buffer - nu, nx̂, Nk = model.nu, estim.nx̂, estim.Nk[] + nx̂, Nk = estim.nx̂, estim.Nk[] nk̄ = get_nk̄(model, estim.transcription) x̂0arr, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations + V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getx̂0arr!(x̂0arr, estim, Z̃) predict_mhe!( - V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index fcb36655d..9af64640b 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1104,8 +1104,8 @@ function set_warmstart_mhe!( Z̃s[(nx̃+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ - Û0, K = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations + V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + K = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) @@ -1223,9 +1223,8 @@ function set_warmstart_mhe!( Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Ŷ - Û0, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations - A0, Ā = Vector{NT}(undef, na*Nk), Vector{NT}(undef, nā*Nk) # TODO: remove the 2 allocations + V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero @@ -1307,8 +1306,8 @@ function set_warmstart_mhe!( Z̃s[(nx̃+nX̂+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ - Û0, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove the 2 allocations + V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old a0arr = geta0arr!(a0arr, estim, transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero diff --git a/test/5_test_extensions.jl b/test/5_test_extensions.jl index ee4ec83be..1384c0bd5 100644 --- a/test/5_test_extensions.jl +++ b/test/5_test_extensions.jl @@ -1,4 +1,3 @@ -# TODO: add `skip=(Sys.ARCH==:x86)` because bug in DAQP.jl, and activate Linux x86 CI job in juliaci.yml @testitem "LinearMPCext general" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP import LinearMPC From cb8d223482d8a419ae15a89b75142a4292ff3bd8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 12:18:07 -0400 Subject: [PATCH 28/52] =?UTF-8?q?added:=20precompute=20`nx=CC=83=3Dn=CE=B5?= =?UTF-8?q?+nx=CC=82`=20at=20MHE=20construction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/construct.jl | 6 ++++-- src/estimator/mhe/execute.jl | 9 +++------ src/estimator/mhe/transcription.jl | 19 +++++++------------ 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fec3f6b73..82a19cb58 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -109,6 +109,7 @@ struct MovingHorizonEstimator{ nym::Int nyu::Int nxs::Int + nx̃::Int p::PT Mo::SparseMatrixCSC{NT, Int} Co::SparseMatrixCSC{NT, Int} @@ -180,7 +181,7 @@ struct MovingHorizonEstimator{ As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs - nŵ = nx̂ + nŵ = nx̂ Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) @@ -203,6 +204,7 @@ struct MovingHorizonEstimator{ ES, GS, JS, BS, gc!, nc ) + nx̃ = nx̂ + nε nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) @@ -231,7 +233,7 @@ struct MovingHorizonEstimator{ covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nε, - i_ym, nx̂, nym, nyu, nxs, + i_ym, nx̂, nym, nyu, nxs, nx̃, p, Mo, Co, λo, As, Cs_u, Cs_y, nint_u, nint_ym, diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 2cd7cf852..7862bf80d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -43,8 +43,7 @@ Reset warm-starting values `estim.Z̃` at values stored in `estim.model` """ function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) model = estim.model - nx, nx̂, nε, He = model.nx, estim.nx̂, estim.nε, estim.He - nx̃ = nε + nx̂ + nx, nx̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He x0s = model.buffer.x x0s .= model.xs_0 .- model.xop estim.Z̃ .= 0 @@ -56,8 +55,7 @@ function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) end function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocation) model = estim.model - nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) - nx̃ = nε + nx̂ + nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) as_0 = get_as_0(model) x0s = model.buffer.x x0s .= model.xs_0 .- model.xop @@ -74,8 +72,7 @@ function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocatio end function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation) model = estim.model - nx, nx̂, nε, He, na = model.nx, estim.nx̂, estim.nε, estim.He, get_na(model) - nx̃ = nε + nx̂ + nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) as_0 = get_as_0(model) x0s = model.buffer.x x0s .= model.xs_0 .- model.xop diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 9af64640b..0cab8d33d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -11,10 +11,9 @@ end "Get the element indices in the decision vector `Z̃` that applies to a `Nk` window length." function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) - nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] + nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.Nk[] na = get_na(estim.model) nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk - nx̃ = estim.nε + nx̂ nx̃_nX̂_He = nx̃ + nx̂*estim.He na_nA_He = na + na*estim.He nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He @@ -28,11 +27,10 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) return i_Z̃_NK end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) - nx̂, nŵ, Nk = estim.nx̂, estim.nx̂, estim.Nk[] + nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.Nk[] na = get_na(estim.model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk - nx̃ = estim.nε + nx̂ nx̃_nX̂_He = nx̃ + nx̂*estim.He na_nA_He = na + na*estim.He nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He @@ -1480,8 +1478,7 @@ function predict_mhe!( x̂0arr, a0arr, _ , Z̃ ) nd, ny, na = model.nd, model.ny, get_na(model) - nx̂, nε, nym, Nk = estim.nx̂, estim.nε, estim.nym, estim.Nk[] - nx̃ = nε + nx̂ + nx̂, nx̃, nym, Nk = estim.nx̂, estim.nx̃, estim.nym, estim.Nk[] nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] # skip x0arr @@ -1630,11 +1627,10 @@ function con_nonlinprogeq_mhe!( x̂0arr, _ , Ŵ, Z̃ ) nu, nx, nd, nk̄ = model.nu, model.nx, model.nd, model.nk̄ - nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He + nx̂, nx̃, nxs, nŵ, He = estim.nx̂, estim.nx̃, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads nw = nŵ - nxs - nx̃ = estim.nε + nx̂ p = estim.direct ? 0 : 1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) @@ -1704,13 +1700,13 @@ function con_nonlinprogeq_mhe!( x̂0arr, a0arr, Ŵ, Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h - nx̂, nxs, nŵ, He = estim.nx̂, estim.nxs, estim.nx̂, estim.He + nx̂, nx̃, nxs, nŵ, He = estim.nx̂, estim.nx̃, estim.nxs, estim.nx̂, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads Ts = model.Ts na = get_na(model) nk̄ = get_nk̄(model, transcription) - nw, nx̃ = nŵ - nxs, estim.nε + nx̂ + nw = nŵ - nxs nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 @@ -1815,14 +1811,13 @@ function con_nonlinprogeq_mhe!( x̂0arr, a0arr, _ , Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h - nx̂, He = estim.nx̂, estim.He + nx̂, nx̃, He = estim.nx̂, estim.nx̃, estim.He Nk = estim.Nk[] f_threads = transcription.f_threads no, τ, τendIsNotOne = transcription.no, transcription.τ, transcription.τendIsNotOne Mo = estim.Mo na = get_na(model) nā, nk̄ = na*no, get_nk̄(model, transcription) - nx̃ = estim.nε + nx̂ nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk̄, nA, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 From 202d3356374a0fbb1151b4ece1727136d0f32ead Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 12:18:42 -0400 Subject: [PATCH 29/52] changed: support algebraic variables in `fill0unused!` --- src/estimator/mhe/transcription.jl | 41 +++++++++++++++++------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 0cab8d33d..aac202c7f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -46,8 +46,7 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo return i_Z̃_NK end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::SingleShooting) - nŵ, Nk = estim.nx̂, estim.Nk[] - nx̃ = estim.nε + estim.nx̂ + nx̃, nŵ, Nk = estim.nx̃, estim.nx̂, estim.Nk[] return (1):(nx̃ + nŵ*Nk) end @@ -1328,28 +1327,36 @@ end "Fill the unused decision variables in `Z̃` with `0`s (only when `Nk < He`)." function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::SingleShooting) - nŵ, nx̂, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nx̃ = estim.nε + nx̂ - Z̃[(nx̃ + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nŵ, nx̃, Nk = estim.nx̂, estim.nx̃, estim.Nk[] + Z̃[(1 + nx̃ + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::TranscriptionMethod) - nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃ = estim.nε + nx̂ - nx̃_nX̂_He = nx̃ + nx̂*He - Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector - Z̃[(nx̃_nX̂_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nŵ, nx̂, nx̃, He, Nk = estim.nx̂, estim.nx̂, estim.nx̃, estim.He, estim.Nk[] + na = get_na(estim.model) + nx̃_nX̂_He = nx̃ + nx̂*He + nx̃_nX̂_na_nA_He = nx̃ + nx̂*He + na + na*He + nx̃_nX̂_na_nA_nĀ_He = nx̃ + nx̂*He + na + na*He + na*He + Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after A0 vector + Z̃[(1 + nx̃_nX̂_na_nA_He + na*Nk):(nx̃_nX̂_na_nA_nĀ_He)] .= 0 # unused vars after Ā vector + Z̃[(1 + nx̃_nX̂_na_nA_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end function fill0unused!(Z̃, estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) - nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃ = estim.nε + nx̂ + nŵ, nx̂, nx̃, He, Nk = estim.nx̂, estim.nx̂, estim.nx̃, estim.He, estim.Nk[] + na = get_na(estim.model) nk̄ = get_nk̄(estim.model, transcription) - nx̃_nX̂_He = nx̃ + nx̂*He - nx̃_nX̂_nK_He = nx̃_nX̂_He + nk̄*He - Z̃[(nx̃ + nx̂*Nk + 1):(nx̃_nX̂_He)] .= 0 # unused decision variables after X̂0 vector - Z̃[(nx̃_nX̂_He + nk̄*Nk + 1):(nx̃_nX̂_nK_He)] .= 0 # unused decision variables after K vector - Z̃[(nx̃_nX̂_nK_He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + nā = transcription.no*na + nx̃_nX̂_He = nx̃ + nx̂*He + nx̃_nX̂_na_nA_He = nx̃ + nx̂*He + na + na*He + nx̃_nX̂_na_nA_nK̄_He = nx̃ + nx̂*He + na + na*He + nk̄*He + nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃ + nx̂*He + na + na*He + nk̄*He + nā*He + Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after A0 vector + Z̃[(1 + nx̃_nX̂_na_nA_He + nk̄*Nk):(nx̃_nX̂_na_nA_nK̄_He)] .= 0 # unused vars after K̄ vector + Z̃[(1 + nx̃_nX̂_na_nA_nK̄_He + nā*Nk):(nx̃_nX̂_na_nA_nK̄_nĀ_He)] .= 0 # unused vars after Ā vector + Z̃[(1 + nx̃_nX̂_na_nA_nK̄_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing end From 09e9727829f688d420b4c7ccc4027c704052118e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 12:38:05 -0400 Subject: [PATCH 30/52] added: remove one allocation in `getinfo(mhe)` --- src/estimator/mhe/execute.jl | 4 ++-- src/estimator/mhe/transcription.jl | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 7862bf80d..490d5cccc 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -211,9 +211,9 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ Z̃ = estim.Z̃ info = Dict{Symbol, Any}() - V̂, Ŵ, X̂0, A0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.Ŷ + V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ x̂0arr, a0arr = buffer.x̂, buffer.a - x̄, Û0, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) + x̄, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nk̄*Nk) x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index aac202c7f..f0c0b1793 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1087,7 +1087,6 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ = nε + nx̂, nŵ*He @@ -1188,7 +1187,7 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu, na = model.nu, get_na(model) + na = get_na(model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂, nA, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He @@ -1286,7 +1285,6 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer - nu = model.nu nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He From c7636ba15d30a47ce8174b580331d00a83cec0eb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 13:20:26 -0400 Subject: [PATCH 31/52] =?UTF-8?q?changed:=20renamed=20`a0`=20->=20`a=CC=82?= =?UTF-8?q?0`=20and=20`A0`=20->=20`A=CC=820`=20To=20be=20consistent=20with?= =?UTF-8?q?=20`X=CC=820`=20vector=20in=20the=20decision=20variables.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/construct.jl | 6 +- src/estimator/execute.jl | 6 +- src/estimator/mhe/construct.jl | 44 ++++++------ src/estimator/mhe/execute.jl | 96 ++++++++++++------------- src/estimator/mhe/transcription.jl | 108 ++++++++++++++--------------- src/transcription.jl | 56 +++++++-------- 6 files changed, 158 insertions(+), 158 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index af128edc1..39510d725 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -44,12 +44,12 @@ function StateEstimatorBuffer{NT}( û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) x̂ = Vector{NT}(undef, nx̂) - a = Vector{NT}(undef, na) + â = Vector{NT}(undef, na) Z̃ = Vector{NT}(undef, nZ̃) V̂ = Vector{NT}(undef, nV̂) Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) - A = Vector{NT}(undef, nA) +  = Vector{NT}(undef, nA) Ŷ = Vector{NT}(undef, nŶ) U = Vector{NT}(undef, nU) D = Vector{NT}(undef, nD) @@ -62,7 +62,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, a, Z̃, V̂, Ŵ, X̂, A, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, k̄, x̂, â, Z̃, V̂, Ŵ, X̂, Â, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty ) end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 5d546fa95..433b04e43 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -142,14 +142,14 @@ function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) end """ - ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x0, a0, d0) + ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) Same than [`ĥ!`](@ref) for [`SimModel`](@ref), but call [`h_dae!`](@ref) internally. """ -function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, a0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] - h_dae!(ŷ0, model, xd, a0, d0) + h_dae!(ŷ0, model, xd, â0, d0) mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 82a19cb58..9cc23c8e8 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1455,7 +1455,7 @@ function get_nonlinobj_op( strict = Val(true) J::Vector{JNT} = zeros(JNT, 1) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) - a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) @@ -1464,16 +1464,16 @@ function get_nonlinobj_op( Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end Z̃_J = zeros(JNT, nZ̃) J_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1575,7 +1575,7 @@ function get_nonlincon_oracle( strict = Val(true) myInf = convert(JNT, Inf) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) - a0arr::Vector{JNT}, A0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) @@ -1587,26 +1587,26 @@ function get_nonlincon_oracle( gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end function ℓ_gi( - Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = zeros(JNT, nZ̃) ∇gi_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) @@ -1619,8 +1619,8 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), @@ -1669,24 +1669,24 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end function ℓ_geq( - Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end Z̃_∇geq = zeros(JNT, nZ̃) ∇geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) @@ -1698,8 +1698,8 @@ function get_nonlincon_oracle( ∇geq_structure = init_diffstructure(∇geq) if !isnothing(hess) ∇²geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 490d5cccc..b2a6882ab 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -211,17 +211,17 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ Z̃ = estim.Z̃ info = Dict{Symbol, Any}() - V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ - x̂0arr, a0arr = buffer.x̂, buffer.a + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ + x̂0arr, â0arr = buffer.x̂, buffer.â x̄, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nk̄*Nk) x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + â0arr = getâ0arr!(â0arr, estim, estim.transcription, Z̃) Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) - V̂, X̂0, A0 = predict_mhe!( - V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + V̂, X̂0, Â0 = predict_mhe!( + V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) - Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, A0, x̂0arr, a0arr) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, Â0, x̂0arr, â0arr) J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) yopm = model.yop[estim.i_ym] Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)] @@ -288,7 +288,7 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) - a0arr, A0 = zeros(NT, na), zeros(NT, nA) + â0arr, Â0 = zeros(NT, na), zeros(NT, nA) Ŵ = zeros(NT, nŴ) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) Ŵe = zeros(NT, nŴe) @@ -299,15 +299,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher geq = zeros(NT, neq) gi = zeros(NT, ngi) J_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function J!(Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -322,15 +322,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- inequality constraint derivatives --- ∇g_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function gi!(gi, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing @@ -354,18 +354,18 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²g_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) function ℓ_gi( - Z̃, λi, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -378,15 +378,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end # --- equality constraint derivatives --- ∇geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end @@ -409,17 +409,17 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher end end ∇²geq_cache = ( - Cache(x̂0arr), Cache(a0arr), Cache(x̄), - Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(A0), + Cache(x̂0arr), Cache(â0arr), Cache(x̄), + Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) function ℓ_geq( - Z̃, λeq, x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -540,15 +540,15 @@ function getx̂0arr!(x̂0arr, estim::MovingHorizonEstimator, Z̃) return x̂0arr .= @views Z̃[(estim.nε + 1):(estim.nε + estim.nx̂)] end -"Get algebraic variable at arrival `a0arr` from the decision vector `Z̃`." -function geta0arr!( - a0arr, estim::MovingHorizonEstimator, ::CollocationMethod, Z̃ +"Get algebraic variable at arrival `â0arr` from the decision vector `Z̃`." +function getâ0arr!( + â0arr, estim::MovingHorizonEstimator, ::CollocationMethod, Z̃ ) na = get_na(estim.model) nx̃_nX̂ = estim.nε + estim.nx̂ + estim.nx̂*estim.He - return a0arr .= @views Z̃[(nx̃_nX̂ + 1):(nx̃_nX̂ + na)] + return â0arr .= @views Z̃[(nx̃_nX̂ + 1):(nx̃_nX̂ + na)] end -geta0arr!(a0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = a0arr +getâ0arr!(â0arr, ::MovingHorizonEstimator, ::TranscriptionMethod, _ ) = â0arr "Get the estimation error at arrival from the estimated state at arrival `x̂0arr`." getx̄!(x̄, estim::MovingHorizonEstimator, x̂0arr) = (x̄ .= estim.x̂0arr_old .- x̂0arr) @@ -724,13 +724,13 @@ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real model, buffer = estim.model, estim.buffer nx̂, Nk = estim.nx̂, estim.Nk[] nk̄ = get_nk̄(model, estim.transcription) - x̂0arr, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + x̂0arr, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getx̂0arr!(x̂0arr, estim, Z̃) predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃ + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing @@ -929,40 +929,40 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ end @doc raw""" - predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) -> Ŷ0 + predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, Â0, x̂0arr, â0arr) -> Ŷ0 Predict in-place the outputs of `estim` [`MovingHorizonEstimator`](@ref). This function is not used for the optimization, but it can be useful to predict the estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the -`Ŷ0` vector. The arguments `X̂0` and `A0` are computed from [`predict_mhe!`](@ref) and +`Ŷ0` vector. The arguments `X̂0` and `Â0` are computed from [`predict_mhe!`](@ref) and contains the estimated states and algebraic variable from ``k-N_k+1+p`` to ``k+p``. The -argument `x̂0arr` and `a0arr` are computed from [`getx̂0arr!`](@ref) and [`geta0arr!`](@ref). +argument `x̂0arr` and `â0arr` are computed from [`getx̂0arr!`](@ref) and [`getâ0arr!`](@ref). They respectively contains the arrival state and algebraic estimates for the time step ``k-N_k+p``. """ -function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, A0, x̂0arr, a0arr) +function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, Â0, x̂0arr, â0arr) model = estim.model nd, ny, nx̂, Nk = model.nd, model.ny, estim.nx̂, estim.Nk[] na = get_na(model) D0 = estim.D0 p = estim.direct ? 0 : 1 x̂0 = @views estim.direct ? X̂0[1:nx̂] : x̂0arr[1:nx̂] - a0 = @views estim.direct ? A0[1:na] : a0arr[1:na] + â0 = @views estim.direct ? Â0[1:na] : â0arr[1:na] for j=1:Nk d0 = @views D0[(1 + nd*j):(nd*(j+1))] # 1st data in D0 is d0(k-Nk), not used here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] - ĥ!(ŷ0, estim, estim.model, x̂0, d0) + ĥ!(ŷ0, estim, estim.model, x̂0, â0, d0) j < Nk || break x̂0 = @views X̂0[(1 + nx̂*(j-p)):(nx̂*(j-p+1))] - a0 = @views A0[(1 + na*(j-p)):(na*(j-p+1))] + â0 = @views Â0[(1 + na*(j-p)):(na*(j-p+1))] end return Ŷ0 end """ update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -971,23 +971,23 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_predictions!( - x̂0arr, a0arr, x̄, Ŵ, V̂, X̂0, A0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃) + â0arr = getâ0arr!(â0arr, estim, estim.transcription, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) - V̂, X̂0, A0 = predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + V̂, X̂0, Â0 = predict_mhe!( + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) ε = getslack(estim, Z̃) gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) geq = con_nonlinprogeq_mhe!( - geq, X̂0, A0, Û0, K̄, estim, model, transcription, x̂0arr, a0arr, Ŵ, Z̃ + geq, X̂0, Â0, Û0, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f0c0b1793..7d248ef4c 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1099,14 +1099,14 @@ function set_warmstart_mhe!( Z̃s[(nx̃+1):(nx̃+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nŵ+1):(nx̃+nŴ)] Z̃s[(nx̃+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + x̄, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ K = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) predict_mhe!( - V̂, X̂0, A0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1201,7 +1201,7 @@ function set_warmstart_mhe!( i_base = nx̃ Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views estim.Z̃[(i_base+nx̂+1):(i_base+nX̂)] Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] - # --- algebraic variables a0arr and A0 --- + # --- algebraic variables â0arr and Â0 --- i_base = nx̃ + nX̂ Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] @@ -1218,14 +1218,14 @@ function set_warmstart_mhe!( Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + x̄, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old - a0arr = geta0arr!(a0arr, estim, estim.transcription, Z̃s) + â0arr = getâ0arr!(â0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s) + predict_mhe!(V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 @@ -1300,15 +1300,15 @@ function set_warmstart_mhe!( Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nŵ+1):(nx̃+nX̂+nŴ)] Z̃s[(nx̃+nX̂+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- - x̄, a0arr = buffer.x̂, buffer.a - V̂, Ŵ, X̂0, A0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.A, buffer.U, buffer.Ŷ + x̄, â0arr = buffer.x̂, buffer.â + V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation x̂0arr = estim.x̂0arr_old - a0arr = geta0arr!(a0arr, estim, transcription, Z̃s) + â0arr = getâ0arr!(â0arr, estim, transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, a0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1336,7 +1336,7 @@ function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::TranscriptionMethod) nx̃_nX̂_na_nA_He = nx̃ + nx̂*He + na + na*He nx̃_nX̂_na_nA_nĀ_He = nx̃ + nx̂*He + na + na*He + na*He Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector - Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after A0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after Â0 vector Z̃[(1 + nx̃_nX̂_na_nA_He + na*Nk):(nx̃_nX̂_na_nA_nĀ_He)] .= 0 # unused vars after Ā vector Z̃[(1 + nx̃_nX̂_na_nA_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector return nothing @@ -1351,7 +1351,7 @@ function fill0unused!(Z̃, estim::MovingHorizonEstimator, transcription::Orthogo nx̃_nX̂_na_nA_nK̄_He = nx̃ + nx̂*He + na + na*He + nk̄*He nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃ + nx̂*He + na + na*He + nk̄*He + nā*He Z̃[(1 + nx̃ + nx̂*Nk):(nx̃_nX̂_He)] .= 0 # unused vars after X̂0 vector - Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after A0 vector + Z̃[(1 + nx̃_nX̂_He + na + na*Nk):(nx̃_nX̂_na_nA_He)] .= 0 # unused vars after Â0 vector Z̃[(1 + nx̃_nX̂_na_nA_He + nk̄*Nk):(nx̃_nX̂_na_nA_nK̄_He)] .= 0 # unused vars after K̄ vector Z̃[(1 + nx̃_nX̂_na_nA_nK̄_He + nā*Nk):(nx̃_nX̂_na_nA_nK̄_nĀ_He)] .= 0 # unused vars after Ā vector Z̃[(1 + nx̃_nX̂_na_nA_nK̄_nĀ_He + nŵ*Nk):end] .= 0 # unused vars after Ŵ vector @@ -1360,10 +1360,10 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, - x̂0arr, a0arr, Ŵ, Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, Ŵ, Z̃ + ) -> V̂, X̂0, Â0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -1376,10 +1376,10 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k \mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂} \end{aligned} ``` -The vector `A0` is ignored and returned unchanged. +The vector `Â0` is ignored and returned unchanged. """ function predict_mhe!( - V̂, X̂0, A0, _ , _ , _ , + V̂, X̂0, Â0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, _ , _ , _ , Z̃ ) @@ -1399,25 +1399,25 @@ function predict_mhe!( end V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F X̂0_res .= mul!(X̂0_res, ẼX̂, Z̃) .+ FX̂ - return V̂, X̂0, A0 + return V̂, X̂0, Â0 end @doc raw""" predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, - x̂0arr, a0arr, Ŵ, Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, Ŵ, Z̃ + ) -> V̂, X̂0, Â0 Compute the vectors when `model` is a [`NonLinModel`](@ref) with [`SingleShooting`](@ref). -The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The vector `A0` is +The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The vector `Â0` is ignored and returned unchanged. The augmented model of [`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated process noise ``\mathbf{ŵ}``. """ function predict_mhe!( - V̂, X̂0, A0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, _ , Ŵ, _ ) @@ -1462,15 +1462,15 @@ function predict_mhe!( V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 end - return V̂, X̂0, A0 + return V̂, X̂0, Â0 end @doc raw""" predict_mhe!( - V̂, X̂0, A0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, ::TranscriptionMethod, - x̂0arr, a0arr, _ , Z̃ - ) -> V̂, X̂0, A0 + x̂0arr, â0arr, _ , Z̃ + ) -> V̂, X̂0, Â0 Compute the vectors for all other cases. @@ -1478,23 +1478,23 @@ The function mutates `V̂`, `X̂0`, and `Ŷ0` vector arguments. The augmented o [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( - V̂, X̂0, A0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, transcription::TranscriptionMethod, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) nd, ny, na = model.nd, model.ny, get_na(model) nx̂, nx̃, nym, Nk = estim.nx̂, estim.nx̃, estim.nym, estim.Nk[] nx̃_nX̂ = nx̃ + nx̂*estim.He h_threads = transcription.h_threads X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] # skip x0arr - A0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + na*Nk)] # skip a0arr + Â0[1:na*Nk] .= @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + na*Nk)] # skip â0arr @threadsif h_threads for j=1:Nk if estim.direct x̂0 = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] - a0 = @views A0[(1+na*(j-1)):(na*j)] + â0 = @views Â0[(1+na*(j-1)):(na*j)] else x̂0 = @views j < 2 ? x̂0arr[1:nx̂] : X̂0[(1+nx̂*(j-2)):(nx̂*(j-1))] - a0 = @views j < 2 ? a0arr[1:na] : A0[(1+na*(j-2)):(na*(j-1))] + â0 = @views j < 2 ? â0arr[1:na] : Â0[(1+na*(j-2)):(na*(j-1))] end d0 = @views estim.D0[(1+nd*j):(nd*(j+1))] # the 1st nd elements are not needed here ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1510,9 +1510,9 @@ function predict_mhe!( if Nk < estim.He # fill unused values with 0s for tracer sparsity detection: V̂[nym*Nk+1:end] .= 0 X̂0[nx̂*Nk+1:end] .= 0 - A0[na*Nk+1:end] .= 0 + Â0[na*Nk+1:end] .= 0 end - return V̂, X̂0, A0 + return V̂, X̂0, Â0 end """ @@ -1664,7 +1664,7 @@ end con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, - x̂0arr, a0arr, Ŵ, Z̃ + x̂0arr, â0arr, Ŵ, Z̃ ) -> geq Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`TrapezoidalCollocation`](@ref). @@ -1702,7 +1702,7 @@ and also one final residual at `k+p`: function con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, - x̂0arr, a0arr, Ŵ, Z̃ + x̂0arr, â0arr, Ŵ, Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, nx̃, nxs, nŵ, He = estim.nx̂, estim.nx̃, estim.nxs, estim.nx̂, estim.He @@ -1715,8 +1715,8 @@ function con_nonlinprogeq_mhe!( nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk, nA, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x0arr components - A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping â0arr components A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) (Nk < He) && (geq .= 0) @@ -1726,10 +1726,10 @@ function con_nonlinprogeq_mhe!( @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0_Z̃ = @views a0arr[1:na] + â0_Z̃ = @views â0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - a0_Z̃ = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] @@ -1750,10 +1750,10 @@ function con_nonlinprogeq_mhe!( # the previous iteration (j-1) may not be executed (iterations are re-orderable) computeDynamicsAtBegin = f_threads || h < 1 || j < 2 if computeDynamicsAtBegin - fq_dae!(k̇0, q0, model, x̂d_Z̃, a0_Z̃, û0, d0) + fq_dae!(k̇0, q0, model, x̂d_Z̃, â0_Z̃, û0, d0) else # piecewise linear inputs u and disturbances d: k̇0 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) - q0 .= @views a0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] + q0 .= @views â0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] end if h > 0 && j < Nk û1 = @views Û0[(1 + nu*j):(nu*(j+1))] @@ -1765,7 +1765,7 @@ function con_nonlinprogeq_mhe!( end # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] - a0end = @views A0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + a0end = @views Â0_Z̃[(1 + na*(Nk-1)):(na*Nk)] if na > 0 && h < 1 # compute the final residual at k+p: x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) @@ -1782,7 +1782,7 @@ end con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) -> geq Nonlinear MHE equality constrains for [`SimModel`](@ref) and [`OrthogonalCollocation`](@ref). @@ -1813,7 +1813,7 @@ estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continui function con_nonlinprogeq_mhe!( geq, _ , _ , Û0, K̄, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, - x̂0arr, a0arr, _ , Z̃ + x̂0arr, â0arr, _ , Z̃ ) nu, nx, nd, h = model.nu, model.nx, model.nd, transcription.h nx̂, nx̃, He = estim.nx̂, estim.nx̃, estim.He @@ -1826,8 +1826,8 @@ function con_nonlinprogeq_mhe!( nx̃_nX̂ = nx̃ + nx̂*estim.He nŜk̄, nA, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 - X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x0arr components - A0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping a0arr components + X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping â0arr components K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nK̄)] Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA + nK̄):(nx̃_nX̂ + na + nA + nK̄ + nĀ)] Dtemp = estim.buffer.D @@ -1839,10 +1839,10 @@ function con_nonlinprogeq_mhe!( @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] - a0_Z̃ = @views a0arr[1:na] + â0_Z̃ = @views â0arr[1:na] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - a0_Z̃ = @views A0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] end d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] @@ -1862,9 +1862,9 @@ function con_nonlinprogeq_mhe!( # the previous iteration (j-1) may not be executed (iterations are re-orderable) computeDynamicsAtBegin = τendIsNotOne || f_threads || h < 1 || j < 2 if na > 0 && computeDynamicsAtBegin - @views fq_dae!(k̄dot[1:nx] ,q0 , model, x̂d_Z̃, a0_Z̃, û0, d0) + @views fq_dae!(k̄dot[1:nx] ,q0 , model, x̂d_Z̃, â0_Z̃, û0, d0) else - q0 .= @views a0_Z̃ .- Ā_Z̃[(1 + nā*(j-1) - na):(nā*(j-1))] # a0[j] = a_no[j-1] + q0 .= @views â0_Z̃ .- Ā_Z̃[(1 + nā*(j-1) - na):(nā*(j-1))] # a0[j] = a_no[j-1] end # ----------------- collocation constraint defects ----------------------------- Δk = k̄dot @@ -1892,7 +1892,7 @@ function con_nonlinprogeq_mhe!( end # keep "end" in names, it solves a weird race cond. with f_threads on Julia v1.13.0 q0end = @views Q0[(1 + na*Nk):(na*Nk + na)] - a0end = @views A0_Z̃[(1 + na*(Nk-1)):(na*Nk)] + a0end = @views Â0_Z̃[(1 + na*(Nk-1)):(na*Nk)] if na > 0 && h < 1 # compute the final residual at k+p: x̂dend = @views X̂0_Z̃[(1 + nx̂*(Nk-1)):((nx̂*(Nk-1) + nx))] û0end = @views Û0[(1 + nu*(Nk-1)):((nu*(Nk-1) + nu))] # û0(k+p) ≈ û0(k+p-1) diff --git a/src/transcription.jl b/src/transcription.jl index 28407aaa2..c436f5410 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -181,14 +181,14 @@ transcription method. \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ - \mathbf{A_0} \\ + \mathbf{Â_0} \\ \mathbf{Ā} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k+1) \\ - \mathbf{a_0}(k+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k+1) \\ + \mathbf{â_0}(k+2) \\ \vdots \\ - \mathbf{a_0}(k+H_p) \end{bmatrix} + \mathbf{â_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{a_1}(k+0) \\ @@ -202,19 +202,19 @@ transcription method. \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ - \mathbf{a_0}(k-N_k+p) \\ - \mathbf{A_0} \\ - \mathbf{0_a} \\ + \mathbf{â_0}(k-N_k+p) \\ + \mathbf{Â_0} \\ + \mathbf{0_â} \\ \mathbf{Ā} \\ \mathbf{0_ā} \\ \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k-N_k+p+1) \\ + \mathbf{â_0}(k-N_k+p+2) \\ \vdots \\ - \mathbf{a_0}(k+p) \end{bmatrix} + \mathbf{â_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{a_1}(k-N_k+p+0) \\ @@ -225,8 +225,8 @@ transcription method. See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at the beginning (``N_k < H_e``). The predicted outputs are computed from - the algebraic variables in ``\mathbf{A_0}``, while the values in ``\mathbf{Ā}`` are - strictly reserved for the the `fq!` function. The ``\mathbf{a_0}`` vector is at the left + the algebraic variables in ``\mathbf{Â_0}``, while the values in ``\mathbf{Ā}`` are + strictly reserved for the the `fq!` function. The ``\mathbf{â_0}`` vector is at the left endpoint of the trapezoid, while the ``\mathbf{a_1}`` is at the right endpoint. Note that the stochastic model of the unmeasured disturbances is strictly linear and @@ -344,15 +344,15 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ - \mathbf{A_0} \\ + \mathbf{Â_0} \\ \mathbf{K̄} \\ \mathbf{Ā} \end{bmatrix} , \: - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k+1) \\ - \mathbf{a_0}(k+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k+1) \\ + \mathbf{â_0}(k+2) \\ \vdots \\ - \mathbf{a_0}(k+H_p) \end{bmatrix} + \mathbf{â_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k+0) \\ @@ -366,9 +366,9 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{X̂_0} \\ \mathbf{0_x̂} \\ - \mathbf{a_0}(k-N_k+p) \\ - \mathbf{A_0} \\ - \mathbf{0_a} \\ + \mathbf{â_0}(k-N_k+p) \\ + \mathbf{Â_0} \\ + \mathbf{0_â} \\ \mathbf{K̄} \\ \mathbf{0_k̄} \\ \mathbf{Ā} \\ @@ -376,11 +376,11 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{Ŵ} \\ \mathbf{0_ŵ} \end{bmatrix} , \; - \mathbf{A_0} = \begin{bmatrix} - \mathbf{a_0}(k-N_k+p+1) \\ - \mathbf{a_0}(k-N_k+p+2) \\ + \mathbf{Â_0} = \begin{bmatrix} + \mathbf{â_0}(k-N_k+p+1) \\ + \mathbf{â_0}(k-N_k+p+2) \\ \vdots \\ - \mathbf{a_0}(k+p) \end{bmatrix} + \mathbf{â_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} \mathbf{ā}(k-N_k+p+0) \\ @@ -390,8 +390,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). ``` All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision variables at the beginning in the [`MovingHorizonEstimator`](@ref) (``N_k < H_e``). The predicted - outputs are computed from the algebraic variables in ``\mathbf{A_0}``, while the values - in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. The ``\mathbf{A_0}`` + outputs are computed from the algebraic variables in ``\mathbf{Â_0}``, while the values + in ``\mathbf{Ā}`` are strictly reserved for the `fq!` function. The ``\mathbf{Â_0}`` vector must be explicitly included in the decision variables since the output function `h!` is evaluated at different locations than the collocation points, in general. From d7cb7311050d1e5fe27b5907ebce8f8e6c8901f2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 13:56:58 -0400 Subject: [PATCH 32/52] =?UTF-8?q?debug:=20correctly=20dispatch=20`h=CC=82!?= =?UTF-8?q?`=20+=20rename=20everything?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/construct.jl | 4 ++-- src/estimator/execute.jl | 22 +++++++++++----------- src/estimator/mhe/transcription.jl | 2 +- src/model/nonlinmodeldae.jl | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 39510d725..849c2aa96 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -3,12 +3,12 @@ struct StateEstimatorBuffer{NT<:Real} û ::Vector{NT} k̄ ::Vector{NT} x̂ ::Vector{NT} - a ::Vector{NT} + â ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} Ŵ ::Vector{NT} X̂ ::Vector{NT} - A ::Vector{NT} +  ::Vector{NT} Ŷ ::Vector{NT} U ::Vector{NT} D ::Vector{NT} diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 433b04e43..4b59ce8ea 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -98,12 +98,12 @@ Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument """ function f̂!(x̂0next, û0, k̄, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] - @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] - @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] - mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + @views x̂dnext, x̂snext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] + mul!(û0, Cs_u, x̂s) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - f!(xdnext, k̄, model, xd, û0, d0, model.p) - mul!(xsnext, As, xs) + f!(x̂dnext, k̄, model, x̂d, û0, d0, model.p) + mul!(x̂snext, As, x̂s) x̂0next .+= f̂op .- x̂op return nothing end @@ -135,9 +135,9 @@ Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument """ function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] - @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] - h!(ŷ0, model, xd, d0, model.p) # y0 = h(xd, d0) - mul!(ŷ0, Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + h!(ŷ0, model, x̂d, d0, model.p) # y0 = h(xd, d0) + mul!(ŷ0, Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end @@ -148,9 +148,9 @@ Same than [`ĥ!`](@ref) for [`SimModel`](@ref), but call [`h_dae!`](@ref) inter """ function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] - @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] - h_dae!(ŷ0, model, xd, â0, d0) - mul!(ŷ0, estim.Cs_y, xs, 1, 1) # ŷ0 = y0 + Cs_y*xs + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + h_dae!(ŷ0, model, x̂d, â0, d0) + mul!(ŷ0, estim.Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 7d248ef4c..f10b6b25f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1500,7 +1500,7 @@ function predict_mhe!( ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] v̂ = @views V̂[(1 + nym*(j-1)):(nym*j)] y0m = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] - ĥ!(ŷ0, estim, model, x̂0, a0, d0) + ĥ!(ŷ0, estim, model, x̂0, â0, d0) ŷ0m = @views ŷ0[estim.i_ym] if any(isnan, y0m) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)] diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index accefc48c..2a15a9d76 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -821,7 +821,7 @@ See also [`fq_dae!`](@ref). function h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) return model.h!(y0, x0, a0, d0, model.p) end -h_dae!(y0, model::NonLinModel, x0, _ , d0) = model.h!(y0, x0, d0, model.p) +h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0) function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) From 3e2b37ba15a0858c88f38cab953a359d7c2b275d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 14:02:15 -0400 Subject: [PATCH 33/52] doc: minor details --- src/model/nonlinmodeldae.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 2a15a9d76..eceb05f07 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -816,7 +816,7 @@ fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0) = model.f!(ẋ0, x0, u0, d Call `model.h!` with the `a0` argument if [`NonLinModelDAE`](@ref), else without. -See also [`fq_dae!`](@ref). +The `a0` argument is ignored for [`SimModelODE`](@ref)s. See also [`fq_dae!`](@ref). """ function h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) return model.h!(y0, x0, a0, d0, model.p) From 03e38e8f484198ee6433601b79da7a11a82d893b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 14:11:38 -0400 Subject: [PATCH 34/52] =?UTF-8?q?debug:=20correctly=20dispatch=20in=20`h?= =?UTF-8?q?=CC=82!`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/model/nonlinmodeldae.jl | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index eceb05f07..317387e7b 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -806,10 +806,8 @@ Call `model.fq!` for [`NonLinModelDAE`](@ref) or `model.f!` for [`NonLinModel`]( Both the algebraic variable `a0` and the residual `q0` arguments are ignored if `model` is a [`NonLinModel`](@ref). """ -function fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) - return model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) -end -fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0) = model.f!(ẋ0, x0, u0, d0, model.p) +fq_dae!(ẋ0, q0, model::NonLinModelDAE, x0, a0, u0, d0) = model.fq!(ẋ0, q0, x0, a0, u0, d0, model.p) +fq_dae!(ẋ0, _ , model::NonLinModel, x0, _ , u0, d0) = model.f!(ẋ0, x0, u0, d0, model.p) """ h_dae!(y0, model, x0, a0, d0) @@ -818,10 +816,8 @@ Call `model.h!` with the `a0` argument if [`NonLinModelDAE`](@ref), else without The `a0` argument is ignored for [`SimModelODE`](@ref)s. See also [`fq_dae!`](@ref). """ -function h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) - return model.h!(y0, x0, a0, d0, model.p) -end -h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0) +h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) = model.h!(y0, x0, a0, d0, model.p) +h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0, model.p) function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) From de63cec7eb172b42bacc09591476536307016e05 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 14:29:41 -0400 Subject: [PATCH 35/52] test: new estim. and `getinfo` tests with MHE+DAE+OC --- test/2_test_state_estim.jl | 65 +++++++++++++++++++++++++++++++++++--- 1 file changed, 61 insertions(+), 4 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index fa8acb17e..044d5e372 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1460,10 +1460,67 @@ end @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 initstate!(mhe2, [0], [0], [0]) - @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 - @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] - @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 - @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 + @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 # a1 in Ā +end + +@testitem "MHE estim. & getinfo (NonLinModelDAE, OC)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer + import ForwardDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = 2 * x[] + a[] + 0.1 * d[] + end + Ts, p = 100.0, [0.01] + as_0, xs_0 = [-0.01], [0.01] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + + no, roots = 3, :gausslegendre + transcription = OrthogonalCollocation(0, no; roots, f_threads=true, h_threads=false) + mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) + preparestate!(mhe, [0.0], [0.0]) + x̂ = updatestate!(mhe, [0.0], [0.0], [0.0]) + @test x̂ ≈ zeros(mhe.nx̂) atol=1e-6 + @test mhe.x̂0 ≈ zeros(mhe.nx̂) atol=1e-6 + preparestate!(mhe, [0], [0]) + info = getinfo(mhe) + @test info[:x̂] ≈ x̂ atol=1e-6 + @test info[:Ŷ][end] ≈ 0 atol=1e-6 + for i in 1:40 + preparestate!(mhe, [0], [0]) + updatestate!(mhe, [3.0], [0], [0]) + end + preparestate!(mhe, [0], [0]) + @test mhe([0]) ≈ [0] atol=1e-3 + for i in 1:40 + preparestate!(mhe, [7.0], [0]) + updatestate!(mhe, [0], [7.0], [0]) + end + preparestate!(mhe, [7.0], [0]) + @test mhe([0]) ≈ [7.0] atol=1e-3 + + no, roots = 2, :gaussradau + # h=1 & f_threads=false options test the branch with reuse of q̄ from prev. iter: + transcription = OrthogonalCollocation(1, no; roots, f_threads=false, h_threads=true) + mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) + preparestate!(mhe2, [0.0], [0.0]) + x̂ = updatestate!(mhe2, [0.0], [0.0], [0.0]) + @test x̂ ≈ zeros(mhe2.nx̂) atol=1e-6 + @test mhe2.x̂0 ≈ zeros(mhe2.nx̂) atol=1e-6 + initstate!(mhe2, [0], [0], [0]) + @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 + @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 + @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[13:13] ≈ xs_0 # xd in K̄ + @test mhe2.Z̃[14:14] ≈ mhe2.Z̃[15:15] ≈ as_0 # a in Ā end @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin From 8ac0761f7fb87d22529ee57e9402af4ec24fe0ed Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 14:45:59 -0400 Subject: [PATCH 36/52] test: looser tolerances for MHE+DAE+OC tests --- 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 044d5e372..6c4b22d94 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1499,13 +1499,13 @@ end updatestate!(mhe, [3.0], [0], [0]) end preparestate!(mhe, [0], [0]) - @test mhe([0]) ≈ [0] atol=1e-3 + @test mhe([0]) ≈ [0] atol=5e-3 for i in 1:40 preparestate!(mhe, [7.0], [0]) updatestate!(mhe, [0], [7.0], [0]) end preparestate!(mhe, [7.0], [0]) - @test mhe([0]) ≈ [7.0] atol=1e-3 + @test mhe([0]) ≈ [7.0] atol=5e-3 no, roots = 2, :gaussradau # h=1 & f_threads=false options test the branch with reuse of q̄ from prev. iter: From 9af079f57d0c000f892a679f860b187ba3c7a538 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:08:12 -0400 Subject: [PATCH 37/52] debug: `reset_warmstart!` for MHE and OC now works. --- src/estimator/mhe/execute.jl | 62 ----------------------------- src/estimator/mhe/transcription.jl | 64 +++++++++++++++++++++++++++++- src/transcription.jl | 10 ++--- test/2_test_state_estim.jl | 4 +- 4 files changed, 70 insertions(+), 70 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index b2a6882ab..ce6f2634b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -36,68 +36,6 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) return nothing end -""" - reset_warmstart!(estim::MovingHorizonEstimator, ::TranscriptionMethod) - -Reset warm-starting values `estim.Z̃` at values stored in `estim.model` -""" -function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) - model = estim.model - nx, nx̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - estim.Z̃ .= 0 - estim.Z̃[nε+1:nε+nx] = x0s - for j in 1:He - estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s - end - return nothing -end -function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocation) - model = estim.model - nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) - as_0 = get_as_0(model) - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - a0s = as_0 - estim.Z̃ .= 0 - estim.Z̃[nε+1:nε+nx] = x0s - estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s - for j in 1:He - estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s - estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s - estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+j*na)] .= a0s - end - return nothing -end -function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation) - model = estim.model - nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) - as_0 = get_as_0(model) - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - a0s = as_0 - estim.Z̃ .= 0 - estim.Z̃[nε+1:nε+nx] = x0s - estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s - for j in 1:He - estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s - estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s - estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*nx̂+1):(nx̃+nx̂*He+na+na*He+(j-1)*nx̂+nx)] .= x0s - estim.Z̃[(nx̃+nx̂*He+na+na*He+nx*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+nx*He+j*na)] .= a0s - end - return nothing -end -function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) - model = estim.model - nx, nε = model.nx, estim.nε - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - estim.Z̃ .= 0 - estim.Z̃[nε+1:nε+nx] = x0s - return nothing -end - """ correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f10b6b25f..6d35f6c37 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1049,6 +1049,68 @@ end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing +""" + reset_warmstart!(estim::MovingHorizonEstimator, ::TranscriptionMethod) + +Reset warm-starting values `estim.Z̃` at values stored in `estim.model` +""" +function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) + model = estim.model + nx, nx̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + end + return nothing +end +function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocation) + model = estim.model + nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+j*na)] .= a0s + end + return nothing +end +function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation) + model = estim.model + nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + estim.Z̃[(nx̃ + nx̂*He + 1):(nx̃ + nx̂*He + na)] = a0s + for j in 1:He + estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*nx+1):(nx̃+nx̂*He+na+na*He+j*nx)] .= x0s + estim.Z̃[(nx̃+nx̂*He+na+na*He+nx*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+nx*He+j*na)] .= a0s + end + return nothing +end +function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) + model = estim.model + nx, nε = model.nx, estim.nε + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + return nothing +end + @doc raw""" set_warmstart_mhe!( estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var @@ -1857,7 +1919,7 @@ function con_nonlinprogeq_mhe!( else d1 = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available at time k: end - # ----------------- residual at sampling times (τ=0) --------------------------- + # ----------------- residual at sampling instants (τ=0) ----------------------- # we need to recompute q0 with multi-threading, even with h>0 and τ[end]≈1, since # the previous iteration (j-1) may not be executed (iterations are re-orderable) computeDynamicsAtBegin = τendIsNotOne || f_threads || h < 1 || j < 2 diff --git a/src/transcription.jl b/src/transcription.jl index c436f5410..c9bedbd9c 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -395,11 +395,11 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). vector must be explicitly included in the decision variables since the output function `h!` is evaluated at different locations than the collocation points, in general. - More precisely, the outputs are at the sampling times, while the collocation points are - at the roots of orthogonal polynomials, which is "optimal" for approximating the state - trajectories with polynomials of degree ``n_o``. The method then enforces the system - dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau - but only A-stable, while the latter being L-stable. See [`init_orthocolloc`](@ref), + More precisely, the outputs are at the sampling instants, while the collocation points + are at the roots of orthogonal polynomials, which is "optimal" for approximating the + state trajectories with polynomials of degree ``n_o``. The method then enforces the + system dynamics at these points. The Gauss-Legendre scheme is more accurate than + Gauss-Radau but only A-stable, while the latter being L-stable. See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 6c4b22d94..d17376c63 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1519,8 +1519,8 @@ end @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 - @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[13:13] ≈ xs_0 # xd in K̄ - @test mhe2.Z̃[14:14] ≈ mhe2.Z̃[15:15] ≈ as_0 # a in Ā + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ xs_0 # xd in K̄ + @test mhe2.Z̃[13:13] ≈ mhe2.Z̃[14:14] ≈ as_0 # a in Ā end @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin From f1b85cd9724816b5e2403cafa3bfbba346716e18 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:19:41 -0400 Subject: [PATCH 38/52] debug: correct warm-starting for MHE+DAE+TC --- src/estimator/mhe/transcription.jl | 59 ++++++++++++++++++------------ 1 file changed, 36 insertions(+), 23 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 6d35f6c37..edaa8a758 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1207,14 +1207,14 @@ The warm-starting value is provided in Extended Help. \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{0_x̂} \\ - \mathbf{a_0}(k-N_k+p|k-1) \\ - \mathbf{a_0}(k-N_k+p+1|k-1) \\ - \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \mathbf{â_0}(k-N_k+p|k-1) \\ + \mathbf{â_0}(k-N_k+p+1|k-1) \\ + \mathbf{â_0}(k-N_k+p+2|k-1) \\ \vdots - \mathbf{a_0}(k-p-2|k-1) \\ + \mathbf{â_0}(k-p-2|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{a_0}(k-p-1|k-1) \\ - \mathbf{a_0}(k-p-1|k-1) \\ - \mathbf{0_a} \\ + \mathbf{0_â} \\ \mathbf{k̄}(k-N_k+p+0|k-1) \\ \mathbf{k̄}(k-N_k+p+1|k-1) \\ \vdots \\ @@ -1240,10 +1240,11 @@ The warm-starting value is provided in Extended Help. ``` where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point - ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` include the ``n_o`` intermediate - stage predictions for the interval ``k-j``, and is also computed at the last control period. - See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for - the defintion of vectors ``\mathbf{0_x̂}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` and ``\mathbf{ā}(k-j|k-1) + include the ``n_o`` intermediate stage predictions for the interval ``k-j``, and is also + computed at the last control period. See the Extended Help of [`MultipleShooting`](@ref) + and [`OrthogonalCollocation`](@ref) for the defintion of vectors ``\mathbf{0_x̂}``, + ``\mathbf{0_â}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. """ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var @@ -1325,14 +1326,14 @@ The warm-starting value is provided in Extended Help. \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{0_x̂} \\ - \mathbf{a_0}(k-N_k+p|k-1) \\ - \mathbf{a_0}(k-N_k+p+1|k-1) \\ - \mathbf{a_0}(k-N_k+p+2|k-1) \\ + \mathbf{â_0}(k-N_k+p|k-1) \\ + \mathbf{â_0}(k-N_k+p+1|k-1) \\ + \mathbf{â_0}(k-N_k+p+2|k-1) \\ \vdots - \mathbf{a_0}(k-p-2|k-1) \\ - \mathbf{a_0}(k-p-1|k-1) \\ - \mathbf{a_0}(k-p-1|k-1) \\ - \mathbf{0_a} \\ + \mathbf{â_0}(k-p-2|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ + \mathbf{â_0}(k-p-1|k-1) \\ + \mathbf{0_â} \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ \vdots \\ @@ -1347,6 +1348,7 @@ function set_warmstart_mhe!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer + na = get_na(model) nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He @@ -1354,13 +1356,24 @@ function set_warmstart_mhe!( # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- - Z̃s[nε+1:nx̃] = estim.x̂0arr_old - # --- state estimates X̂0 --- - Z̃s[(nx̃+1):(nx̃+nX̂-nx̂)] .= @views estim.Z̃[(nx̃+nx̂+1):(nx̃+nX̂)] - Z̃s[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] .= @views estim.Z̃[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] + i_base = nε + Z̃s[i_base+1:nx̃] = estim.x̂0arr_old + # --- state estimates X̂0 --- + i_base = nx̃ + Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views estim.Z̃[(i_base+nx̂+1):(i_base+nX̂)] + Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] + # --- algebraic variables â0arr and Â0 --- + i_base = nx̃ + nX̂ + Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] + Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] + # --- algebraic variables Ā --- + i_base = nx̃ + nX̂ + na + nA + Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] + Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- process noise estimates Ŵ --- - Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nŵ+1):(nx̃+nX̂+nŴ)] - Z̃s[(nx̃+nX̂+nŴ-nŵ+1):end] .= 0 + i_base = nx̃ + nX̂ + na + nA + nĀ + Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] + Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- x̄, â0arr = buffer.x̂, buffer.â V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ From 2fafb60a115787f4791fa4544a5eef3c34957e9f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:33:59 -0400 Subject: [PATCH 39/52] doc: detail nonlinear equality constraints for MHE+DAEs+OC --- src/estimator/mhe/transcription.jl | 14 ++++++++++---- src/transcription.jl | 1 + 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index edaa8a758..ae2b246cb 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1756,7 +1756,7 @@ variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from the continu function [`fq_dae!`](@ref) and: ```math \begin{aligned} -\mathbf{k̇_0}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇_0}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ \mathbf{k̇_1}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` @@ -1765,13 +1765,13 @@ is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`] also computed from ``j = 0, 1, ... , N_k-1`` and: ```math \begin{aligned} -\mathbf{q_0}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q_0}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ \mathbf{q_1}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{a_1}(ℓ+j), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` and also one final residual at `k+p`: ```math -\mathbf{q_0}(k+p) = \mathbf{q}\Big(\mathbf{x̂_d}(k+p), \mathbf{a_0}(k+p), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) +\mathbf{q_0}(k+p) = \mathbf{q}\Big(\mathbf{x̂_d}(k+p), \mathbf{â_0}(k+p), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) ``` """ function con_nonlinprogeq_mhe!( @@ -1881,7 +1881,13 @@ are computed by: for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{x̂_d}(ℓ+j)`` vectors are extracted from the decision variables in `Z̃`. The ``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function [`fq_dae!`](@ref), -as described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the +as described in [`init_orthocolloc`](@ref). The nonlinear equality constraints also include +the residuals at the collocation points ``\mathf{q}_i(ℓ+j)``, see [`init_orthocolloc`](@ref). +Additionnaly, the residuals at the sampling instants ```\mathbf{q_0}(ℓ+j)`` are given by: +```math +\mathbf{q_0}_i(ℓ+j) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) +``` +for ``j = 0, 1, ... , N_k``. The defects for the continuity constraints and the stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). The estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ diff --git a/src/transcription.jl b/src/transcription.jl index c9bedbd9c..1ab65fbcf 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -505,6 +505,7 @@ constraints: \begin{aligned} \mathbf{q}_i(k) &= \mathbf{q}\Big(\mathbf{k}_i(k), \mathbf{a}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big) \\ &= \mathbf{0} +\end{aligned} ``` Based on the normalized time ``τ_i`` and the hold order `transcription.h`, the inputs and disturbances are either piecewise constant or linear: From 76445754209dc116f10ddc755b7b0dff89211cb4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:36:43 -0400 Subject: [PATCH 40/52] doc: minor detail --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ae2b246cb..793699f9a 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1883,7 +1883,7 @@ for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function [`fq_dae!`](@ref), as described in [`init_orthocolloc`](@ref). The nonlinear equality constraints also include the residuals at the collocation points ``\mathf{q}_i(ℓ+j)``, see [`init_orthocolloc`](@ref). -Additionnaly, the residuals at the sampling instants ```\mathbf{q_0}(ℓ+j)`` are given by: +The residuals at the sampling instants ```\mathbf{q_0}(ℓ+j)`` are other constraints: ```math \mathbf{q_0}_i(ℓ+j) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) ``` From e2615fd0bc6b218e6e121e5f86ce63e6348f41a1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:51:18 -0400 Subject: [PATCH 41/52] added: MHE+DAE to precompilation workflow --- src/precompile.jl | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/precompile.jl b/src/precompile.jl index d49bd98d9..def9ac4d5 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -104,8 +104,13 @@ R̂y = repeat([55; 30], 3) linearizemodel = linearize(nlmodel) setmodel!(mpc_kf, linearizemodel) - daemodel = NonLinModelDAE(fq_dae!, h_dae!, 1.0, 1, 1, 1, 1) + daemodel = NonLinModelDAE(fq_dae!, h_dae!, 1.0, 1, 1, 1, 1; transcription) sim!(daemodel, 2, [10]) + + mhe_dae = MovingHorizonEstimator(daemodel, He=2, direct=false) + preparestate!(mhe, [5]) + updatestate(mhe, [2], [5]) + end end # @setup_workload \ No newline at end of file From 70179c82737e2ddc90529c5d9168057d539df0ca Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 15:58:10 -0400 Subject: [PATCH 42/52] debug: precompile workload --- src/precompile.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/precompile.jl b/src/precompile.jl index def9ac4d5..84df87c39 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -108,8 +108,8 @@ R̂y = repeat([55; 30], 3) sim!(daemodel, 2, [10]) mhe_dae = MovingHorizonEstimator(daemodel, He=2, direct=false) - preparestate!(mhe, [5]) - updatestate(mhe, [2], [5]) + preparestate!(mhe_dae, [5]) + updatestate!(mhe_dae, [2], [5]) end From c17de8baf4ffa17cfc0d45d604c4c3a5e52fd337 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:01:50 -0400 Subject: [PATCH 43/52] debug: idem --- src/precompile.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/precompile.jl b/src/precompile.jl index 84df87c39..d569cb199 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -22,7 +22,7 @@ end p = (sys2.A, sys2.B, sys2.C) function fq_dae!(ẋ, res, x, a, u, _ , _ ) - ẋ[1] = -0.5*(x[1] - 0.2*u[1]) + ẋ[1] = -0.005*(x[1] - 0.2*u[1]) res .= (x .- a) return nothing end @@ -104,7 +104,7 @@ R̂y = repeat([55; 30], 3) linearizemodel = linearize(nlmodel) setmodel!(mpc_kf, linearizemodel) - daemodel = NonLinModelDAE(fq_dae!, h_dae!, 1.0, 1, 1, 1, 1; transcription) + daemodel = NonLinModelDAE(fq_dae!, h_dae!, 100.0, 1, 1, 1, 1; transcription) sim!(daemodel, 2, [10]) mhe_dae = MovingHorizonEstimator(daemodel, He=2, direct=false) From 2d1180cba1916c582ae4dd8484c0231d7f59d949 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:04:32 -0400 Subject: [PATCH 44/52] debug: compute `nA` in TC warm-starting --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 793699f9a..d1780fb5e 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1351,7 +1351,7 @@ function set_warmstart_mhe!( na = get_na(model) nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He + nx̃, nŴ, nX̂, nA = nε + nx̂, nŵ*He, nx̂*He, na*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) From be5c998dbdf09028967288359580a590ab154edf Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:07:23 -0400 Subject: [PATCH 45/52] debug: similar bugfilx --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index d1780fb5e..c21fef760 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1351,7 +1351,7 @@ function set_warmstart_mhe!( na = get_na(model) nk̄ = get_nk̄(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nA = nε + nx̂, nŵ*He, nx̂*He, na*He + nx̃, nŴ, nX̂, nA, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, na*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) From f4ce8b84b0091977c75ba726e7efd84e5be36514 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:12:43 -0400 Subject: [PATCH 46/52] added: algebraic variables over `Nk` in `getinfo(mhe)` --- src/estimator/mhe/execute.jl | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ce6f2634b..d5b4d7af8 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -99,12 +99,14 @@ following fields: - `:ε` or *`:epsilon`* : optimal slack variable, ``ε`` - `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k``, ``\mathbf{X̂}`` - `:x̂` or *`:xhat`* : optimal estimated state, ``\mathbf{x̂}_k(k+p)`` +- `:Â` or *`:Ahat`* : optimal estimated algebraic variables over ``N_k``, ``\mathbf{Â}`` - `:V̂` or *`:Vhat`* : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}`` - `:P̄` or *`:Pbar`* : estimation error covariance at arrival, ``\mathbf{P̄}`` - `:x̄` or *`:xbar`* : optimal estimation error at arrival, ``\mathbf{x̄}`` - `:Ŷ` or *`:Yhat`* : optimal estimated outputs over ``N_k``, ``\mathbf{Ŷ}`` - `:Ŷm` or *`:Yhatm`* : optimal estimated measured outputs over ``N_k``, ``\mathbf{Ŷ^m}`` - `:x̂arr` or *`:xhatarr`* : optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+p)`` +- `:âarr` or *`:ahatarr`* : optimal estimated algebraic variable at arrival, ``\mathbf{â}(k-N_k+p)`` - `:J` : objective value optimum, ``J`` - `:Ym` : measured outputs over ``N_k``, ``\mathbf{Y^m}`` - `:U` : manipulated inputs over ``N_k``, ``\mathbf{U}`` @@ -176,6 +178,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŵ] = Ŵ[1:nŵ*Nk] info[:ε] = getslack(estim, Z̃) info[:X̂] = X̂[1:nx̂*Nk] + info[:Â] = Â0[1:na*Nk] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂[1:nym*Nk] info[:P̄] = estim.P̂arr_old @@ -183,6 +186,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Ŷ] = Ŷ[1:ny*Nk] info[:Ŷm] = Ŷ[vec(estim.i_ym .+ ny.*(0:Nk-1)')] info[:x̂arr] = x̂arr + info[:âarr] = â0arr info[:J] = J info[:Ym] = Ym info[:U] = U @@ -191,9 +195,11 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real # --- non-Unicode fields --- info[:What] = info[:Ŵ] info[:xhatarr] = info[:x̂arr] + info[:ahararr] = info[:âarr] info[:epsilon] = info[:ε] info[:Xhat] = info[:X̂] - info[:xhat] = info[:x̂] + info[:Ahat] = info[:Â] + info[:ahat] = info[:â] info[:Vhat] = info[:V̂] info[:Pbar] = info[:P̄] info[:xbar] = info[:x̄] From ee821ce1b62e000c17c0b505a64b944d05792ab7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:13:01 -0400 Subject: [PATCH 47/52] debug: warm-starting with MHE and TC --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index c21fef760..39a67c289 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1349,9 +1349,9 @@ function set_warmstart_mhe!( ) where NT<:Real model, buffer = estim.model, estim.buffer na = get_na(model) - nk̄ = get_nk̄(estim.model, transcription) + nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nA, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, na*He + nx̃, nŴ, nX̂, nA, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) From 62d5ba694df74bdc7fe646170cd7b4580ff22fc4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:22:52 -0400 Subject: [PATCH 48/52] debug: `getinfo(mhe)` --- src/estimator/mhe/execute.jl | 3 +-- src/general.jl | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d5b4d7af8..a9b83766c 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -148,7 +148,7 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3) function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, get_nk̄(model, estim.transcription) - nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ + nx̂, nym, nŵ, na = estim.nx̂, estim.nym, estim.nx̂, get_na(model) Z̃ = estim.Z̃ info = Dict{Symbol, Any}() V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ @@ -199,7 +199,6 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:epsilon] = info[:ε] info[:Xhat] = info[:X̂] info[:Ahat] = info[:Â] - info[:ahat] = info[:â] info[:Vhat] = info[:V̂] info[:Pbar] = info[:P̄] info[:xbar] = info[:x̄] diff --git a/src/general.jl b/src/general.jl index e81d7c6f6..fe2829a9d 100644 --- a/src/general.jl +++ b/src/general.jl @@ -28,7 +28,8 @@ const DEFAULT_JACSPARSE = AutoSparse( ) const HIDDEN_GETINFO_KEYS_MHE = ( - :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ, + :What, :xhatarr, :ahatarr, :epsilon, :Xhat, :Ahat, :xhat, :Vhat, + :Pbar, :xbar, :Yhat, :Yhatm, :ϵ, :nablaJ, :nabla2J, :nabla2J_ncolors, :nablag, :nablag_ncolors, :nabla2lg, :nabla2lg_ncolors, :nablageq, :nablag_ncolors, :nabla2lgeq, :nabla2lgeq_ncolors From 4c8e9a8e91c2999d50e2e72dc73adfe9bf006a69 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:39:28 -0400 Subject: [PATCH 49/52] =?UTF-8?q?changed=20:=20rename=20`nA`=20->=20`nA?= =?UTF-8?q?=CC=82`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/construct.jl | 4 +-- src/estimator/mhe/construct.jl | 8 ++--- src/estimator/mhe/execute.jl | 4 +-- src/estimator/mhe/transcription.jl | 54 +++++++++++++++--------------- 4 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 849c2aa96..4541155d0 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -39,7 +39,7 @@ function StateEstimatorBuffer{NT}( transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk̄, nŵ, na) - nV̂, nŴ, nX̂, nA, nŶ, nU, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nu*He, nd*(He+1) + nV̂, nŴ, nX̂, nÂ, nŶ, nU, nD = nym*He, nŵ*He, nx̂*He, na*He, ny*He, nu*He, nd*(He+1) u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) @@ -49,7 +49,7 @@ function StateEstimatorBuffer{NT}( V̂ = Vector{NT}(undef, nV̂) Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) -  = Vector{NT}(undef, nA) +  = Vector{NT}(undef, nÂ) Ŷ = Vector{NT}(undef, nŶ) U = Vector{NT}(undef, nU) D = Vector{NT}(undef, nD) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 9cc23c8e8..3125ab540 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1449,13 +1449,13 @@ function get_nonlinobj_op( nk̄ = get_nk̄(model, estim.transcription) He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) - nŴ, nV̂, nX̂, nA, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) + nŴ, nV̂, nX̂, nÂ, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(con.i_g), length(estim.Z̃) nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) J::Vector{JNT} = zeros(JNT, 1) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) - â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nÂ) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) @@ -1569,13 +1569,13 @@ function get_nonlincon_oracle( nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) - nŴ, nV̂, nX̂, nA, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) + nŴ, nV̂, nX̂, nÂ, nZ̃ = He*nx̂, He*nym, He*nx̂, He*na, length(estim.Z̃) nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myInf = convert(JNT, Inf) x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂) - â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nA) + â0arr::Vector{JNT}, Â0::Vector{JNT} = zeros(JNT, na), zeros(JNT, nÂ) Ŵ::Vector{JNT} = zeros(JNT, nŴ) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index a9b83766c..41753e344 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -227,11 +227,11 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher nc, neq, ng = con.nc, con.neq, length(con.i_g) i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) - nV̂, nX̂, nA, nŴ = He*nym, He*nx̂, He*na, He*nx̂ + nV̂, nX̂, nÂ, nŴ = He*nym, He*nx̂, He*na, He*nx̂ nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) - â0arr, Â0 = zeros(NT, na), zeros(NT, nA) + â0arr, Â0 = zeros(NT, na), zeros(NT, nÂ) Ŵ = zeros(NT, nŴ) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) Ŵe = zeros(NT, nŴe) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 39a67c289..33702a1bb 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -13,15 +13,15 @@ end function get_i_Z̃_Nk(estim::MovingHorizonEstimator, ::TranscriptionMethod) nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.Nk[] na = get_na(estim.model) - nŴ, nX̂, nA = nŵ*Nk, nx̂*Nk, na*Nk + nŴ, nX̂, n = nŵ*Nk, nx̂*Nk, na*Nk nx̃_nX̂_He = nx̃ + nx̂*estim.He na_nA_He = na + na*estim.He nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He nx̃_nX̂_na_nA_nĀ_He = nx̃_nX̂_na_nA_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA) - (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nA) + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nÂ) + (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nÂ) (1 + nx̃_nX̂_na_nA_nĀ_He):(nx̃_nX̂_na_nA_nĀ_He + nŴ) ] return i_Z̃_NK @@ -30,7 +30,7 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo nx̂, nx̃, nŵ, Nk = estim.nx̂, estim.nx̃, estim.nx̂, estim.Nk[] na = get_na(estim.model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) - nŴ, nX̂, nA, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk + nŴ, nX̂, nÂ, nK̄, nĀ = nŵ*Nk, nx̂*Nk, na*Nk, nk̄*Nk, nā*Nk nx̃_nX̂_He = nx̃ + nx̂*estim.He na_nA_He = na + na*estim.He nx̃_nX̂_na_nA_He = nx̃_nX̂_He + na_nA_He @@ -38,7 +38,7 @@ function get_i_Z̃_Nk(estim::MovingHorizonEstimator, transcription::OrthogonalCo nx̃_nX̂_na_nA_nK̄_nĀ_He = nx̃_nX̂_na_nA_nK̄_He + na*estim.He i_Z̃_NK = [ (1):(nx̃ + nX̂); - (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nA); + (1 + nx̃_nX̂_He):(nx̃_nX̂_He + na + nÂ); (1 + nx̃_nX̂_na_nA_He):(nx̃_nX̂_na_nA_He + nK̄); (1 + nx̃_nX̂_na_nA_nK̄_He):(nx̃_nX̂_na_nA_nK̄_He + nĀ); (1 + nx̃_nX̂_na_nA_nK̄_nĀ_He):(nx̃_nX̂_na_nA_nK̄_nĀ_He + nŴ); @@ -1253,7 +1253,7 @@ function set_warmstart_mhe!( na = get_na(model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nA, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He + nx̃, nŴ, nX̂, nÂ, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) @@ -1266,18 +1266,18 @@ function set_warmstart_mhe!( Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] # --- algebraic variables â0arr and Â0 --- i_base = nx̃ + nX̂ - Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] - Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] + Z̃s[(i_base+1):(i_base+nÂ)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nÂ)] + Z̃s[(i_base+nÂ+1):(i_base+na+nÂ)] .= @views estim.Z̃[(i_base+nÂ+1):(i_base+na+nÂ)] # --- deterministic states at collocation points K̄ --- - i_base = nx̃ + nX̂ + na + nA + i_base = nx̃ + nX̂ + na + n Z̃s[(i_base+1):(i_base+nK̄-nk̄)] .= @views estim.Z̃[(i_base+nk̄+1):(i_base+nK̄)] Z̃s[(i_base+nK̄-nk̄+1):(i_base+nK̄)] .= @views estim.Z̃[(i_base+nK̄-nk̄+1):(i_base+nK̄)] # --- algebraic variables at collocation points Ā --- - i_base = nx̃ + nX̂ + na + nA + nK̄ + i_base = nx̃ + nX̂ + na + n + nK̄ Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- process noise estimates Ŵ --- - i_base = nx̃ + nX̂ + na + nA + nK̄ + nĀ + i_base = nx̃ + nX̂ + na + n + nK̄ + nĀ Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- @@ -1351,7 +1351,7 @@ function set_warmstart_mhe!( na = get_na(model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] - nx̃, nŴ, nX̂, nA, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nā*He + nx̃, nŴ, nX̂, nÂ, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) @@ -1364,14 +1364,14 @@ function set_warmstart_mhe!( Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views estim.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] # --- algebraic variables â0arr and Â0 --- i_base = nx̃ + nX̂ - Z̃s[(i_base+1):(i_base+nA)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nA)] - Z̃s[(i_base+nA+1):(i_base+na+nA)] .= @views estim.Z̃[(i_base+nA+1):(i_base+na+nA)] + Z̃s[(i_base+1):(i_base+nÂ)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nÂ)] + Z̃s[(i_base+nÂ+1):(i_base+na+nÂ)] .= @views estim.Z̃[(i_base+nÂ+1):(i_base+na+nÂ)] # --- algebraic variables Ā --- - i_base = nx̃ + nX̂ + na + nA + i_base = nx̃ + nX̂ + na + n Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- process noise estimates Ŵ --- - i_base = nx̃ + nX̂ + na + nA + nĀ + i_base = nx̃ + nX̂ + na + n + nĀ Z̃s[(i_base+1):(i_base+nŴ-nŵ)] .= @views estim.Z̃[(i_base+nŵ+1):(i_base+nŴ)] Z̃s[(i_base+nŴ-nŵ+1):end] .= 0 # --- verify definiteness of objective function --- @@ -1788,16 +1788,16 @@ function con_nonlinprogeq_mhe!( nk̄ = get_nk̄(model, transcription) nw = nŵ - nxs nx̃_nX̂ = nx̃ + nx̂*estim.He - nŜk, nA, nĀ = nx*He, na*He, na*He + nŜk, nÂ, nĀ = nx*He, na*He, na*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components - Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping â0arr components - A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nĀ)] + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) (Nk < He) && (geq .= 0) Ŝk = @views geq[1:nŜk] - Q0 = @views geq[(1 + nŜk):(nŜk + nA + na)] - Q̄ = @views geq[(1 + nŜk + nA + na):end] + Q0 = @views geq[(1 + nŜk):(nŜk + n + na)] + Q̄ = @views geq[(1 + nŜk + n + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1905,18 +1905,18 @@ function con_nonlinprogeq_mhe!( na = get_na(model) nā, nk̄ = na*no, get_nk̄(model, transcription) nx̃_nX̂ = nx̃ + nx̂*estim.He - nŜk̄, nA, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He + nŜk̄, nÂ, nK̄, nĀ = nk̄*He, na*He, nk̄*He, nā*He i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components - Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nA)] # skipping â0arr components - K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA):(nx̃_nX̂ + na + nA + nK̄)] - Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nA + nK̄):(nx̃_nX̂ + na + nA + nK̄ + nĀ)] + Â0_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + K̄_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nK̄)] + Ā_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + n + nK̄):(nx̃_nX̂ + na + n + nK̄ + nĀ)] Dtemp = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) (Nk < He) && (geq .= 0) Ŝk̄ = @views geq[1:nŜk̄] - Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + nA + na)] - Q̄ = @views geq[(1 + nŜk̄ + nA + na):end] + Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + n + na)] + Q̄ = @views geq[(1 + nŜk̄ + n + na):end] @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] From 1bb137737feff3a28c330fba64ca919ae48671bf Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 16:46:42 -0400 Subject: [PATCH 50/52] debug: correct filed in `getinfo(mhe)` --- 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 41753e344..7d5d43075 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -195,7 +195,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real # --- non-Unicode fields --- info[:What] = info[:Ŵ] info[:xhatarr] = info[:x̂arr] - info[:ahararr] = info[:âarr] + info[:ahatarr] = info[:âarr] info[:epsilon] = info[:ε] info[:Xhat] = info[:X̂] info[:Ahat] = info[:Â] From 63b4f86f04373e5247e669f0247bf2731e39e744 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 17:21:16 -0400 Subject: [PATCH 51/52] doc: minor correction --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 33702a1bb..3416f0fbc 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1883,9 +1883,9 @@ for ``j = 0, 1, ... , N_k-1``, and knowing that the ``\mathbf{k}_i(ℓ+j)`` and ``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function [`fq_dae!`](@ref), as described in [`init_orthocolloc`](@ref). The nonlinear equality constraints also include the residuals at the collocation points ``\mathf{q}_i(ℓ+j)``, see [`init_orthocolloc`](@ref). -The residuals at the sampling instants ```\mathbf{q_0}(ℓ+j)`` are other constraints: +The residuals at the sampling instants ``\mathbf{q_0}(ℓ+j)`` are other constraints: ```math -\mathbf{q_0}_i(ℓ+j) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) +\mathbf{q_0}(ℓ+j) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{â_0}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) ``` for ``j = 0, 1, ... , N_k``. The defects for the continuity constraints and the stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). The From 9752fcdde596f33c617d0e1abd5731b28a6cfc5b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 22 Sep 2026 17:50:45 -0400 Subject: [PATCH 52/52] doc: minor correction --- src/estimator/mhe/construct.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 3125ab540..b0ade0423 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -259,7 +259,7 @@ end Construct a moving horizon estimator (MHE) based on `model`. -It supports ([`LinModel`](@ref), [`NonLinModel`](@ref)), [`NonLinModelDAE`](@ref)) and +It supports ([`LinModel`](@ref), [`NonLinModel`](@ref), [`NonLinModelDAE`](@ref)) and constraints on the estimates. Additionally, `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the probability distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher,