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", diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 91966b4f1..a7f9a9118 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 @@ -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 @@ -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,20 +1770,21 @@ 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 for 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!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) + # 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[] @@ -1792,75 +1793,64 @@ 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_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) + 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] - 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̄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_Z̃ = @views x̂0arr[1:nx] - â0_Z̃ = @views â0arr[1:na] - 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 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 - 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)] - 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̄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)] + 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: - 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̇⁺ & 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̇0, q0, model, x̂d_Z̃, â0_Z̃, û0, d0) + fq_dae!(k̇⁺, q⁺, model, x̂d, a⁺, û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] - 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 + 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̇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)] + 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 @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄, + geq, _ , _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) -> geq @@ -1888,16 +1878,17 @@ 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 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,81 +1903,67 @@ 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) + 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̄] - 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̄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_Z̃ = @views x̂0arr[1:nx] - â0_Z̃ = @views â0arr[1:na] - 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)] - 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)] - if estim.direct || j < Nk - d1 = @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: + 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)] + 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 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 - # ----------------- 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 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̄_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 + di, ûi = @views Dtemp[(1 + nd*(j-1)):(nd*j)], Utemp[(1 + nu*(j-1)):(nu*j)] 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 (τ=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 end 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..3447c5561 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 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