From 9c3f11a33618ef3e978862d8564aebcb902dd9cc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 8 Oct 2026 16:21:57 -0400 Subject: [PATCH 01/13] =?UTF-8?q?doc:=20more=20precise=20notation=20for=20?= =?UTF-8?q?the=20algebraic=20variable=20This=20get=20VERY=20confusing=20wi?= =?UTF-8?q?th=20ZOH,=20it's=20better=20to=20be=20as=20precise=20as=20possi?= =?UTF-8?q?ble=20in=20the=20notation,=20and=20to=20stick=20to=20a=20conven?= =?UTF-8?q?tion:=20-=20The=20predicted=20output,=20both=20in=20MPC=20and?= =?UTF-8?q?=20MHE,=20are=20computed=20from=20the=20`a=CC=820`=20vectors=20?= =?UTF-8?q?and=20`h!`=20function,=20ALWAYS=20corresponding=20to=20the=20le?= =?UTF-8?q?ft=20limit=20of=20the=20sampling=20instants.=20This=20conventio?= =?UTF-8?q?n=20is=20intuitive=20since=20it's=20the=20holding=20of=20the=20?= =?UTF-8?q?inputs=20for=20`Ts`=20seconds=20that=20change=20the=20predicted?= =?UTF-8?q?=20outputs.=20-=20The=20variables=20in=20`A=CC=84`=20are=20stri?= =?UTF-8?q?ctly=20reserved=20for=20the=20dynamic=20function=20`fq!`.=20-?= =?UTF-8?q?=20It=20does=20not=20mean=20that=20`a=CC=820`=20vectors=20are?= =?UTF-8?q?=20never=20used=20in=20the=20`fq!`.=20A=20notorious=20exception?= =?UTF-8?q?=20is=20the=20MHE=20with=20`p=3D1`=20:=20the=20last=20`a=CC=820?= =?UTF-8?q?`=20vector=20is=20used=20in=20the=20last=20trapezoid,=20for=20e?= =?UTF-8?q?xample.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/model/nonlinmodeldae.jl | 16 ++++++------- src/transcription.jl | 47 ++++++++++++++++++++----------------- 2 files changed, 34 insertions(+), 29 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 5fab8733f..8b339a93d 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -694,12 +694,12 @@ function con_nonlinprogeq!( geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z ) nx, na = model.nx, model.na - Ts = model.Ts - x0next_Z, a0_Z, a1_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] + Ts = model.Ts + x0next_Z, aP_Z, a0_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, a1_Z, u0, d0, model.p) + model.fq!(k̇0, q0, x0, aP_Z, u0, d0, model.p) + model.fq!(k̇1, q1, x0next_Z, a0_Z, u0, d0, model.p) sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇0 + k̇1) return geq end @@ -717,9 +717,9 @@ function con_nonlinprogeq!( nx, na = model.nx, model.na Mo, no = model.Mo, transcription.no nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) - a0_Z, k̄_Z, ā_Z = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] + aP_Z, k̄_Z, ā_Z = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] q0, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] - @views model.fq!(k̄[1:nx], q0, x0, a0_Z, u0, d0, model.p) + @views model.fq!(k̄[1:nx], q0, x0, aP_Z, u0, d0, model.p) Δk = k̄ for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z[(1 + (i-1)*nx):(i*nx)] .- x0 @@ -761,8 +761,8 @@ end Solve the optimization `model.optim` problem for [`NonLinModelDAE`](@ref). After solving, the next state ``\mathbf{x_0}(k+1)`` will be stored in-place in the `x0next` -argument. The next algebraic variable ``\mathbf{a_0}(k+1)`` will be also stored at -`model.a0`. +argument. The right limit of the current algebraic variable ``\mathbf{a^+}(k+0)`` will be +also stored at `model.a0`. """ function f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) nx, na = model.nx, model.na diff --git a/src/transcription.jl b/src/transcription.jl index 1ab65fbcf..003cddb6b 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -168,13 +168,14 @@ transcription method. # Extended Help !!! details "Extended Help" - The algebraic vectors at the future time step ``\mathbf{a_0}`` is included in the - decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): + Introducing the **left limit** ``\mathbf{a_0}`` and the **right limit** ``\mathbf{a^+}`` + of the algebraic variables (to support zero-order hold inputs), the decision vector for + open-loop simulations of [`NonLinModelDAE`](@ref) include both vectors: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{x_0}(k+1) \\ - \mathbf{a_0}(k+0) \\ - \mathbf{a_1}(k+0) \end{bmatrix} + \mathbf{a^+}(k+0) \\ + \mathbf{a_0}(k+1) \end{bmatrix} ``` For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: ```math @@ -191,10 +192,10 @@ transcription method. \mathbf{â_0}(k+H_p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a_1}(k+0) \\ - \mathbf{a_1}(k+1) \\ + \mathbf{a^+}(k+0) \\ + \mathbf{a^+}(k+1) \\ \vdots \\ - \mathbf{a_1}(k+H_p-1) \end{bmatrix} + \mathbf{a^+}(k+H_p-1) \end{bmatrix} ``` and, for [`MovingHorizonEstimator`](@ref) with DAEs: ```math @@ -217,17 +218,18 @@ transcription method. \mathbf{â_0}(k+p) \end{bmatrix} \: \text{and} \: \mathbf{Ā} = \begin{bmatrix} - \mathbf{a_1}(k-N_k+p+0) \\ - \mathbf{a_1}(k-N_k+p+1) \\ + \mathbf{a^+}(k-N_k+p+0) \\ + \mathbf{a^+}(k-N_k+p+1) \\ \vdots \\ - \mathbf{a_1}(k+p-1) \end{bmatrix} + \mathbf{a^+}(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 variables at the beginning (``N_k < H_e``). The predicted outputs are computed from - 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. + the left limit values ``\mathbf{â_0}``, while the variables in the ``\mathbf{Ā}`` vector + are for the the `fq!` function. From a collocation perspective, the ``\mathbf{a^+}`` + vectors correspond to the left endpoints of the trapezoids, and ``\mathbf{â_0}``, the + right endpoints. Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). @@ -330,16 +332,19 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \vdots \\ \mathbf{a}_{n_o}(k+j) \end{bmatrix} ``` - Although not strictly needed, the current algebraic variable ``\mathbf{a_0}(k+0)`` is - still included in the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): + Although not strictly needed, the current algebraic variable at the right limit + ``\mathbf{a^+}(k+0)`` is included in the decision vector for open-loop simulations of + [`NonLinModelDAE`](@ref): ```math \mathbf{Z} = \begin{bmatrix} \mathbf{x_0}(k+1) \\ - \mathbf{a_0}(k+0) \\ + \mathbf{a^+}(k+0) \\ \mathbf{k̄}(k+0) \\ \mathbf{ā}(k+0) \end{bmatrix} ``` - For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: + Introducing the **left limit** of the algebraic variables ``\mathbf{â_0}`` (to support + zero-order hold inputs), the decision vector for [`NonLinMPC`](@ref) based on + [`NonLinModelDAE`](@ref) is: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ @@ -390,10 +395,10 @@ 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{Â_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. + outputs are computed from the left limit values ``\mathbf{â_0}``, while the variables in + the ``\mathbf{Ā}`` vector are for the 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. 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 From d308408bc6743d4bb057e1254456a9c567f08920 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 8 Oct 2026 17:14:34 -0400 Subject: [PATCH 02/13] doc: new notation in `con_nonlinproge_mhe!` for TC --- src/estimator/mhe/transcription.jl | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 91966b4f1..bd8fd93d7 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1752,7 +1752,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̇_0}(ℓ+j) + \mathbf{k̇_1}(ℓ+j)] +\mathbf{ŝ_k}(ℓ+j) = \mathbf{x̂_d}(ℓ+j) + 0.5 T_s [\mathbf{k̇^+}(ℓ+j) + \mathbf{k̇^-}(ℓ+j+1)] - \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 @@ -1761,8 +1761,8 @@ 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{â_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) +\mathbf{k̇^+}(ℓ+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a^+}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{k̇^-}(ℓ+j+1) &= \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{â_0}(ℓ+j+1), \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}`` @@ -1770,13 +1770,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{â_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) +\mathbf{q^+}(ℓ+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{a^+}(ℓ+j), \mathbf{û_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) \\ +\mathbf{q^-}(ℓ+j+1) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{â_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` -and also one final residual at `k+p`: +plus one last residual at the arrival: ```math -\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) +\mathbf{q}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{â_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) ``` """ function con_nonlinprogeq_mhe!( @@ -1784,6 +1784,9 @@ function con_nonlinprogeq_mhe!( estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) + # TODO: MODIFIER LE CODE EN FONCTION DE LA NOUVELLE NOTATION CI-DESSUS + # TODO: MODIFIER LA DOCSTRING THE OC CI-DESSUS AVEC LA NOUVELLE NOTATION (ET INIT_ORTHOCOLLOC, SI NÉCESSAIRE) + # TODO: MODIFIE LE CODE THE OC CI-DESSUS SI LA NOTATION DANS LA DOCSTRING A CHANGÉ 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 Nk = estim.Nk[] From c475411e2c46e38d5e5dca5a8b0de9322a1617f8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 8 Oct 2026 17:20:42 -0400 Subject: [PATCH 03/13] doc: minor correction --- 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 bd8fd93d7..f3c43fd4b 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1774,7 +1774,7 @@ also computed from ``j = 0, 1, ... , N_k-1`` and: \mathbf{q^-}(ℓ+j+1) &= \mathbf{q}\Big(\mathbf{x̂_d}(ℓ+j+1), \mathbf{â_0}(ℓ+j+1), \mathbf{û_0}(ℓ+j+h), \mathbf{d_0}(ℓ+j+1), \mathbf{p}\Big) \end{aligned} ``` -plus one last residual at the arrival: +plus one last residual for the arrival: ```math \mathbf{q}(ℓ) = \mathbf{q}\Big(\mathbf{x̂_d}(ℓ), \mathbf{â_0}(ℓ), \mathbf{û_0}(ℓ), \mathbf{d_0}(ℓ), \mathbf{p}\Big) ``` From 2d17694438e0f71db684321e14f3ff3557341fca Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 11 Oct 2026 12:20:57 -0400 Subject: [PATCH 04/13] wip: new notation for algebraic variable in decision var. --- src/estimator/mhe/transcription.jl | 34 ++++++++++++++---------------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index f3c43fd4b..48240fb63 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1800,12 +1800,22 @@ function con_nonlinprogeq_mhe!( 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 + nÂ)] # skipping â0arr components - A1_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] + AP_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 + n + na)] - Q̄ = @views geq[(1 + nŜk + n + na):end] + qarr = @views geq[(1 + nŜk):(nŜk + na)] + Q⁻ = @views geq[(1 + nŜk + na):(nŜk + na + nÂ)] + Q⁺ = @views geq[(1 + nŜk + na + nÂ):end] + + + + if na > 0 + k̇arr, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] + û0arr, d0arr = @views Û0[1:nu], estim.D0[1:nd] + fq_dae!(k̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) + end + @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1817,9 +1827,9 @@ 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̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a1_Z̃ = @views A1_Z̃[(1 + na*(j-1)):(na*j)] + a1_Z̃ = @views AP_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)] + q1 = @views QP[(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)] @@ -1836,7 +1846,7 @@ function con_nonlinprogeq_mhe!( 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 â0_Z̃ .- A1_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] + q0 .= @views â0_Z̃ .- AP_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))] @@ -1846,18 +1856,6 @@ 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 - # 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 Â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) - 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 a7d63c763c36c7398fbd31b5de830fb1cba5df5d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 11 Oct 2026 13:26:09 -0400 Subject: [PATCH 05/13] wip: idem --- 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 48240fb63..ac3c3b62b 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1799,62 +1799,59 @@ function con_nonlinprogeq_mhe!( 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 + nÂ)] # skipping â0arr components - AP_Z̃ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] + Â0 = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + A⁺ = @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] qarr = @views geq[(1 + nŜk):(nŜk + na)] Q⁻ = @views geq[(1 + nŜk + na):(nŜk + na + nÂ)] - Q⁺ = @views geq[(1 + nŜk + na + nÂ):end] - - + Q⁺ = @views geq[(1 + nŜk + na + nÂ):end] if na > 0 k̇arr, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] - û0arr, d0arr = @views Û0[1:nu], estim.D0[1:nd] + û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + id_0arr):(nd + 1 + id_0arr)] fq_dae!(k̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) end @threadsif f_threads for j=1:Nk if j < 2 - x̂d_Z̃ = @views x̂0arr[1:nx] - â0_Z̃ = @views â0arr[1:na] + x̂d = @views x̂0arr[1:nx] else - x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + x̂d = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] 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̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a1_Z̃ = @views AP_Z̃[(1 + na*(j-1)):(na*j)] - q0 = @views Q0[(1 + na*(j-1)):(na*j)] - q1 = @views QP[(1 + na*(j-1)):(na*j)] + a⁺ = @views A⁺[(1 + na*(j-1)):(na*j)] + q⁻ = @views Q⁻[(1 + na*(j-1)):(na*j)] + q⁺ = @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)] + x̂dnext = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] - k̇0, k̇1 = @views k̄dot[1:nx], k̄dot[nx+1:2nx] + k̇⁺, k̇⁻ = @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)] + d0next = @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 + d0next = d0 end - # we need to recompute k̇0 & q0 with multi-threading, even with h>0, since + # we need to recompute k̇⁺ & 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̃, â0_Z̃, û0, d0) + fq_dae!(k̇⁺, q⁺, model, x̂d, â⁺, û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 â0_Z̃ .- AP_Z̃[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a1[j-1] + k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) + q⁺ .= @views â0_Z̃ .- A⁺[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a⁺[j-1] end if h > 0 && j < Nk - û1 = @views Û0[(1 + nu*j):(nu*(j+1))] + û0next = @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 + û0next = û0 end - 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 + fq_dae!(k̇⁻, q⁻, model, x̂dnext, â0next, û0next, d0next) + ŝk .= @. x̂d - x̂dnext + 0.5*Ts*(k̇⁺ + k̇⁻) + ŵd end return geq end From 0fe64932ba778d7ac74f837e8b8fde7e75f258cd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 11 Oct 2026 15:35:15 -0400 Subject: [PATCH 06/13] debug: correct variable names --- src/estimator/mhe/transcription.jl | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ac3c3b62b..7e951813a 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1784,9 +1784,7 @@ function con_nonlinprogeq_mhe!( estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) - # TODO: MODIFIER LE CODE EN FONCTION DE LA NOUVELLE NOTATION CI-DESSUS - # TODO: MODIFIER LA DOCSTRING THE OC CI-DESSUS AVEC LA NOUVELLE NOTATION (ET INIT_ORTHOCOLLOC, SI NÉCESSAIRE) - # TODO: MODIFIE LE CODE THE OC CI-DESSUS SI LA NOTATION DANS LA DOCSTRING A CHANGÉ + # TODO: u0next et d0next sont des bons noms 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 Nk = estim.Nk[] @@ -1807,13 +1805,11 @@ function con_nonlinprogeq_mhe!( qarr = @views geq[(1 + nŜk):(nŜk + na)] Q⁻ = @views geq[(1 + nŜk + na):(nŜk + na + nÂ)] Q⁺ = @views geq[(1 + nŜk + na + nÂ):end] - if na > 0 k̇arr, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] - û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + id_0arr):(nd + 1 + id_0arr)] + û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + i_d0arr):(nd + i_d0arr)] fq_dae!(k̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) end - @threadsif f_threads for j=1:Nk if j < 2 x̂d = @views x̂0arr[1:nx] @@ -1840,9 +1836,9 @@ 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̇⁺, q⁺, model, x̂d, â⁺, û0, d0) + fq_dae!(k̇⁺, q⁺, model, x̂d, a⁺, û0, d0) else # piecewise linear inputs u and disturbances d: - k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇1[j-1] (prev. iter) + k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇⁻[j-1] (prev. iter) q⁺ .= @views â0_Z̃ .- A⁺[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a⁺[j-1] end if h > 0 && j < Nk From 137d688f41a3360536b031d6f8dd52b5ca1d17fe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 12 Oct 2026 13:54:07 -0400 Subject: [PATCH 07/13] doc: new source for `SparseConnectivityTracer` --- docs/make.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/make.jl b/docs/make.jl index 0a668960b..c7988d8ce 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -13,7 +13,7 @@ links = InterLinks( "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", "MathOptInterface" => "https://jump.dev/MathOptInterface.jl/stable/objects.inv", "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", - "SparseConnectivityTracer" => "https://adrianhill.de/SparseConnectivityTracer.jl/stable/objects.inv", + "SparseConnectivityTracer" => "https://juliadiff.org/SparseConnectivityTracer.jl/stable/objects.inv", "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv", "LowLevelParticleFilters" => "https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/objects.inv", "LinearMPC" => "https://darnstrom.github.io/LinearMPC.jl/stable/objects.inv", From cdf244d7cc4ed5c62c7a21c4ff26e7e293d232d7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 12 Oct 2026 14:25:04 -0400 Subject: [PATCH 08/13] wip: new notation --- src/estimator/mhe/transcription.jl | 163 ++++++++++++++--------------- src/transcription.jl | 4 +- 2 files changed, 80 insertions(+), 87 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 7e951813a..d5685855c 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1566,7 +1566,7 @@ function predict_mhe!( 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 + X̂0[1:nx̂*Nk] .= @views Z̃[(1 + nx̃):(nx̃ + nx̂*Nk)] # skip x̂0arr Â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 @@ -1793,13 +1793,13 @@ function con_nonlinprogeq_mhe!( na = get_na(model) nk̄ = get_nk̄(model, transcription) nw = nŵ - nxs - nx̃_nX̂ = nx̃ + nx̂*estim.He + nx̃_nX̂ = nx̃ + nx̂*estim.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 = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components - A⁺ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] - Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + X̂0 = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0 = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + A⁺ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nĀ)] + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0, estim.U0) (Nk < He) && (geq .= 0) Ŝk = @views geq[1:nŜk] qarr = @views geq[(1 + nŜk):(nŜk + na)] @@ -1814,40 +1814,40 @@ function con_nonlinprogeq_mhe!( if j < 2 x̂d = @views x̂0arr[1:nx] else - x̂d = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + x̂d = @views X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] 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̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a⁺ = @views A⁺[(1 + na*(j-1)):(na*j)] - q⁻ = @views Q⁻[(1 + na*(j-1)):(na*j)] - q⁺ = @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)] - â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] - ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] - k̇⁺, k̇⁻ = @views k̄dot[1:nx], k̄dot[nx+1:2nx] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + a⁺ = @views A⁺[(1 + na*(j-1)):(na*j)] + q⁺ = @views Q⁺[(1 + na*(j-1)):(na*j)] + ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] + ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] + q⁻next = @views Q⁻[(1 + na*(j-1)):(na*j)] + x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] + k̇⁺, k̇⁻next = @views k̄dot[1:nx], k̄dot[nx+1:2nx] if estim.direct || j < Nk d0next = @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: d0next = d0 end - # we need to recompute k̇⁺ & q0 with multi-threading, even with h>0, since + if h > 0 && j < Nk + û0next = @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: + û0next = û0 + end + # we need to recompute k̇⁺ & q⁺ 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̇⁺, q⁺, model, x̂d, a⁺, û0, d0) else # piecewise linear inputs u and disturbances d: - k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇⁻[j-1] (prev. iter) - q⁺ .= @views â0_Z̃ .- A⁺[(1 + na*(j-2)):(na*(j-1))] # a0[j] = a⁺[j-1] + k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇⁺[j] = k̇⁻[j-1] (prev. iter) + q⁺ .= @views a⁺ .- Â0[(1 + na*(j-2)):(na*(j-1))] # a⁺[j] = â0[j-1] (prev. iter) end - if h > 0 && j < Nk - û0next = @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: - û0next = û0 - end - fq_dae!(k̇⁻, q⁻, model, x̂dnext, â0next, û0next, d0next) - ŝk .= @. x̂d - x̂dnext + 0.5*Ts*(k̇⁺ + k̇⁻) + ŵd + fq_dae!(k̇⁻next, q⁻next, model, x̂dnext, â0next, û0next, d0next) + ŝk .= @. x̂d + 0.5*Ts*(k̇⁺ + k̇⁻next) - x̂dnext + ŵd end return geq end @@ -1882,9 +1882,10 @@ 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 left limit of the sampling instants ``\mathbf{q^-}(ℓ+j)`` are other +constraints: ```math -\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^-}(ℓ+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 @@ -1906,81 +1907,73 @@ function con_nonlinprogeq_mhe!( nx̃_nX̂ = nx̃ + nx̂*estim.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 + 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Ā)] + X̂0 = @views Z̃[(1 + nx̃):(nx̃_nX̂)] # skipping x̂0arr components + Â0 = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components + K̄ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nK̄)] + Ā = @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) + Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0, estim.U0) (Nk < He) && (geq .= 0) Ŝk̄ = @views geq[1:nŜk̄] - Q0 = @views geq[(1 + nŜk̄):(nŜk̄ + n + na)] - Q̄ = @views geq[(1 + nŜk̄ + n + na):end] + qarr = @views geq[(1 + nŜk̄):(nŜk̄ + na)] + Q⁻ = @views geq[(1 + nŜk̄ + na):(nŜk̄ + na + nÂ)] + Q̄ = @views geq[(1 + nŜk̄ + na + nÂ):end] + if na > 0 + k̇arr, 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̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) + end @threadsif f_threads for j=1:Nk if j < 2 - x̂d_Z̃ = @views x̂0arr[1:nx] - â0_Z̃ = @views â0arr[1:na] + x̂d = @views x̂0arr[1:nx] else - x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - â0_Z̃ = @views Â0_Z̃[(1 + na*(j-2)):(na*(j-2) + na)] + x̂d = @views X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] 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̄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)] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + ā = @views Ā[(1 + nā*(j-1)):(nā*j)] + q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] + ŝk̄ = @views Ŝk̄[(1 + nk̄*(j-1)):(nk̄*j)] + q⁻next = @views Q⁻[(1 + na*(j-1)):(na*j)] + x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] if estim.direct || j < Nk - d1 = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] + d0next = @views estim.D0[(1 + nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] else - d1 = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) is not available at time k: + d0next = d0 # special case: d0(k+1)≈d0(k), since d0(k+1) not available at time k end - # ----------------- 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 - if na > 0 && computeDynamicsAtBegin - @views fq_dae!(k̄dot[1:nx] ,q0 , model, x̂d_Z̃, â0_Z̃, û0, d0) - else - q0 .= @views â0_Z̃ .- Ā_Z̃[(1 + nā*(j-1) - na):(nā*(j-1))] # a0[j] = a_no[j-1] + if h > 0 && j < Nk + û0next = @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: + û0next = û0 end # ----------------- collocation constraint defects ----------------------------- Δ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̃ + Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x̂d end mul!(ŝk̄, Mo, Δk) di = @views Dtemp[(1 + nd*(j-1)):(nd*j)] - if h > 0 - ûi = similar(û0) # TODO: remove this allocation - end + ûi = similar(û0) # TODO: remove this allocation for i=1:no - 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 - û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) + k̇i, ki = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄[(1 + (i-1)*nx):(i*nx)] + qi, ai = @views q̄[(1 + (i-1)*na):(i*na)], ā[(1 + (i-1)*na):(i*na)] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next + di .= (1-τ[i]).*d0 .+ τ[i].*d0next + fq_dae!(k̇i, qi, model, ki, ai, ûi, di) end ŝ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 Â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) - 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] + # ----------------- 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 + if na > 0 && computeDynamicsAtBegin + @views fq_dae!(k̄dot[1:nx], q⁻next, model, x̂dnext, â0next, û0next, d0next) + else # â0next[j] = a_no[j-1] (prev. iter.) + q⁻next .= @views â0next .- Ā[(1 + nā*(j-1) - na):(nā*(j-1))] + end end return geq end diff --git a/src/transcription.jl b/src/transcription.jl index 003cddb6b..261f07e2a 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -168,7 +168,7 @@ transcription method. # Extended Help !!! details "Extended Help" - Introducing the **left limit** ``\mathbf{a_0}`` and the **right limit** ``\mathbf{a^+}`` + Introducing the *left limit* ``\mathbf{a_0}`` and the *right limit* ``\mathbf{a^+}`` of the algebraic variables (to support zero-order hold inputs), the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref) include both vectors: ```math @@ -342,7 +342,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \mathbf{k̄}(k+0) \\ \mathbf{ā}(k+0) \end{bmatrix} ``` - Introducing the **left limit** of the algebraic variables ``\mathbf{â_0}`` (to support + Introducing the *left limit* of the algebraic variables ``\mathbf{â_0}`` (to support zero-order hold inputs), the decision vector for [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref) is: ```math From 3e0047a42dbe5bdcfc29b87a66e684fda4b84a01 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 12 Oct 2026 15:53:18 -0400 Subject: [PATCH 09/13] changed: more precise comment --- 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 d5685855c..70b64498b 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1965,7 +1965,7 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇i, qi, model, ki, ai, ûi, di) end ŝk̄ .-= k̄dot - # ----------------- residual at sampling instants (τ=0) ----------------------- + # ----------------- residual at sampling instants (τ=1) ----------------------- # 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 From 585be13536f802c0cd1d40e469293202fc4dc08b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 13 Oct 2026 11:35:18 -0400 Subject: [PATCH 10/13] doc: minor correction --- src/transcription.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index 261f07e2a..3447c5561 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -332,7 +332,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). \vdots \\ \mathbf{a}_{n_o}(k+j) \end{bmatrix} ``` - Although not strictly needed, the current algebraic variable at the right limit + Although not strictly needed, the current algebraic variable at the *right limit* ``\mathbf{a^+}(k+0)`` is included in the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): ```math @@ -395,8 +395,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 left limit values ``\mathbf{â_0}``, while the variables in - the ``\mathbf{Ā}`` vector are for the the `fq!` function. The ``\mathbf{Â_0}`` vector + outputs are computed from the *left limit* values ``\mathbf{â_0}``, while the variables + in the ``\mathbf{Ā}`` vector are for the 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 8f210c6f0ab1ae3d7b97d828d0772c8ec2563b9f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 13 Oct 2026 15:01:21 -0400 Subject: [PATCH 11/13] debug: resolve conflict in variable names --- src/estimator/mhe/transcription.jl | 88 +++++++++++++----------------- 1 file changed, 39 insertions(+), 49 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 70b64498b..2354d7a34 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1696,7 +1696,7 @@ end Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K` vectors in argument. By introducing +The method mutates the `geq`, `X̂0`, `Û0` and `K̄` vectors in argument. By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the defects of the estimated states are computed with: ```math @@ -1742,7 +1742,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) -> geq @@ -1780,7 +1780,7 @@ plus one last residual for the arrival: ``` """ function con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) @@ -1806,37 +1806,33 @@ function con_nonlinprogeq_mhe!( Q⁻ = @views geq[(1 + nŜk + na):(nŜk + na + nÂ)] Q⁺ = @views geq[(1 + nŜk + na + nÂ):end] if na > 0 - k̇arr, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] + k̇arr, x̂darr = @views K̄dot[1:nx], x̂0arr[1:nx] û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + i_d0arr):(nd + i_d0arr)] fq_dae!(k̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) end @threadsif f_threads for j=1:Nk - if j < 2 - x̂d = @views x̂0arr[1:nx] - else - x̂d = @views X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - 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̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - a⁺ = @views A⁺[(1 + na*(j-1)):(na*j)] - q⁺ = @views Q⁺[(1 + na*(j-1)):(na*j)] - ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] - ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] - q⁻next = @views Q⁻[(1 + na*(j-1)):(na*j)] - x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] - k̇⁺, k̇⁻next = @views k̄dot[1:nx], k̄dot[nx+1:2nx] - if estim.direct || j < Nk - d0next = @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: - d0next = d0 - end + x̂d = @views j < 2 ? x̂0arr[1:nx] : X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k̄dot = @views K̄dot[(1 + nk̄*(j-1)):(nk̄*j)] + a⁺ = @views A⁺[(1 + na*(j-1)):(na*j)] + q⁺ = @views Q⁺[(1 + na*(j-1)):(na*j)] + ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] + ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] + q⁻next = @views Q⁻[(1 + na*(j-1)):(na*j)] + x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] + k̇⁺, k̇⁻next = @views k̄dot[1:nx], k̄dot[nx+1:2nx] if h > 0 && j < Nk û0next = @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: û0next = û0 end + if estim.direct || j < Nk # d0(k+1) is never needed if estim.direct + d0next = @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: + d0next = d0 + end # we need to recompute k̇⁺ & q⁺ 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 @@ -1854,7 +1850,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) -> geq @@ -1892,7 +1888,7 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) @@ -1912,6 +1908,7 @@ function con_nonlinprogeq_mhe!( K̄ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nK̄)] Ā = @views Z̃[(1 + nx̃_nX̂ + na + n + nK̄):(nx̃_nX̂ + na + n + nK̄ + nĀ)] Dtemp = estim.buffer.D + Utemp = similar(Û0) # TODO: remove this allocation Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0, estim.U0) (Nk < He) && (geq .= 0) Ŝk̄ = @views geq[1:nŜk̄] @@ -1919,19 +1916,15 @@ function con_nonlinprogeq_mhe!( Q⁻ = @views geq[(1 + nŜk̄ + na):(nŜk̄ + na + nÂ)] Q̄ = @views geq[(1 + nŜk̄ + na + nÂ):end] if na > 0 - k̇arr, x̂darr = @views K̄[1:nx], x̂0arr[1:nx] + k̇arr, x̂darr = @views K̄dot[1:nx], x̂0arr[1:nx] û0arr, d0arr = @views Û0[1:nu], estim.D0[(1 + i_d0arr):(nd + i_d0arr)] fq_dae!(k̇arr, qarr, model, x̂darr, â0arr, û0arr, d0arr) end @threadsif f_threads for j=1:Nk - if j < 2 - x̂d = @views x̂0arr[1:nx] - else - x̂d = @views X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] - end + x̂d = @views j < 2 ? x̂0arr[1:nx] : X̂0[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄dot = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄dot = @views K̄dot[(1 + nk̄*(j-1)):(nk̄*j)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] ā = @views Ā[(1 + nā*(j-1)):(nā*j)] q̄ = @views Q̄[(1 + nā*(j-1)):(nā*j)] @@ -1939,24 +1932,23 @@ function con_nonlinprogeq_mhe!( q⁻next = @views Q⁻[(1 + na*(j-1)):(na*j)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] â0next = @views Â0[(1 + na*(j-1)):(na*(j-1) + na)] - 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) not available at time k - end if h > 0 && j < Nk û0next = @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: û0next = û0 end + if estim.direct || j < Nk # d0(k+1) is never needed if estim.direct + d0next = @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) not available at time k: + d0next = d0 + end # ----------------- collocation constraint defects ----------------------------- Δk = k̄dot for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x̂d end mul!(ŝk̄, Mo, Δk) - di = @views Dtemp[(1 + nd*(j-1)):(nd*j)] - ûi = similar(û0) # TODO: remove this allocation + di, ûi = @views Dtemp[(1 + nd*(j-1)):(nd*j)], Utemp[(1 + nu*(j-1)):(nu*j)] for i=1:no k̇i, ki = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄[(1 + (i-1)*nx):(i*nx)] qi, ai = @views q̄[(1 + (i-1)*na):(i*na)], ā[(1 + (i-1)*na):(i*na)] @@ -1965,14 +1957,12 @@ function con_nonlinprogeq_mhe!( fq_dae!(k̇i, qi, model, ki, ai, ûi, di) end ŝk̄ .-= k̄dot - # ----------------- residual at sampling instants (τ=1) ----------------------- - # 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], q⁻next, model, x̂dnext, â0next, û0next, d0next) - else # â0next[j] = a_no[j-1] (prev. iter.) - q⁻next .= @views â0next .- Ā[(1 + nā*(j-1) - na):(nā*(j-1))] + # ----------------- residual at sampling instants (τ=1) ------------------------ + if na > 0 && τendIsNotOne + k̇⁻next = @views k̄dot[1:nx] + fq_dae!(k̇⁻next, q⁻next, model, x̂dnext, â0next, û0next, d0next) + else + q⁻next .= @views â0next .- ā[end-na+1:end] end end return geq From 912155e5adde84e4e2ca2e9202b070aca394d197 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 13 Oct 2026 15:30:24 -0400 Subject: [PATCH 12/13] debug: correct variable name --- 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 2354d7a34..a7f9a9118 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1839,7 +1839,7 @@ function con_nonlinprogeq_mhe!( if computeDynamicsAtBegin fq_dae!(k̇⁺, q⁺, model, x̂d, a⁺, û0, d0) else # piecewise linear inputs u and disturbances d: - k̇⁺ .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇⁺[j] = k̇⁻[j-1] (prev. iter) + k̇⁺ .= @views K̄dot[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k̇⁺[j] = k̇⁻[j-1] (prev. iter) q⁺ .= @views a⁺ .- Â0[(1 + na*(j-2)):(na*(j-1))] # a⁺[j] = â0[j-1] (prev. iter) end fq_dae!(k̇⁻next, q⁻next, model, x̂dnext, â0next, û0next, d0next) From 92c286f2d665cd4cc144ad519e889937224e4d2e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 13 Oct 2026 17:07:49 -0400 Subject: [PATCH 13/13] test: slightly looser tolerances in MHE with collocations --- test/2_test_state_estim.jl | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b47ebeee8..91a386e0f 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1446,12 +1446,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]) @@ -1470,8 +1470,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 # xd in X̂0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 @@ -1501,12 +1501,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]) @@ -1526,8 +1526,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 # xd in X̂0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0