From 4b11e912035c2bce728869cbc1e4989d3d1750a4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 3 Sep 2025 09:46:15 -0400 Subject: [PATCH 1/6] doc: add trapezoidal collocation to readme --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 680d47754..6797cc193 100644 --- a/README.md +++ b/README.md @@ -115,6 +115,7 @@ for more detailed examples. - supported transcription methods of the optimization problem: - direct single shooting - direct multiple shooting + - trapezoidal collocation - additional information about the optimum to ease troubleshooting - real-time control loop features: - implementations that carefully limits the allocations From 1ccfe12bbc168c6f438c00fd1d4f0b74b3063753 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Sep 2025 11:11:25 -0400 Subject: [PATCH 2/6] doc: details on internal `predict!`, `con_nonlinprog!` and `con_nonlinprogeq!` --- docs/src/internals/predictive_control.md | 3 ++ docs/src/internals/state_estim.md | 2 + src/controller/transcription.jl | 33 +++++++++++----- src/estimator/construct.jl | 11 ++++-- src/estimator/mhe/execute.jl | 48 +++++++++++++++--------- 5 files changed, 67 insertions(+), 30 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 18e960663..2a6480a86 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -41,5 +41,8 @@ ModelPredictiveControl.linconstrainteq! ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) ModelPredictiveControl.set_warmstart! +ModelPredictiveControl.predict! +ModelPredictiveControl.con_nonlinprog! +ModelPredictiveControl.con_nonlinprogeq! ModelPredictiveControl.getinput! ``` diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 8b04a57bd..fe0ebd6ad 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -40,6 +40,8 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel) ```@docs ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) ModelPredictiveControl.set_warmstart_mhe! +ModelPredictiveControl.predict_mhe! +ModelPredictiveControl.con_nonlinprog_mhe! ``` ## Remove Operating Points diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8f7f602b8..bb2864ca3 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1119,8 +1119,14 @@ getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_ Compute the predictions `Ŷ0`, terminal states `x̂0end` if model is a [`LinModel`](@ref). The method mutates `Ŷ0` and `x̂0end` vector arguments. The `x̂end` vector is used for -the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computations are -identical for any [`TranscriptionMethod`](@ref) if the model is linear. +the terminal constraints applied on ``\mathbf{x̂_0}(k+H_p)``. The computations are +identical for any [`TranscriptionMethod`](@ref) if the model is linear: +```math +\begin{aligned} +\mathbf{Ŷ_0} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ +\mathbf{x̂_0}(k+H_p) &= \mathbf{ẽ_x̂ Z̃} + \mathbf{f_x̂} +\end{aligned} +``` """ function predict!( Ŷ0, x̂0end, _, _, _, @@ -1142,7 +1148,8 @@ end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). -The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of +[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop. """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K0, @@ -1174,17 +1181,18 @@ end predict!( Ŷ0, x̂0end, _ , _ , _ , mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod, - U0, Z̃ + _ , Z̃ ) -> Ŷ0, x̂0end Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). -The method mutates `Ŷ0` and `x̂0end` arguments. +The method mutates `Ŷ0` and `x̂0end` arguments. The states `X̂0` are extracted from the +decisions variables `Z̃` and the augmented output function [`ĥ!`](@ref) is applied on them. """ function predict!( Ŷ0, x̂0end, _, _, _, mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod, - U0, Z̃ + _ , Z̃ ) nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ] @@ -1209,7 +1217,7 @@ end Nonlinear constrains when `model` is a [`LinModel`](@ref). The method mutates the `g` vectors in argument and returns it. Only the custom constraints -are include in the `g` vector. +`gc` are include in the `g` vector. """ function con_nonlinprog!( g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ @@ -1286,7 +1294,7 @@ function con_nonlinprog!( return g end -""" +@doc raw""" con_nonlinprogeq!( geq, X̂0, Û0, K0 mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, @@ -1295,7 +1303,12 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The defects are +updated with: +```math +\mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) + + \text{TODO} +``` """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1394,4 +1407,6 @@ function con_nonlinprogeq!( end return geq end + +"No equality constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq \ No newline at end of file diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 1097cb297..8ae2558b4 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -356,11 +356,14 @@ end """ default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) -One integrator on each measured output by default if `model` is not a [`LinModel`](@ref). +One integrator on each measured output by default for other cases e.g. [`NonLinModel`](@ref). -Theres is no verification the augmented model remains observable. If the integrator quantity -per manipulated input `nint_u ≠ 0`, the method returns zero integrator on each measured -output. +If the integrator quantity per manipulated input `nint_u ≠ 0`, the method returns zero +integrator on each measured output. + +!!! warning + Theres is no verification the augmented model remains observable. The resulting + [`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref). """ function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) validate_ym(model, i_ym) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 9d1551421..0d62381d9 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -115,7 +115,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) û0, k0, ŷ0 = buffer.û, buffer.k, buffer.ŷ - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] x̄ = estim.x̂0arr_old - x̂0arr X̂0 = [x̂0arr; X̂0] @@ -418,7 +418,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real # --------- update estimate ----------------------- û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next return estim.Z̃ @@ -461,7 +461,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var # --- process noise estimates Ŵ --- Z̃s[nx̃+1:end] = estim.Ŵ # verify definiteness of objective function: - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] = 0 @@ -578,16 +578,22 @@ function obj_nonlinprog!( return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ end -""" - predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 +@doc raw""" + predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. -The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The vector `V̂` is the -estimated sensor noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from -``k-N_k+2`` to ``k+1``. +The function mutates `V̂` and `X̂0` vector arguments. The vector `V̂` is the estimated sensor +noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k-N_k+2`` to +``k+1``. The computations are (by truncating the matrices when `N_k < H_e`): +```math +\begin{aligned} +\mathbf{V̂} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ +\mathbf{X̂_0} &= \mathbf{Ẽ_x̂ Z̃} + \mathbf{F_x̂} +\end{aligned} +``` """ -function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) +function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk nZ̃ = nϵ + estim.nx̂ + nŴ @@ -596,8 +602,16 @@ function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinMod return V̂, X̂0 end -"Compute the two vectors when `model` is not a `LinModel`." -function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) +@doc raw""" + predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 + +Compute the vectors when `model` is *not* a [`LinModel`](@ref). + +The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. 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, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nϵ + nx̂ @@ -646,18 +660,18 @@ The method mutates all the arguments before `estim` argument. """ function update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) model = estim.model - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) ϵ = getϵ(estim, Z̃) - g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) + g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ϵ) return nothing end """ - con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) + con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) -> g -Nonlinear constrains for [`MovingHorizonEstimator`](@ref). +Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref). """ -function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ) +function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ) nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[] nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] for i in eachindex(g) @@ -684,7 +698,7 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂ end "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g +con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g "Throw an error if P̂ != nothing." function setstate_cov!(::MovingHorizonEstimator, P̂) From 7db34ce79f91109afe980aaa1800795f9c3c4495 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Sep 2025 15:54:56 -0400 Subject: [PATCH 3/6] doc: minor correction --- src/estimator/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 0bcf08c9a..8336ab1de 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -22,7 +22,7 @@ By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_mo the function returns the next state of the augmented model, as deviation vectors: ```math \begin{aligned} - \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) + \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\ \mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) \end{aligned} ``` From 5fa54d29e226d2a0653ca56b071e2962ba81cd5f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Sep 2025 15:55:18 -0400 Subject: [PATCH 4/6] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 4caf686c7..e6914e024 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.9.0" +version = "1.9.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" From 42e6549206a19386776a41dffa60a72156121298 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Sep 2025 17:12:32 -0400 Subject: [PATCH 5/6] doc: internal details on the defect computation --- src/controller/transcription.jl | 55 ++++++++++++++++++++------------- src/estimator/execute.jl | 15 ++++++++- 2 files changed, 48 insertions(+), 22 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fe779bca4..95ca0043e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1149,7 +1149,7 @@ end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of -[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop. +[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop: """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K0, @@ -1302,12 +1302,14 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The defects are -updated with: +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The nonlinear +equality constraints `geq` only includes the state defects, computed with: ```math \mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) - + \text{TODO} + - \mathbf{x̂_0^†}(k+1) ``` +in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variables +`Z̃`, and ``\mathbf{f̂}``, the augmented state function defined in [`f̂!`](@ref). """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1346,21 +1348,33 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The deterministic -and stochastic states are handled separately since collocation methods require -continuous-time state-space models, and the stochastic model of the unmeasured disturbances -is discrete-time. Also note that operating points in `model` are typically zeros for -[`NonLinModel`](@ref), but they are handled rigorously here if it's not the case. It should -be noted that linearization of continuous-time dynamics at non-equilibrium points leads to: +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. + +The nonlinear equality constraints `geq` only includes the state defects. The deterministic +and stochastic states are handled separately since collocation methods require continuous- +time state-space models, and the stochastic model of the unmeasured disturbances +is discrete-time. The deterministic and stochastic defects are respectively computed with: ```math - \mathbf{ẋ_0}(t) ≈ \mathbf{A x_0}(t) + \mathbf{B_u u_0}(t) + \mathbf{B_d d_0}(t) +\begin{aligned} +\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0^†}(k+1) + + 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\ +\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s^†}(k+1) +\end{aligned} ``` -as opposed to, for discrete-time models: +in which ``\mathbf{x_0^†}`` and ``\mathbf{x_s^†}`` are the deterministic and stochastic +states extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are +evaluated from the continuous-time function `model.f!` and: ```math - \mathbf{x_0}(k+1) ≈ \mathbf{A x_0}(k) + \mathbf{B_u u_0}(k) + \mathbf{B_d d_0}(k) - + \mathbf{f_{op}} - \mathbf{x_{op}} +\begin{aligned} +\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\ +\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0^†}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) +\end{aligned} ``` -hence no need to add `model.fop` and subtract `model.xop` in the collocation equations. +and the input of the augmented model is: +```math +\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) +``` +the ``\mathbf{A_s, C_{s_u}}`` matrices are defined in [`init_estimstoch`](@ref) doc. """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1391,13 +1405,12 @@ function con_nonlinprogeq!( sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - ẋ0, ẋ0next = @views k0[1:nx], k0[nx+1:2*nx] - # no need to handle model.fop and model.xop, see docstring: - model.f!(ẋ0, xd, û0, d0, p) - model.f!(ẋ0next, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u + k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] + model.f!(k1, xd, û0, d0, p) + model.f!(k2, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u xsnext = @views x̂0next[nx+1:end] mul!(xsnext, As, xs) - sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(ẋ0 + ẋ0next) + sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2) ssnext .= @. xsnext - xsnext_Z̃ x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) d0 = d0next @@ -1405,5 +1418,5 @@ function con_nonlinprogeq!( return geq end -"No equality constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." +"No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq \ No newline at end of file diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 8336ab1de..45cc50558 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -65,10 +65,23 @@ function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end -""" +@doc raw""" f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref). + +# Extended Help +!!! details "Extended Help" + + This method computes: + ```math + \begin{aligned} + \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k) + + \mathbf{f̂_{op}} - \mathbf{x̂_{op}} \\ + \mathbf{ŷ_0}(k) &= \mathbf{Ĉ x̂_0}(k) + \mathbf{D̂_d d_0}(k) + \end{aligned} + ``` + with the augmented matrices constructed by [`augment_model`](@ref). """ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) mul!(x̂0next, estim.Â, x̂0) From 29be8055b12113ba4325ad727cecd225fee42083 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Sep 2025 17:55:48 -0400 Subject: [PATCH 6/6] doc: add link for details on the two kind of defects --- src/controller/transcription.jl | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 95ca0043e..8c512140b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -75,8 +75,7 @@ order hold) between the samples, but linear interpolation will be added soon. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle -moderately stiff systems and is A-stable. However, it may not be as efficient as more -advanced collocation methods for highly stiff systems. Note that the built-in [`StateEstimator`](@ref) +moderately stiff systems and is A-stable. Note that 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). See Extended Help for more details. @@ -89,7 +88,8 @@ transcription method. Note that the stochastic model of the unmeasured disturbances is strictly discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods require continuous-time dynamics. Because of this, the stochastic states are transcribed - separately using a [`MultipleShooting`](@ref) method. + separately using a [`MultipleShooting`](@ref) method. See [`con_nonlinprogeq!`](@ref) + for more details. """ struct TrapezoidalCollocation <: CollocationMethod nc::Int @@ -1149,7 +1149,13 @@ end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of -[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop: +[`f̂!`](@ref) and [`ĥ!`](@ref) functions is called recursively in a `for` loop: +```math +\begin{aligned} +\mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k) \Big) \\ +\mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k) \Big) +\end{aligned} +``` """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K0, @@ -1185,8 +1191,12 @@ end Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). -The method mutates `Ŷ0` and `x̂0end` arguments. The states `X̂0` are extracted from the -decisions variables `Z̃` and the augmented output function [`ĥ!`](@ref) is applied on them. +The method mutates `Ŷ0` and `x̂0end` arguments. The augmented output function [`ĥ!`](@ref) +is called multiple times in a `for` loop: +```math +\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0^†}(k+j), \mathbf{d_0}(k) \Big) +``` +in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variable `Z̃`. """ function predict!( Ŷ0, x̂0end, _, _, _, @@ -1303,7 +1313,7 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The nonlinear -equality constraints `geq` only includes the state defects, computed with: +equality constraints `geq` only includes the augmented state defects, computed with: ```math \mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) - \mathbf{x̂_0^†}(k+1)