From 774d84ee2704d002ba157d592236b3da9d83c714 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 3 Oct 2026 14:10:19 -0400 Subject: [PATCH 01/28] wip: `NonLinModelDAE` support in `NonLinMPC` --- src/controller/construct.jl | 4 ++-- src/controller/nonlinmpc.jl | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 2c195cf60..8672bf9c6 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -94,7 +94,7 @@ end "Outer constructor to validate and convert weight matrices if necessary." function ControllerWeights( - model::SimModelODE{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 + model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 ) where {NT<:Real} validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp) @@ -102,7 +102,7 @@ function ControllerWeights( end "Validate predictive controller weight and horizon specified values." -function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) +function validate_weights(model::SimModel, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) nu, ny = model.nu, model.ny nM, nN, nL = ny*Hp, nu*Hc, nu*Hp Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f7f2749c2..5b8a55cf2 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -229,7 +229,8 @@ This controller allocates memory at each time step for the optimization. Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). -- `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. +- `transcription=default_transcription(model)` : a [`TranscriptionMethod`](@ref) object, + default to [`SingleShooting`](@ref) or `model.transcription` for [`NonLinModelDAE`](@ref). - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective @@ -364,7 +365,7 @@ function NonLinMPC( gc ::Function = gc!, nc::Int = 0, p = model.p, - transcription::TranscriptionMethod = SingleShooting(), + transcription::TranscriptionMethod = default_transcription(model), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -437,7 +438,7 @@ function NonLinMPC( gc ::Function = gc!, nc = 0, p = estim.model.p, - transcription::TranscriptionMethod = SingleShooting(), + transcription::TranscriptionMethod = default_transcription(estim.model), optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), @@ -464,6 +465,7 @@ function NonLinMPC( end default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) +default_estimator(model::SimModelDAE; kwargs...) = MovingHorizonEstimator(model; kwargs...) default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) """ From 67e5cb8eb5f2aac57d16aa009d4451cedddce73d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 5 Oct 2026 09:04:52 -0400 Subject: [PATCH 02/28] doc: minor modification in `SimModel` --- src/sim_model.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index 3d4f4d9e8..8c755dc52 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -1,7 +1,7 @@ @doc raw""" abstract type SimModel end -Supertype of [`LinModel`](@ref) and [`NonLinModel`](@ref) types. +Supertype of all the plant model types. --- From 00a4a7a02b488a5662d8da2fc35102c517322ca2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 5 Oct 2026 17:11:22 -0400 Subject: [PATCH 03/28] added: `init_predmat` for DAE and MPC --- src/controller/transcription.jl | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a6d893050..5e8ee0e1b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -3,10 +3,12 @@ function get_nZ_mpc(estim::StateEstimator, ::SingleShooting, _ , Hc) return estim.model.nu*Hc end function get_nZ_mpc(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) - return estim.model.nu*Hc + estim.nx̂*Hp + na = get_na(estim.model) + return estim.model.nu*Hc + estim.nx̂*Hp + 2*na*Hp end function get_nZ_mpc(estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc) - return estim.model.nu*Hc + estim.nx̂*Hp + estim.model.nx*transcription.no*Hp + no, na = transcription.no, get_na(estim.model) + return estim.model.nu*Hc + estim.nx̂*Hp + na*Hp + estim.model.nx*no*Hp + na*no*Hp end @doc raw""" @@ -241,13 +243,13 @@ end """ init_predmat( - model::NonLinModel, estim, transcription::SingleShooting, Hp, Hc, nb + model::SimModel, estim, transcription::SingleShooting, Hp, Hc, nb ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ -Return empty matrices for [`SingleShooting`](@ref) of [`NonLinModel`](@ref) +Return empty matrices for non-[`LinModel`](@ref) and [`SingleShooting`](@ref). """ function init_predmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, _ + model::SimModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd nZ = get_nZ_mpc(estim, transcription, Hp, Hc) @@ -263,10 +265,10 @@ end @doc raw""" init_predmat( - model::NonLinModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb + model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ -Return the terminal state matrices for [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). +Return the terminal state matrices for non-`LinModel` and other [`TranscriptionMethod`](@ref). The output prediction matrices are all empty matrices. The terminal state matrices are given in the Extended Help section. @@ -279,7 +281,7 @@ given in the Extended Help section. [\begin{smallmatrix}\mathbf{0} & \mathbf{I} & \mathbf{0}\end{smallmatrix}]`` otherwise. """ function init_predmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, _ + model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd nΔU = nu*Hc From ae5140adad5c31d3c4c80ad3da246ce5d1354443 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 8 Oct 2026 08:35:19 -0400 Subject: [PATCH 04/28] wip: `NonLinModelDAE` for `NonLinMPC` --- src/controller/nonlinmpc.jl | 13 ++-- src/controller/transcription.jl | 119 +++++++++++++++-------------- src/estimator/mhe/transcription.jl | 22 +++--- 3 files changed, 80 insertions(+), 74 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 5b8a55cf2..ba3118248 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -741,11 +741,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end """ - init_optimization!(mpc::NonLinMPC, model::SimModelODE, optim::JuMP.GenericModel) -> nothing + init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) -> nothing Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!(mpc::NonLinMPC, model::SimModelODE, optim::JuMP.GenericModel) +function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) # --- variables and linear constraints --- con = mpc.con nZ̃ = length(mpc.Z̃) @@ -846,8 +846,8 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end Z̃_J = zeros(JNT, nZ̃) @@ -1117,7 +1117,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, Â0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -1126,7 +1126,8 @@ Update in-place all vectors for the predictions of `mpc` controller at decision The method mutates all the arguments before the `mpc` argument. """ function update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, Â0, gc, g, geq, + mpc::PredictiveController, Z̃ ) model, transcription = mpc.estim.model, mpc.transcription U0 = getU0!(U0, mpc, Z̃) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5e8ee0e1b..62cadab0b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -417,7 +417,7 @@ end @doc raw""" init_defectmat( - model::SimModelODE, estim::StateEstimator, transcription::TranscriptionMethod, + model::SimModel, estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS @@ -440,26 +440,30 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H the defect matrices are computed with: ```math \begin{aligned} - \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ + \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} - \mathbf{0} & \mathbf{-I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{bmatrix} \\ + \mathbf{0} & \mathbf{-I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_{S}^{â}} &= \mathbf{0} \\ + \mathbf{E_{S}^{ā}} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{â}} & \mathbf{E_{S}^{ā}} \end{bmatrix} \\ \mathbf{K_S} &= \begin{bmatrix} - \mathbf{0} & \mathbf{A_s} \\ - \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} \end{bmatrix} + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} \end{aligned} ``` """ function init_defectmat( - model::SimModelODE, estim::StateEstimator{NT}, ::TranscriptionMethod, + model::SimModel, estim::StateEstimator{NT}, ::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs + na, nā = get_na(model), get_nā(model, transcription) As = estim.As # --- current state estimates x̂0 --- KS = zeros(NT, nxs*Hp, nx̂) @@ -474,7 +478,9 @@ function init_defectmat( iCol = (nx+1:nx̂) .+ nx̂*(j-1) ESx̂[iRow, iCol] = As end - ES = [ESΔu ESx̂] + ESâ = zeros(NT, nxs*Hp, na*Hp) + ESā = zeros(NT, nxs*Hp, nā*Hp) + ES = [ESΔu ESx̂ ESâ ESā] # --- current measured disturbances d0 and predictions D̂0 --- GS = zeros(NT, nxs*Hp, nd) JS = zeros(NT, nxs*Hp, nd*Hp) @@ -485,7 +491,7 @@ end @doc raw""" init_defectmat( - model::SimModelODE, estim::StateEstimator, transcription::OrthogonalCollocation, + model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo ) -> ES, GS, JS, KS, VS, BS @@ -509,30 +515,33 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H constants of [`init_orthocolloc`](@ref), the defect matrices are: ```math \begin{aligned} - \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ + \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} - \mathbf{-I} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{-I} & \mathbf{0} &\mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - λ_o\mathbf{I} & \mathbf{0} &\mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{A_s} & \mathbf{0} &\mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ - \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{-I} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{-I} & \mathbf{0} &\mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + λ_o\mathbf{I} & \mathbf{0} &\mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} &\mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ \mathbf{E_{S}^{k̄}} &= \begin{bmatrix} - \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k̄}} \end{bmatrix} \\ + \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ + \mathbf{E_{S}^{â}} &= \mathbf{0} \\ + \mathbf{E_{S}^{ā}} &= \mathbf{0} \\ + \mathbf{E_S} &= \begin{bmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{â}} & \mathbf{E_{S}^{k̄}} & \mathbf{E_{S}^{ā}} \end{bmatrix} \\ \mathbf{K_S} &= \begin{bmatrix} - λ_o\mathbf{I} & \mathbf{0} \\ - \mathbf{0} & \mathbf{A_s} \\ - \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} \end{bmatrix} + λ_o\mathbf{I} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} \end{aligned} ``` Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented @@ -540,7 +549,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H are removed (2nd row of blocks, 4th row of blocks, and so on). """ function init_defectmat( - model::NonLinModel, estim::StateEstimator, transcription::OrthogonalCollocation, + model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo ) As, nxs = estim.As, estim.nxs @@ -548,7 +557,7 @@ function init_defectmat( end function init_defectmat( - model::NonLinModel, estim::InternalModel{NT}, transcription::OrthogonalCollocation, + model::SimModel, estim::InternalModel{NT}, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo ) where {NT<:Real} As, nxs = zeros(NT, 0, 0), 0 # the state vector is not augmented, no stochastic defects @@ -556,10 +565,11 @@ function init_defectmat( end function init_defectmat_orthocolloc( - model::NonLinModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, + model::SimModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, Hp, Hc, Co, λo, As, nxs ) where {NT<:Real} nu, nx, nd, nx̂ = model.nu, model.nx, model.nd, estim.nx̂ + na, nā = get_na(model), get_nā(model, transcription) nk̄ = get_nk̄(model, transcription) λo_I = λo*I(nx) # --- current state estimates x̂0 --- @@ -580,7 +590,9 @@ function init_defectmat_orthocolloc( ESx̂[iRow_As, iCol_As] = As end ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], Hp) - ES = [ESΔu ESx̂ ESk̄] + ESâ = zeros(NT, nx̂*Hp, na*Hp) + ESā = zeros(NT, nx̂*Hp, nā*Hp) + ES = [ESΔu ESx̂ ESâ ESk̄ ESā] # --- current measured disturbances d0 and predictions D̂0 --- GS = zeros(NT, nx̂*Hp, nd) JS = zeros(NT, nx̂*Hp, nd*Hp) @@ -591,14 +603,14 @@ end """ init_defectmat( - model::NonLinModel, estim::InternalModel, ::TranscriptionMethod, + model::SimModel, estim::InternalModel, ::TranscriptionMethod, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Return empty matrices for [`InternalModel`](@ref) (the state vector is not augmented). """ function init_defectmat( - ::NonLinModel, estim::InternalModel, transcription::TranscriptionMethod, + ::SimModel, estim::InternalModel, transcription::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) @@ -619,13 +631,6 @@ function init_defectmat( return init_defectmat_empty(estim, transcription, Hp, Hc) end -function init_defectmat( - ::NonLinModel, estim::InternalModel, transcription::SingleShooting, - Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing -) - return init_defectmat_empty(estim, transcription, Hp, Hc) -end - function init_defectmat_empty( estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc ) where {NT<:Real} @@ -643,7 +648,7 @@ end @doc raw""" init_matconstraint_mpc( - model::LinModel, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nϵ + model::LinModel, transcription::ShootingMethod, Z̃min, Z̃max, nc, nϵ U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, args... ) -> i_b, i_g, A, Aeq, neq @@ -668,7 +673,7 @@ case, `args` needs to contain all the inequality and equality constraint matric The integer `neq` is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( - model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, _ , + model::LinModel{NT}, transcription::ShootingMethod, Z̃min, Z̃max, nc, _ , U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, args... ) where {NT<:Real} @@ -731,9 +736,9 @@ function init_matconstraint_mpc( return i_b, i_g, A, Aeq, neq end -"Init `i_b, A` without output constraints if `NonLinModel` and other `TranscriptionMethod`." +"Init `i_b, A` without output constraints if not `LinModel` and other `TranscriptionMethod`." function init_matconstraint_mpc( - model::NonLinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nϵ, + model::SimModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nϵ, U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, args... ) where {NT<:Real} @@ -783,7 +788,7 @@ end boxconstraint_terminal!(Z̃min, Z̃max, ::SingleShooting, _, _ , _, _, _, _, _) = Z̃min, Z̃max "Unset `i_ΔUmin` and `i_ΔUmax` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max) +function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max) nΔU = length(i_ΔUmin) ΔUmin, ΔUmax = @views Z̃min[1:nΔU], @views Z̃max[1:nΔU] foreach(i -> !isinf(ΔUmin[i]) && (i_ΔUmin[i] = false), eachindex(ΔUmin)) @@ -792,14 +797,14 @@ function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModelODE, ::TranscriptionMet end "Unset `i_x̂min` and `i_x̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nΔU, nX̂) +function deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nΔU, nX̂) nx̂ = length(i_x̂min) x̂0min, x̂0max = @views Z̃min[nΔU+nX̂-nx̂+1:nΔU+nX̂], @views Z̃max[nΔU+nX̂-nx̂+1:nΔU+nX̂] foreach(i -> !isinf(x̂0min[i]) && (i_x̂min[i] = false), eachindex(x̂0min)) foreach(i -> !isinf(x̂0max[i]) && (i_x̂max[i] = false), eachindex(x̂0max)) return i_x̂min, i_x̂max end -deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModelODE, ::SingleShooting, _, _, _, _) = i_x̂min, i_x̂max +deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_x̂min, i_x̂max @doc raw""" linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) @@ -1131,8 +1136,8 @@ the terminal constraints applied on ``\mathbf{x̂_0}(k+H_p)``. The computations 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̂} + \mathbf{Ŷ_0} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ + \mathbf{x̂_0}(k+H_p) &= \mathbf{ẽ_x̂ Z̃} + \mathbf{f_x̂} \end{aligned} ``` """ diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 91966b4f1..0ebc81099 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -556,9 +556,9 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{I} \end{bmatrix} \\ - \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S^â} &= \mathbf{0} \\ \mathbf{E_S^ā} &= \mathbf{0} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^â} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \end{aligned} ``` """ @@ -578,9 +578,9 @@ function init_defectmat_mhe( ESx̂[iRow, iCol] = As end ESŵ = repeatdiag([zeros(NT, nxs, nŵd) I], He) - ESa = zeros(nxs*He, na + na*He) - ESā = zeros(nxs*He, nā*He) - ES = [ESx̂ ESa ESā ESŵ] + ESâ = zeros(NT, nxs*He, na + na*He) + ESā = zeros(NT, nxs*He, nā*He) + ES = [ESx̂ ESâ ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -627,9 +627,9 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{E_S^ŵ} &= \mathbf{I} \\ - \mathbf{E_S^a} &= \mathbf{0} \\ + \mathbf{E_S^â} &= \mathbf{0} \\ \mathbf{E_S^ā} &= \mathbf{0} \\ - \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^a} & \mathbf{E_S^k̄} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_S^x̂} & \mathbf{E_S^â} & \mathbf{E_S^k̄} & \mathbf{E_S^ā} & \mathbf{E_S^ŵ} \end{bmatrix} \\ \end{aligned} ``` """ @@ -653,9 +653,9 @@ function init_defectmat_mhe( end ESk̄ = repeatdiag([Co; zeros(NT, nxs, nk̄)], He) ESŵ = I # will be different if nŵ ≠ nx̂ is implemented - ESa = zeros(nx̂*He, na + na*He) - ESā = zeros(nx̂*He, nā*He) - ES = [ESx̂ ESa ESk̄ ESā ESŵ] + ESâ = zeros(NT, nx̂*He, na + na*He) + ESā = zeros(NT, nx̂*He, nā*He) + ES = [ESx̂ ESâ ESk̄ ESā ESŵ] GS = zeros(NT, nxs*He, model.nu*He) JS = zeros(NT, nxs*He, model.nd*(He+1)) BS = zeros(NT, nxs*He) @@ -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 From 5b39ddd064d21269e372421a0ad25f21f3f6fae2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 10:01:45 -0400 Subject: [PATCH 05/28] removed: useless argument in `con_nonlinprogeq_mhe!` --- src/estimator/mhe/execute.jl | 2 +- src/estimator/mhe/transcription.jl | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 7d5d43075..706a84cb8 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -930,7 +930,7 @@ function update_predictions!( gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) geq = con_nonlinprogeq_mhe!( - geq, X̂0, Â0, Û0, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ + geq, X̂0, Û0, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ca9054706..045bc000d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1689,7 +1689,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, _ , Û0, K̄, + geq, X̂0, Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) -> geq @@ -1707,7 +1707,7 @@ for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). """ function con_nonlinprogeq_mhe!( - geq, X̂0, _ , Û0, K̄, + geq, X̂0, Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) @@ -1742,7 +1742,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄dot, + 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̄dot, + geq, _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) @@ -1850,7 +1850,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , _ , Û0, K̄dot, + geq, _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) -> geq @@ -1888,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̄dot, + geq, _ , Û0, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) @@ -1969,4 +1969,4 @@ function con_nonlinprogeq_mhe!( end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file From 53979c019980c613eeb16d1fb244918f72a3b904 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 12:37:44 -0400 Subject: [PATCH 06/28] added: `reset_warmstart!` for MPC --- src/controller/execute.jl | 23 +-- src/controller/linmpc.jl | 1 + src/controller/nonlinmpc.jl | 5 +- src/controller/transcription.jl | 271 +++++++++++++++++++++-------- src/estimator/mhe/transcription.jl | 54 +++--- 5 files changed, 247 insertions(+), 107 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 561bb946f..fd08eacac 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -7,8 +7,8 @@ It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the inpu ``\mathbf{ΔU}`` to inputs ``\mathbf{U}``. """ function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) - mpc.Z̃ .= 0 mpc.lastu0 .= u .- mpc.estim.model.uop + reset_warmstart!(mpc, mpc.transcription) return initstate!(mpc.estim, u, ym, d) end @@ -144,14 +144,15 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) """ function getinfo(mpc::PredictiveController{NT}) where NT<:Real model, buffer, transcription = mpc.estim.model, mpc.buffer, mpc.transcription - nΔŨ, nK = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nk̄ + nk̄ = get_nk̄(model, transcription) + nΔŨ, nK̄ = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*nk̄ nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp Z̃ = mpc.Z̃ info = Dict{Symbol, Any}() ΔŨ = Vector{NT}(undef, nΔŨ) x̂0end = similar(mpc.estim.x̂0) - K = Vector{NT}(undef, nK) + K = Vector{NT}(undef, nK̄) Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe) U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop) Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0) @@ -277,25 +278,25 @@ function initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂ end @doc raw""" - initpred!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) -> nothing + initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ -function initpred!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) return nothing end """ - initpred_common!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) -> F + initpred_common!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> F -Common computations of `initpred!` for all types of [`SimModelODE`](@ref). +Common computations of `initpred!` for all types of [`SimModel`](@ref). Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` is an [`InternalModel`](@ref). The function returns `mpc.F`. """ function initpred_common!( - mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u + mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u ) mpc.lastu0 .= lastu .- model.uop mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) @@ -327,14 +328,14 @@ end predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) @doc raw""" - linconstraint_custom!(mpc::PredictiveController, model::SimModelODE) + linconstraint_custom!(mpc::PredictiveController, model::SimModel) Init the ``\mathbf{F_w}`` vector for the custom linear inequality constraints. See [`relaxW`](@ref) for the definition of the vector. The function does nothing if `mpc.con.nw < 1`. """ -function linconstraint_custom!(mpc::PredictiveController, model::SimModelODE) +function linconstraint_custom!(mpc::PredictiveController, model::SimModel) mpc.con.nw < 1 && return nothing ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer Fw = mpc.con.Fw @@ -509,7 +510,7 @@ end set_force∇!(::PredictiveController) = nothing "By default, no need to update the objective function." -set_objective_linear_coef!(::PredictiveController, ::SimModelODE, _) = nothing +set_objective_linear_coef!(::PredictiveController, ::SimModel, _) = nothing "Update the linear coefficients of the quadratic objective with `mpc.q̃` for `LinModel`." function set_objective_linear_coef!(mpc::PredictiveController, ::LinModel, Z̃var) diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 135041760..9bd3603f7 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -104,6 +104,7 @@ struct LinMPC{ Uop, Yop, Dop, buffer ) + reset_warmstart!(mpc, transcription) init_optimization!(mpc, model, optim) return mpc end diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index ba3118248..0750d6a35 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -147,6 +147,7 @@ struct NonLinMPC{ Uop, Yop, Dop, buffer ) + reset_warmstart!(mpc, transcription) init_optimization!(mpc, model, optim) return mpc end @@ -1117,7 +1118,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, Â0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -1126,7 +1127,7 @@ Update in-place all vectors for the predictions of `mpc` controller at decision The method mutates all the arguments before the `mpc` argument. """ function update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, Â0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) model, transcription = mpc.estim.model, mpc.transcription diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 62cadab0b..769d442dd 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -459,7 +459,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H ``` """ function init_defectmat( - model::SimModel, estim::StateEstimator{NT}, ::TranscriptionMethod, + model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs @@ -807,7 +807,7 @@ end deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_x̂min, i_x̂max @doc raw""" - linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) + linconstraint!(mpc::PredictiveController, model::LinModel, ::ShootingMethod) Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MPC. @@ -816,7 +816,7 @@ Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mat [`init_predmat`](@ref). The ``\mathbf{F_w}`` vector for the custom linear constraints is also updated, see [`relaxW`](@ref). """ -function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) +function linconstraint!(mpc::PredictiveController, model::LinModel, ::ShootingMethod) nU, nΔU, nY = length(mpc.con.U0min), length(mpc.con.ΔUmin), length(mpc.con.Y0min) nW = length(mpc.con.Wmin) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ @@ -856,7 +856,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." -function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod) +function linconstraint!(mpc::PredictiveController, model::SimModel, ::TranscriptionMethod) nU, nΔU = length(mpc.con.U0min), length(mpc.con.ΔUmin) nW = length(mpc.con.Wmin) nx̂ = mpc.estim.nx̂ @@ -937,7 +937,7 @@ end """ linconstrainteq!( - mpc::PredictiveController, ::SimModelODE, ::StateEstimator, ::TranscriptionMethod + mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod ) By default, fallback to doing same the but using the shorter equations. @@ -947,7 +947,7 @@ constraints of [`OrthogonalCollocation`](@ref), if applicable. See [`init_defect for the equation. """ function linconstrainteq!( - mpc::PredictiveController, ::SimModelODE, ::StateEstimator, ::TranscriptionMethod + mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod ) FS = mpc.con.FS mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS @@ -959,7 +959,7 @@ end """ linconstrainteq!( - mpc::PredictiveController, ::NonLinModel, ::InternalModel, ::OrthogonalCollocation + mpc::PredictiveController, ::SimModel, ::InternalModel, ::OrthogonalCollocation ) Same as above but only for the continuity constraints of [`OrthogonalCollocation`](@ref). @@ -967,7 +967,7 @@ Same as above but only for the continuity constraints of [`OrthogonalCollocation There are no stochastic defects, the [`InternalModel`](@ref) does not augment the states. """ function linconstrainteq!( - mpc::PredictiveController, ::NonLinModel, ::InternalModel, ::OrthogonalCollocation + mpc::PredictiveController, ::SimModel, ::InternalModel, ::OrthogonalCollocation ) FS = mpc.con.FS mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS @@ -977,10 +977,66 @@ function linconstrainteq!( return nothing end "No linear equality constraints for other cases of [`InternalModel`](@ref)." -linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::TranscriptionMethod) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::TranscriptionMethod) = nothing "No linear equality constraints for all cases of [`SingleShooting`](@ref) (N/A)." -linconstrainteq!(::PredictiveController, ::SimModelODE, ::StateEstimator, ::SingleShooting) = nothing -linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::SingleShooting) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::SingleShooting) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::SingleShooting) = nothing + +""" + reset_warmstart!(mpc::PredictiveController, ::TranscriptionMethod) + +Reset warm-starting values `mpc.Z̃` at values stored in `mpc.estim.model` +""" +reset_warmstart!(mpc::PredictiveController, ::SingleShooting) = (mpc.Z̃ .= 0; nothing) +function reset_warmstart!(mpc::PredictiveController, ::MultipleShooting) + model = mpc.estim.model + Hp, Hc, nx, nu = mpc.Hp, mpc.Hc, model.nx, model.nu + nΔU = nu*Hc + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + mpc.Z̃ .= 0 + for j in 1:Hp + mpc.Z̃[(nΔU + nx*(j-1) + 1):(nΔU + nx*j)] .= x0s + end + return nothing +end +function reset_warmstart!(mpc::PredictiveController, ::TrapezoidalCollocation) + model = mpc.estim.model + Hp, Hc, nx̂, nx, nu, na = mpc.Hp, mpc.Hc, mpc.estim.nx̂, model.nx, model.nu, get_na(model) + nΔU, nX̂, n = nu*Hc, nx̂*Hp, na*Hp + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + mpc.Z̃ .= 0 + for j in 1:Hp + mpc.Z̃[(nΔU + nx*(j-1) + 1):(nΔU + nx*j)] .= x0s + mpc.Z̃[(nΔU + nX̂ + na*(j-1) + 1):(nΔU + nX̂ + na*j)] .= a0s + mpc.Z̃[(nΔU + nX̂ + n + na*(j-1) + 1):(nΔU + nX̂ + n + na*j)] .= a0s + end + return nothing +end +function reset_warmstart!(mpc::PredictiveController, transcription::OrthogonalCollocation) + model = mpc.estim.model + Hp, Hc, nx̂, nx, nu, na = mpc.Hp, mpc.Hc, mpc.estim.nx̂, model.nx, model.nu, get_na(model) + nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) + no = transcription.no + nΔU, nX̂, nÂ, nK̄, nĀ = nu*Hc, nx̂*Hp, na*Hp, nk̄*Hp, nā*Hp + as_0 = get_as_0(model) + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + a0s = as_0 + mpc.Z̃ .= 0 + for j in 1:Hp + mpc.Z̃[(nΔU + nx*(j-1) + 1):(nΔU + nx*j)] .= x0s + mpc.Z̃[(nΔU + nX̂ + na*(j-1) + 1):(nΔU + nX̂ + na*j)] .= a0s + end + K̄ = @views mpc.Z̃[(nΔU + nX̂ + n + 1):(nΔU + nX̂ + n + nK̄)] + repeat!(K̄, x0s, no*Hp) + Ā = @views mpc.Z̃[(nΔU + nX̂ + n + nK̄ + 1):(nΔU + nX̂ + n + nK̄ + nĀ)] + repeat!(Ā, a0s, no*Hp) + return nothing +end @doc raw""" set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s @@ -1034,36 +1090,60 @@ It warm-starts the solver at: \mathbf{x̂_0}(k+H_p-2|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+1|k-1) \\ + \mathbf{â_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{â_0}(k+H_p-2|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ \mathbf{k̄}(k+0|k-1) \\ \mathbf{k̄}(k+1|k-1) \\ \vdots \\ \mathbf{k̄}(k+H_p-3|k-1) \\ \mathbf{k̄}(k+H_p-2|k-1) \\ \mathbf{k̄}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+0|k-1) \\ + \mathbf{ā}(k+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+H_p-3|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ ϵ_{k-1} \end{bmatrix} ``` where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` include the ``n_o`` intermediate -stage predictions for the interval ``k+j``, and is also computed at the last control period. +``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` and ``\mathbf{ā}(k-j|k-1) include +the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at +the last control period. """ function set_warmstart_mpc!( mpc::PredictiveController, transcription::OrthogonalCollocation, Z̃var ) nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ Hp, Hc, Z̃s = mpc.Hp, mpc.Hc, mpc.buffer.Z̃ - nk̄ = get_nk̄(mpc.estim.model, transcription) - nΔU, nX̂, nK = nu*Hc, nx̂*Hp, nk̄*Hp + na = get_na(mpc.estim.model) + nk̄, nā = get_nk̄(mpc.estim.model, transcription), get_nā(mpc.estim.model, transcription) + nΔU, nX̂, nÂ, nK̄, nĀ = nu*Hc, nx̂*Hp, na*Hp, nk̄*Hp, nā*Hp # --- input increments ΔU --- Z̃s[1:(nΔU-nu)] .= @views mpc.Z̃[(nu+1):(nΔU)] Z̃s[(nΔU-nu+1):(nΔU)] .= 0 # --- predicted states X̂0 --- - Z̃s[(nΔU+1):(nΔU+nX̂-nx̂)] .= @views mpc.Z̃[(nΔU+nx̂+1):(nΔU+nX̂)] - Z̃s[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] .= @views mpc.Z̃[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] - # --- collocation points K --- - Z̃s[(nΔU+nX̂+1):(nΔU+nX̂+nK-nk̄)] .= @views mpc.Z̃[(nΔU+nX̂+nk̄+1):(nΔU+nX̂+nK)] - Z̃s[(nΔU+nX̂+nK-nk̄+1):(nΔU+nX̂+nK)] .= @views mpc.Z̃[(nΔU+nX̂+nK-nk̄+1):(nΔU+nX̂+nK)] + i_base = nΔU + Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views mpc.Z̃[(i_base+nx̂+1):(i_base+nX̂)] + Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views mpc.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] + # --- algebraic variables Â0 --- + i_base = nΔU + nX̂ + Z̃s[(i_base+1):(i_base+nÂ-na)] .= @views mpc.Z̃[(i_base+na+1):(i_base+nÂ)] + Z̃s[(i_base+nÂ-na+1):(i_base+nÂ)] .= @views mpc.Z̃[(i_base+nÂ-na+1):(i_base+nÂ)] + # --- deterministic states at collocation points K̄ --- + i_base = nΔU + nX̂ + n + Z̃s[(i_base+1):(i_base+nK̄-nk̄)] .= @views mpc.Z̃[(i_base+nk̄+1):(i_base+nK̄)] + Z̃s[(i_base+nK̄-nk̄+1):(i_base+nK̄)] .= @views mpc.Z̃[(i_base+nK̄-nk̄+1):(i_base+nK̄)] + # --- algebraic variables at collocation points Ā --- + i_base = nΔU + nX̂ + n + nK̄ + Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views mpc.Z̃[(i_base+nā+1):(i_base+nĀ)] + Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views mpc.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1090,19 +1170,44 @@ It warm-starts the solver at: \mathbf{x̂_0}(k+H_p-2|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+1|k-1) \\ + \mathbf{â_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{â_0}(k+H_p-2|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{ā}(k+0|k-1) \\ + \mathbf{ā}(k+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+H_p-3|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ ϵ_{k-1} \end{bmatrix} ``` """ -function set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) - nu, nx̂, Hp, Hc, Z̃s = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ - nΔU, nX̂ = nu*Hc, nx̂*Hp +function set_warmstart_mpc!( + mpc::PredictiveController, transcription::TranscriptionMethod, Z̃var +) + nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ + Hp, Hc, Z̃s = mpc.Hp, mpc.Hc, mpc.buffer.Z̃ + na, nā = get_na(mpc.estim.model), get_nā(mpc.estim.model, transcription) + nΔU, nX̂, nÂ, nĀ = nu*Hc, nx̂*Hp, na*Hp, nā*Hp # --- input increments ΔU --- Z̃s[1:(nΔU-nu)] .= @views mpc.Z̃[nu+1:nΔU] Z̃s[(nΔU-nu+1):(nΔU)] .= 0 # --- predicted states X̂0 --- - Z̃s[(nΔU+1):(nΔU+nX̂-nx̂)] .= @views mpc.Z̃[(nΔU+nx̂+1):(nΔU+nX̂)] - Z̃s[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] .= @views mpc.Z̃[(nΔU+nX̂-nx̂+1):(nΔU+nX̂)] + i_base = nΔU + Z̃s[(i_base+1):(i_base+nX̂-nx̂)] .= @views mpc.Z̃[(i_base+nx̂+1):(i_base+nX̂)] + Z̃s[(i_base+nX̂-nx̂+1):(i_base+nX̂)] .= @views mpc.Z̃[(i_base+nX̂-nx̂+1):(i_base+nX̂)] + # --- algebraic variables Â0 --- + i_base = nΔU + nX̂ + Z̃s[(i_base+1):(i_base+nÂ-na)] .= @views mpc.Z̃[(i_base+na+1):(i_base+nÂ)] + Z̃s[(i_base+nÂ-na+1):(i_base+nÂ)] .= @views mpc.Z̃[(i_base+nÂ-na+1):(i_base+nÂ)] + # --- algebraic variables at collocation points Ā --- + i_base = nΔU + nX̂ + n + Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views mpc.Z̃[(i_base+nā+1):(i_base+nĀ)] + Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views mpc.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1199,11 +1304,11 @@ end @doc raw""" predict!( Ŷ0, x̂0end, _ , _ , _ , - mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod, + mpc::PredictiveController, model::SimModel, transcription::TranscriptionMethod, _ , Z̃ ) -> Ŷ0, x̂0end -Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). +Compute the vectors for all other cases. The method mutates `Ŷ0` and `x̂0end` arguments. The augmented output function [`ĥ!`](@ref) is called multiple times in a `for` loop: @@ -1215,18 +1320,21 @@ from the decision variable `Z̃`. """ function predict!( Ŷ0, x̂0end, _, _, _, - mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod, + mpc::PredictiveController, model::SimModel, transcription::TranscriptionMethod, _ , Z̃ ) - nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc + nu, ny, nd, nx̂, na = model.nu, model.ny, model.nd, mpc.estim.nx̂, get_na(model) + Hp, Hc = mpc.Hp, mpc.Hc + nΔU, nX̂, n = nu*Hc, nx̂*Hp, na*Hp h_threads = transcription.h_threads - X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ] + X̂0, Â0 = @views Z̃[(1 + nΔU):(nΔU + nX̂)], Z̃[(1 + nΔU + nX̂):(nΔU + nX̂ + nÂ)] D̂0 = mpc.D̂0 @threadsif h_threads for j=1:Hp x̂0 = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] + â0 = @views Â0[(1+na*(j-1)):(na*j)] d̂0 = @views D̂0[(1+nd*(j-1)):(nd*j)] ŷ0 = @views Ŷ0[(1+ny*(j-1)):(ny*j)] - ĥ!(ŷ0, mpc.estim, model, x̂0, d̂0) + ĥ!(ŷ0, mpc.estim, model, x̂0, â0, d̂0) end Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0. x̂0end .= @views X̂0[(1+nx̂*(Hp-1)):(nx̂*Hp)] @@ -1235,16 +1343,16 @@ end """ con_nonlinprog!( - g, mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, _ , Ŷ0, gc, ϵ + g, mpc::PredictiveController, ::SimModel, ::TranscriptionMethod, _ , Ŷ0, gc, ϵ ) -> g -Nonlinear constrains when `model` is a [`NonLinModel`](@ref) with non-[`SingleShooting`](@ref). +Nonlinear constrains when `model` is any [`SimModel`](@ref) with non-[`SingleShooting`](@ref). The method mutates the `g` vectors in argument and returns it. The output prediction and the custom constraints are included in the `g` vector. """ function con_nonlinprog!( - g, mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, _ , Ŷ0, gc, ϵ + g, mpc::PredictiveController, ::SimModel, ::TranscriptionMethod, _ , Ŷ0, gc, ϵ ) nŶ = length(Ŷ0) for i in eachindex(g) @@ -1301,7 +1409,7 @@ end """ con_nonlinprog!( - g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _ + g, ::PredictiveController, ::LinModel, ::ShootingMethod, _ , _ , gc, _ ) -> g Compute the same but for [`LinModel`](@ref). @@ -1310,7 +1418,7 @@ The nonlinear custom inequality constraints in `gc` are the only nonlinear const for this case. """ function con_nonlinprog!( - g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _ + g, ::PredictiveController, ::LinModel, ::ShootingMethod, _ , _ , gc, _ ) for i in eachindex(g) g[i] = gc[i] @@ -1331,7 +1439,7 @@ The method mutates the `geq`, `X̂0`, `Û0` and `K̄` vectors in argument. The stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). The defects of the deterministic states are computed with: ```math -\mathbf{ŝ_d}(k+j+1) = \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j)\Big) +\mathbf{ŝ_d}(k+j+1) = \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) - \mathbf{x̂_d}(k+j+1) ``` for ``j = 0, 1, ... , H_p-1`` and in which the deterministic state ``\mathbf{x̂_d}`` are the @@ -1373,33 +1481,40 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̄ - mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, + geq, _ , Û0, K̄dot, + mpc::PredictiveController, model::SimModel, transcription::TrapezoidalCollocation, U0, Z̃ ) -> geq -Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). +Nonlinear equality constrains for [`SimModel`](@ref) and [`TrapezoidalCollocation`](@ref). The deterministic state defects are computed with: ```math -\mathbf{ŝ_d}(k+j+1) = \mathbf{x̂_d}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] +\mathbf{ŝ_k}(k+j) = \mathbf{x̂_d}(k+j) + 0.5 T_s [\mathbf{k̇^+}(k+j) + \mathbf{k̇^-}(k+j+1)] - \mathbf{x̂_d}(k+j+1) ``` for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x̂_d}`` are the deterministic states extracted from the decision variable `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated from -the continuous-time function `model.f!` and: +the continuous-time function [`fq_dae!`](@ref) and: ```math \begin{aligned} -\mathbf{k̇}_1(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ -\mathbf{k̇}_2(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d̂_0}(k+j+1), \mathbf{p}\Big) +\mathbf{k̇^+}(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{a^+}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ +\mathbf{k̇^-}(k+j+1) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j+1), \mathbf{â_0}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d̂_0}(k+j+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂!`](@ref) documentation. +is defined in [`f̂!`](@ref) documentation. The residuals for [`NonLinModelDAE`](@ref) are +also computed from ``j = 0, 1, ... , H_p-1`` and: +```math +\begin{aligned} +\mathbf{q^+}(k+j) &= \mathbf{q}\Big(\mathbf{x̂_d}(k+j), \mathbf{a^+}(k+j), \mathbf{û_0}(k+j), \mathbf{d_0}(k+j), \mathbf{p}\Big) \\ +\mathbf{q^-}(k+j+1) &= \mathbf{q}\Big(\mathbf{x̂_d}(k+j+1), \mathbf{â_0}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d_0}(k+j+1), \mathbf{p}\Big) +\end{aligned} +``` """ function con_nonlinprogeq!( - geq, _ , Û0, K̄, - mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, + geq, _ , Û0, K̄dot, + mpc::PredictiveController, model::SimModel, transcription::TrapezoidalCollocation, U0, Z̃ ) nu, nx̂, nd, nx, h = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.h @@ -1407,39 +1522,51 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads Ts = model.Ts + na = get_na(model) nk̄ = get_nk̄(model, transcription) + nŜk, nÂ, nĀ = nx*Hp, na*Hp, na*Hp D̂0 = mpc.D̂0 - X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) + X̂0 = @views Z̃[(nΔU+1):(nΔU+nX̂)] + Â0 = @views Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nÂ)] + A⁺ = @views Z̃[(nΔU+nX̂+nÂ+1):(nΔU+nX̂+nÂ+nĀ)] + Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0, U0) + Ŝk = @views geq[1:nŜk] + Q⁻ = @views geq[(nŜk+1):(nŜk+nÂ)] + Q⁺ = @views geq[(nŜk+nÂ+1):(nŜk+nÂ+nĀ)] @threadsif f_threads for j=1:Hp if j < 2 - x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] + x̂d = @views mpc.estim.x̂0[1:nx] d̂0 = @views mpc.d0[1:nd] else - x̂d_Z̃ = @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)] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ŝdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] - k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2*nx] - if f_threads || h < 1 || j < 2 - # we need to recompute k1 with multi-threading, even with h==1, since the - # last iteration (j-1) may not be executed (iterations are re-orderable) - model.f!(k̇1, x̂d_Z̃, û0, d̂0, model.p) - else - k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 + û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)] + ŝk = @views Ŝk[(1 + nx*(j-1)):(nx*j)] + d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*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:2*nx] + if h > 0 && j < Hp + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + else # j = Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc≤Hp implies Δu(k+Hp) = 0: + û0next = û0 end - if h < 1 - model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, model.p) + # 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, d̂0) else - # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: - û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, model.p) + 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 - ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) + fq_dae!(k̇⁻next, q⁻next, model, x̂dnext, â0next, û0next, d̂0next) + ŝk .= @. x̂d - x̂dnext + 0.5*Ts*(k̇⁺ + k̇⁻next) end return geq end @@ -1447,7 +1574,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̄, + geq, _ , Û0, K̄dot, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) -> geq @@ -1476,7 +1603,7 @@ described in [`init_orthocolloc`](@ref). The defects for the continuity constrai stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). """ function con_nonlinprogeq!( - geq, _ , Û0, K̄, + geq, _ , Û0, K̄dot, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) @@ -1499,7 +1626,7 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄dot[(1 + nk̄*(j-1)):(nk̄*j)] k̄_Z̃ = @views K̄_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 045bc000d..dbc9e7ade 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1054,6 +1054,15 @@ linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothi Reset warm-starting values `estim.Z̃` at values stored in `estim.model` """ +function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) + model = estim.model + nx, nε = model.nx, estim.nε + x0s = model.buffer.x + x0s .= model.xs_0 .- model.xop + estim.Z̃ .= 0 + estim.Z̃[nε+1:nε+nx] = x0s + return nothing +end function reset_warmstart!(estim::MovingHorizonEstimator, ::MultipleShooting) model = estim.model nx, nx̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He @@ -1106,15 +1115,6 @@ function reset_warmstart!(estim::MovingHorizonEstimator, transcription::Orthogon repeat!(Ā, a0s, no*He) return nothing end -function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) - model = estim.model - nx, nε = model.nx, estim.nε - x0s = model.buffer.x - x0s .= model.xs_0 .- model.xop - estim.Z̃ .= 0 - estim.Z̃[nε+1:nε+nx] = x0s - return nothing -end @doc raw""" set_warmstart_mhe!( @@ -1339,6 +1339,13 @@ The warm-starting value is provided in Extended Help. \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{0_â} \\ + \mathbf{ā}(k-N_k+p+0|k-1) \\ + \mathbf{ā}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+p-3|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{ā}(k+p-2|k-1) \\ + \mathbf{0_ā} \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ \vdots \\ @@ -1371,7 +1378,7 @@ function set_warmstart_mhe!( i_base = nx̃ + nX̂ Z̃s[(i_base+1):(i_base+nÂ)] .= @views estim.Z̃[(i_base+na+1):(i_base+na+nÂ)] Z̃s[(i_base+nÂ+1):(i_base+na+nÂ)] .= @views estim.Z̃[(i_base+nÂ+1):(i_base+na+nÂ)] - # --- algebraic variables Ā --- + # --- algebraic variables at collocation points Ā --- i_base = nx̃ + nX̂ + na + n Z̃s[(i_base+1):(i_base+nĀ-nā)] .= @views estim.Z̃[(i_base+nā+1):(i_base+nĀ)] Z̃s[(i_base+nĀ-nā+1):(i_base+nĀ)] .= @views estim.Z̃[(i_base+nĀ-nā+1):(i_base+nĀ)] @@ -1554,8 +1561,8 @@ end Compute the vectors for all other cases. -The function mutates `V̂`, `X̂0`, and `Ŷ0` vector arguments. The augmented output function -[`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. +The function mutates `V̂`, `X̂0`, `Â0`, and `Ŷ0` vector arguments. The augmented output +function [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( V̂, X̂0, Â0, _ , _ , Ŷ0, @@ -1696,15 +1703,19 @@ 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 integer ``ℓ = k - N_k + p`` to shorten the notation, the defects of the estimated states -are computed with: +The method mutates the `geq`, `X̂0`, `Û0` and `K̄` vectors in argument. The defects of the +stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). By +introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the defects of the +estimated states are computed with: ```math -\mathbf{ŝ}(ℓ+j+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(ℓ+j), \mathbf{u_0}(ℓ+j), \mathbf{d_0}(ℓ+j)\Big) - + \mathbf{ŵ}(ℓ+j) - \mathbf{x̂_0}(ℓ+j+1) +\mathbf{ŝ_d}(ℓ+j+1) = \mathbf{f}\Big(\mathbf{x̂_d}(ℓ+j), \mathbf{u_0}(ℓ+j), \mathbf{d_0}(ℓ+j), \mathbf{p}\Big) + + \mathbf{ŵ_d}(ℓ+j) - \mathbf{x̂_d}(ℓ+j+1) ``` -for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf{x̂_0}`` are -extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). +for ``j = 0, 1, ... , N_k-1`` and in which the deterministic state vectors ``\mathbf{x̂_d}`` +are first `model.nx` elements of the augmented states ``\mathbf{x̂_0}``, and they are +extracted from the decision variable `Z̃`. The function ``\mathbf{f}`` is the deterministic +state update function [`f!`](@ref). The disturbed input ``\mathbf{û_0}`` is defined in +[`f̂!`](@ref) documentation. """ function con_nonlinprogeq_mhe!( geq, X̂0, Û0, K̄, @@ -1716,7 +1727,7 @@ function con_nonlinprogeq_mhe!( Nk = estim.Nk[] f_threads = transcription.f_threads nw = nŵ - nxs - p = estim.direct ? 0 : 1 + i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) @threadsif f_threads for j=1:Nk @@ -1725,7 +1736,7 @@ function con_nonlinprogeq_mhe!( else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end - d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] @@ -1784,7 +1795,6 @@ function con_nonlinprogeq_mhe!( 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[] From 999185ae597de52ce6237d2c721502d015d22ab2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 12:40:49 -0400 Subject: [PATCH 07/28] changed: combined warm-starting into single generic function It is now documented in the "Internals -> Misc" section --- docs/src/internals/misc.md | 7 +++++++ docs/src/internals/predictive_control.md | 1 - docs/src/internals/state_estim.md | 1 - src/controller/execute.jl | 4 ++-- src/controller/transcription.jl | 12 ++++++------ src/estimator/mhe/execute.jl | 4 ++-- src/estimator/mhe/transcription.jl | 12 ++++++------ 7 files changed, 23 insertions(+), 18 deletions(-) diff --git a/docs/src/internals/misc.md b/docs/src/internals/misc.md index f82944fa4..1da847fc1 100644 --- a/docs/src/internals/misc.md +++ b/docs/src/internals/misc.md @@ -24,3 +24,10 @@ ModelPredictiveControl.initpred! ModelPredictiveControl.linconstraint! ModelPredictiveControl.linconstrainteq! ``` + +## Warm-starting + +```@docs +ModelPredictiveControl.reset_warmstart! +ModelPredictiveControl.set_warmstart! +``` diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 7f843c2ff..1c241c985 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -34,7 +34,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveContro ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) -ModelPredictiveControl.set_warmstart_mpc! ModelPredictiveControl.predict! ModelPredictiveControl.con_nonlinprog! ModelPredictiveControl.con_nonlinprogeq! diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 359299bfe..549cde9a4 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -105,7 +105,6 @@ ModelPredictiveControl.update_estimate! ```@docs ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) -ModelPredictiveControl.set_warmstart_mhe! ModelPredictiveControl.disturbedinput! ModelPredictiveControl.predict_mhe! ModelPredictiveControl.con_nonlinprog_mhe! diff --git a/src/controller/execute.jl b/src/controller/execute.jl index fd08eacac..6f4be31ac 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -459,7 +459,7 @@ end Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `Z̃`. -If first warm-starts the solver with [`set_warmstart_mpc!`](@ref). It then calls +If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls `JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). @@ -467,7 +467,7 @@ If first warm-starts the solver with [`set_warmstart_mpc!`](@ref). It then calls function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} model, optim = mpc.estim.model, mpc.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - Z̃s = set_warmstart_mpc!(mpc, mpc.transcription, Z̃var) + Z̃s = set_warmstart!(mpc, mpc.transcription, Z̃var) set_force∇!(mpc) set_objective_linear_coef!(mpc, model, Z̃var) try diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 769d442dd..933012ca2 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1039,7 +1039,7 @@ function reset_warmstart!(mpc::PredictiveController, transcription::OrthogonalCo end @doc raw""" - set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s + set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) transcription. @@ -1058,7 +1058,7 @@ If supported by `mpc.optim`, it warm-starts the solver at: where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the last control period ``k-1``, and ``ϵ_{k-1}``, the slack variable of the last control period. """ -function set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) +function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) nu, Hc, Z̃s = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ nΔU = nu*Hc # --- input increments ΔU --- @@ -1071,7 +1071,7 @@ function set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) end @doc raw""" - set_warmstart_mpc!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s + set_warmstart!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s Do the same but for [`OrthogonalCollocation`](@ref). @@ -1117,7 +1117,7 @@ last control period ``k-1``, expressed as a deviation from the operating point the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at the last control period. """ -function set_warmstart_mpc!( +function set_warmstart!( mpc::PredictiveController, transcription::OrthogonalCollocation, Z̃var ) nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ @@ -1151,7 +1151,7 @@ function set_warmstart_mpc!( end @doc raw""" - set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s + set_warmstart!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s Do the same but for other [`TranscriptionMethod`](@ref). @@ -1186,7 +1186,7 @@ It warm-starts the solver at: \end{bmatrix} ``` """ -function set_warmstart_mpc!( +function set_warmstart!( mpc::PredictiveController, transcription::TranscriptionMethod, Z̃var ) nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 706a84cb8..c8e013802 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -598,7 +598,7 @@ end Optimize objective of `estim` [`MovingHorizonEstimator`](@ref) and return the solution `Z̃`. -If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). It then calls +If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls `JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). @@ -606,7 +606,7 @@ If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). It then calls function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real optim = estim.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - Z̃s = set_warmstart_mhe!(estim, estim.transcription, Z̃var) + Z̃s = set_warmstart!(estim, estim.transcription, Z̃var) set_force∇!(estim) # ------- solve optimization problem -------------- try diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index dbc9e7ade..ff85c6a6d 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1117,7 +1117,7 @@ function reset_warmstart!(estim::MovingHorizonEstimator, transcription::Orthogon end @doc raw""" - set_warmstart_mhe!( + set_warmstart!( estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var ) -> Z̃s @@ -1150,7 +1150,7 @@ warm-starts the solver at the value provided in Extended Help. [`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates all the arguments. """ -function set_warmstart_mhe!( +function set_warmstart!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer @@ -1189,7 +1189,7 @@ function set_warmstart_mhe!( end @doc raw""" - set_warmstart_mhe!( + set_warmstart!( estim::MovingHorizonEstimator, transcription::OrthogonalCollocation, Z̃var ) -> Z̃s @@ -1251,7 +1251,7 @@ The warm-starting value is provided in Extended Help. and [`OrthogonalCollocation`](@ref) for the defintion of vectors ``\mathbf{0_x̂}``, ``\mathbf{0_â}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. """ -function set_warmstart_mhe!( +function set_warmstart!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer @@ -1308,7 +1308,7 @@ function set_warmstart_mhe!( end @doc raw""" - set_warmstart_mhe!( + set_warmstart!( estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃var ) -> Z̃s @@ -1356,7 +1356,7 @@ The warm-starting value is provided in Extended Help. \end{bmatrix} ``` """ -function set_warmstart_mhe!( +function set_warmstart!( estim::MovingHorizonEstimator{NT}, transcription::TranscriptionMethod, Z̃var ) where NT<:Real model, buffer = estim.model, estim.buffer From c817d8056a1c91a8a5e5260294b3dd713efb25b3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 13:22:57 -0400 Subject: [PATCH 08/28] =?UTF-8?q?added:=20`NonLinMPC`=20support=20`NonLinM?= =?UTF-8?q?odelDAE`=20=F0=9F=A5=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 93 +++++++++++++++++++++------------ 1 file changed, 59 insertions(+), 34 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 933012ca2..354434316 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1575,11 +1575,11 @@ end @doc raw""" con_nonlinprogeq!( geq, _ , Û0, K̄dot, - mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, + mpc::PredictiveController, model::SimModel, transcription::OrthogonalCollocation, U0, Z̃ ) -> geq -Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). +Nonlinear equality constrains for [`SimModel`](@ref) and [`OrthogonalCollocation`](@ref). The defects between the deterministic state derivative at the ``n_o`` collocation points and the model dynamics are computed by: @@ -1598,62 +1598,87 @@ the model dynamics are computed by: ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` and ``\mathbf{x̂_d}(k+j)`` vectors are extracted from the decision variables in `Z̃`. The -``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as -described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the -stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). +``\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 left limit of the sampling instants ``\mathbf{q^-}(ℓ+j)`` are other +constraints: +```math +\mathbf{q^-}(k+j) = \mathbf{q}\Big(\mathbf{x̂_d}(k+j), \mathbf{â_0}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) +``` +for ``j = 0, 1, ... , H_p-1``. The defects for the continuity constraints and the stochastic +states are linear equality constraints (see [`init_defectmat`](@ref)). """ function con_nonlinprogeq!( geq, _ , Û0, K̄dot, - mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, + mpc::PredictiveController, model::SimModel, transcription::OrthogonalCollocation, U0, Z̃ ) nu, nx̂, nd, nx, h = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.h Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads - Mo, no, τ = mpc.Mo, transcription.no, transcription.τ - nk̄ = get_nk̄(model, transcription) + no, τ, τendIsNotOne = transcription.no, transcription.τ, transcription.τendIsNotOne + Mo = mpc.Mo + na = get_na(model) + nā, nk̄ = na*no, get_nk̄(model, transcription) + nŜk̄, nÂ, nK̄, nĀ = nk̄*Hp, na*Hp, nk̄*Hp, nā*Hp D̂0 = mpc.D̂0 - X̂0_Z̃, K̄_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk̄*Hp)] - D̂temp = mpc.buffer.D̂ - Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) + X̂0 = @views Z̃[(1 + nΔU):(nΔU + nX̂)] + Â0 = @views Z̃[(1 + nΔU + nX̂):(nΔU + nX̂ + nÂ)] + K̄ = @views Z̃[(1 + nΔU + nX̂ + nÂ):(nΔU + nX̂ + n + nK̄)] + Ā = @views Z̃[(1 + nΔU + nX̂ + n + nK̄):(nΔU + nX̂ + n + nK̄ + nĀ)] + Dtemp = mpc.buffer.D̂ + Utemp = similar(Û0) # TODO: remove this allocation + Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0, U0) + Ŝk̄ = @views geq[1:nŜk̄] + Q⁻ = @views geq[(nŜk̄+1):(nŜk̄+nÂ)] + Q̄ = @views geq[(nŜk̄+nÂ+1):end] @threadsif f_threads for j=1:Hp if j < 2 - x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] + x̂d = @views mpc.estim.x̂0[1:nx] d̂0 = @views mpc.d0[1:nd] else - x̂d_Z̃ = @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)] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄dot[(1 + nk̄*(j-1)):(nk̄*j)] - k̄_Z̃ = @views K̄_Z̃[(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)] + ŝk̄ = @views Ŝk̄[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - ŝk̄ = @views geq[(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 < Hp + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + else # j = Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc≤Hp implies Δu(k+Hp) = 0: + û0next = û0 + end # ----------------- collocation constraint defects ----------------------------- - Δk = k̄ + Δk = k̄dot for i=1:no - Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ + Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x̂d end mul!(ŝk̄, Mo, Δk) - d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)] - if h > 0 - ûi = similar(û0) # TODO: remove this allocation - end + d̂i, ûi = @views Dtemp[(1 + nd*(j-1)):(nd*j)], Utemp[(1 + nu*(j-1)):(nu*j)] for i=1:no - k̇i = @views k̄[(1 + (i-1)*nx):(i*nx)] - ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] - d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next - if h < 1 - model.f!(k̇i, ki_Z̃, û0, d̂i, model.p) - else - # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: - û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] - ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next - model.f!(k̇i, ki_Z̃, ûi, d̂i, model.p) - end + 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 + d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next + fq_dae!(k̇i, qi, model, ki, ai, ûi, d̂i) + end + ŝk̄ .-= k̄dot + # ---------------- 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, d̂0next) + else + q⁻next .= @views â0next .- ā[end-na+1:end] end - ŝk̄ .-= k̄ end return geq end From dba86ff88be95ad65ff5ed065256c18933736a02 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 13:33:21 -0400 Subject: [PATCH 09/28] debug: resolve method ambiguities --- src/controller/transcription.jl | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 354434316..dd79ad47f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -618,14 +618,20 @@ end """ init_defectmat( - model::SimModelODE, estim::StateEstimator, transcription::SingleShooting, + model::SimModel, estim::StateEstimator, transcription::SingleShooting, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS -Return empty matrices for [`SingleShooting`](@ref) transcription (N/A). +Return empty matrices for all [`SingleShooting`](@ref) cases (N/A). """ function init_defectmat( - ::SimModelODE, estim::StateEstimator, transcription::SingleShooting, + ::SimModel, estim::StateEstimator, transcription::SingleShooting, + Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing +) + return init_defectmat_empty(estim, transcription, Hp, Hc) +end +function init_defectmat( + ::SimModel, estim::InternalModel, transcription::SingleShooting, Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) From 8f12785e581f372b14e02532bd001fd029cb3f8b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 13:57:36 -0400 Subject: [PATCH 10/28] debug: idem --- src/controller/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index dd79ad47f..69ea4902e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1690,4 +1690,4 @@ function con_nonlinprogeq!( end "No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModelODE,::TranscriptionMethod,_,_)=geq +con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq From 9db293240c24ce7292ee1dd75525986f1b01ab10 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:11:22 -0400 Subject: [PATCH 11/28] doc: warm-starting vector in Extended Help --- src/controller/transcription.jl | 183 +++++++++++++++-------------- src/estimator/mhe/transcription.jl | 22 ++-- 2 files changed, 109 insertions(+), 96 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 69ea4902e..f67512ab8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1049,20 +1049,25 @@ end Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) transcription. -If supported by `mpc.optim`, it warm-starts the solver at: -```math -\mathbf{Z̃_s} = \begin{bmatrix} - \mathbf{Δu}(k+0|k-1) \\ - \mathbf{Δu}(k+1|k-1) \\ - \vdots \\ - \mathbf{Δu}(k+H_c-3|k-1) \\ - \mathbf{Δu}(k+H_c-2|k-1) \\ - \mathbf{0} \\ - ϵ_{k-1} -\end{bmatrix} -``` -where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the -last control period ``k-1``, and ``ϵ_{k-1}``, the slack variable of the last control period. +The warm-starting vector is provided in the Extended Help. + +# Extended Help +!!! details "Extended Help" + If supported by `mpc.optim`, it warm-starts the solver at: + ```math + \mathbf{Z̃_s} = \begin{bmatrix} + \mathbf{Δu}(k+0|k-1) \\ + \mathbf{Δu}(k+1|k-1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ + \mathbf{Δu}(k+H_c-2|k-1) \\ + \mathbf{0} \\ + ϵ_{k-1} + \end{bmatrix} + ``` + where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the + last control period ``k-1``, and ``ϵ_{k-1}``, the slack variable of the last control + period. """ function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) nu, Hc, Z̃s = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ @@ -1081,47 +1086,51 @@ end Do the same but for [`OrthogonalCollocation`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = \begin{bmatrix} - \mathbf{Δu}(k+0|k-1) \\ - \mathbf{Δu}(k+1|k-1) \\ - \vdots \\ - \mathbf{Δu}(k+H_c-3|k-1) \\ - \mathbf{Δu}(k+H_c-2|k-1) \\ - \mathbf{0} \\ - \mathbf{x̂_0}(k+1|k-1) \\ - \mathbf{x̂_0}(k+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+H_p-2|k-1) \\ - \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{â_0}(k+1|k-1) \\ - \mathbf{â_0}(k+2|k-1) \\ - \vdots \\ - \mathbf{â_0}(k+H_p-2|k-1) \\ - \mathbf{â_0}(k+H_p-1|k-1) \\ - \mathbf{â_0}(k+H_p-1|k-1) \\ - \mathbf{k̄}(k+0|k-1) \\ - \mathbf{k̄}(k+1|k-1) \\ - \vdots \\ - \mathbf{k̄}(k+H_p-3|k-1) \\ - \mathbf{k̄}(k+H_p-2|k-1) \\ - \mathbf{k̄}(k+H_p-2|k-1) \\ - \mathbf{ā}(k+0|k-1) \\ - \mathbf{ā}(k+1|k-1) \\ - \vdots \\ - \mathbf{ā}(k+H_p-3|k-1) \\ - \mathbf{ā}(k+H_p-2|k-1) \\ - \mathbf{ā}(k+H_p-2|k-1) \\ - ϵ_{k-1} -\end{bmatrix} -``` -where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the -last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` and ``\mathbf{ā}(k-j|k-1) include -the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at -the last control period. +The warm-starting vector is provided in the Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = \begin{bmatrix} + \mathbf{Δu}(k+0|k-1) \\ + \mathbf{Δu}(k+1|k-1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ + \mathbf{Δu}(k+H_c-2|k-1) \\ + \mathbf{0} \\ + \mathbf{x̂_0}(k+1|k-1) \\ + \mathbf{x̂_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+H_p-2|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+1|k-1) \\ + \mathbf{â_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{â_0}(k+H_p-2|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{k̄}(k+0|k-1) \\ + \mathbf{k̄}(k+1|k-1) \\ + \vdots \\ + \mathbf{k̄}(k+H_p-3|k-1) \\ + \mathbf{k̄}(k+H_p-2|k-1) \\ + \mathbf{k̄}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+0|k-1) \\ + \mathbf{ā}(k+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+H_p-3|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + ϵ_{k-1} + \end{bmatrix} + ``` + where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the + last control period ``k-1``, expressed as a deviation from the operating point + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` and ``\mathbf{ā}(k-j|k-1) + include the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also + computed at the last control period. """ function set_warmstart!( mpc::PredictiveController, transcription::OrthogonalCollocation, Z̃var @@ -1161,36 +1170,40 @@ end Do the same but for other [`TranscriptionMethod`](@ref). -It warm-starts the solver at: -```math -\mathbf{Z̃_s} = \begin{bmatrix} - \mathbf{Δu}(k+0|k-1) \\ - \mathbf{Δu}(k+1|k-1) \\ - \vdots \\ - \mathbf{Δu}(k+H_c-3|k-1) \\ - \mathbf{Δu}(k+H_c-2|k-1) \\ - \mathbf{0} \\ - \mathbf{x̂_0}(k+1|k-1) \\ - \mathbf{x̂_0}(k+2|k-1) \\ - \vdots \\ - \mathbf{x̂_0}(k+H_p-2|k-1) \\ - \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{â_0}(k+1|k-1) \\ - \mathbf{â_0}(k+2|k-1) \\ - \vdots \\ - \mathbf{â_0}(k+H_p-2|k-1) \\ - \mathbf{â_0}(k+H_p-1|k-1) \\ - \mathbf{â_0}(k+H_p-1|k-1) \\ - \mathbf{ā}(k+0|k-1) \\ - \mathbf{ā}(k+1|k-1) \\ - \vdots \\ - \mathbf{ā}(k+H_p-3|k-1) \\ - \mathbf{ā}(k+H_p-2|k-1) \\ - \mathbf{ā}(k+H_p-2|k-1) \\ - ϵ_{k-1} -\end{bmatrix} -``` +The warm-starting vector is provided in the Extended Help. + +# Extended Help +!!! details "Extended Help" + It warm-starts the solver at: + ```math + \mathbf{Z̃_s} = \begin{bmatrix} + \mathbf{Δu}(k+0|k-1) \\ + \mathbf{Δu}(k+1|k-1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ + \mathbf{Δu}(k+H_c-2|k-1) \\ + \mathbf{0} \\ + \mathbf{x̂_0}(k+1|k-1) \\ + \mathbf{x̂_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+H_p-2|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+1|k-1) \\ + \mathbf{â_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{â_0}(k+H_p-2|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{â_0}(k+H_p-1|k-1) \\ + \mathbf{ā}(k+0|k-1) \\ + \mathbf{ā}(k+1|k-1) \\ + \vdots \\ + \mathbf{ā}(k+H_p-3|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + \mathbf{ā}(k+H_p-2|k-1) \\ + ϵ_{k-1} + \end{bmatrix} + ``` """ function set_warmstart!( mpc::PredictiveController, transcription::TranscriptionMethod, Z̃var diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ff85c6a6d..8f63ea40e 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1123,11 +1123,11 @@ end Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). -If supported by `estim.optim` and based a [`SingleShooting`](@ref) transcription, it -warm-starts the solver at the value provided in Extended Help. +The warm-starting vector is provided in the Extended Help. # Extended Help !!! details "Extended Help" + If supported by `estim.optim` it warm-starts the solver at: ```math \mathbf{Z̃_s} = \begin{bmatrix} @@ -1142,13 +1142,13 @@ warm-starts the solver at the value provided in Extended Help. \mathbf{0_ŵ} \end{bmatrix} ``` - where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the - process noise estimates computed at the last time step ``k-1``. The vector - ``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated - at time ``k-N_k``. If the objective function is not finite at this point, all the process - noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended Help of - [`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The method mutates - all the arguments. + where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and + the process noise estimates computed at the last time step ``k-1``. The vector + ``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival + estimated at time ``k-N_k``. If the objective function is not finite at this point, all + the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. See the Extended + Help of [`SingleShooting`](@ref) for the defintion of vector ``\mathbf{0_ŵ}``. The + method mutates all the arguments. """ function set_warmstart!( estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var @@ -1195,7 +1195,7 @@ end Do the same but for [`OrthogonalCollocation`](@ref). -The warm-starting value is provided in Extended Help. +The warm-starting vector is provided in Extended Help. # Extended Help !!! details "Extended Help" @@ -1314,7 +1314,7 @@ end Do the same but for other transcription [`TranscriptionMethod`](@ref). -The warm-starting value is provided in Extended Help. +The warm-starting vector is provided in Extended Help. # Extended Help !!! details "Extended Help" From bf8a3877fa1c3704ce75d246311f661e448cda63 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:33:37 -0400 Subject: [PATCH 12/28] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 022359586..3157d3759 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.14.4" +version = "2.15.0" authors = ["Francis Gagnon"] [deps] From c5b5cb7f74ba1a5e49f46d428858d5025f8f2d4e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:37:23 -0400 Subject: [PATCH 13/28] doc: minor corrections --- src/estimator/mhe/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 8f63ea40e..7ba3f4b97 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1215,7 +1215,7 @@ The warm-starting vector is provided in Extended Help. \mathbf{â_0}(k-N_k+p|k-1) \\ \mathbf{â_0}(k-N_k+p+1|k-1) \\ \mathbf{â_0}(k-N_k+p+2|k-1) \\ - \vdots + \vdots \\ \mathbf{â_0}(k-p-2|k-1) \\ \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{a_0}(k-p-1|k-1) \\ @@ -1334,7 +1334,7 @@ The warm-starting vector is provided in Extended Help. \mathbf{â_0}(k-N_k+p|k-1) \\ \mathbf{â_0}(k-N_k+p+1|k-1) \\ \mathbf{â_0}(k-N_k+p+2|k-1) \\ - \vdots + \vdots \\ \mathbf{â_0}(k-p-2|k-1) \\ \mathbf{â_0}(k-p-1|k-1) \\ \mathbf{â_0}(k-p-1|k-1) \\ From 6c244864e55e1a3a9738eb7de0455f3719a8b9fc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:38:22 -0400 Subject: [PATCH 14/28] doc: idem --- 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 7ba3f4b97..5de935cb0 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1245,7 +1245,7 @@ The warm-starting vector is provided in Extended Help. ``` where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point - ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` and ``\mathbf{ā}(k-j|k-1) + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k-j|k-1)`` and ``\mathbf{ā}(k-j|k-1)`` include the ``n_o`` intermediate stage predictions for the interval ``k-j``, and is also computed at the last control period. See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for the defintion of vectors ``\mathbf{0_x̂}``, From 21ad5b3d857bf454aa3c5d98fdeef0bcaf732ec8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:39:04 -0400 Subject: [PATCH 15/28] doc: idem --- 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 5de935cb0..4102bff67 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1249,7 +1249,7 @@ The warm-starting vector is provided in Extended Help. include the ``n_o`` intermediate stage predictions for the interval ``k-j``, and is also computed at the last control period. See the Extended Help of [`MultipleShooting`](@ref) and [`OrthogonalCollocation`](@ref) for the defintion of vectors ``\mathbf{0_x̂}``, - ``\mathbf{0_â}``, ``\mathbf{0_k}`` and ``\mathbf{0_ŵ}``. + ``\mathbf{0_â}``, ``\mathbf{0_k̄}``, ``\mathbf{0_ā}``, and ``\mathbf{0_ŵ}``. """ function set_warmstart!( estim::MovingHorizonEstimator{NT}, transcription::OrthogonalCollocation, Z̃var From 38094c6805869e5ea025758fbf238164fe0575af Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 14:40:49 -0400 Subject: [PATCH 16/28] doc: idem --- src/controller/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f67512ab8..5cebee1df 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1128,7 +1128,7 @@ The warm-starting vector is provided in the Extended Help. ``` where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the last control period ``k-1``, expressed as a deviation from the operating point - ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` and ``\mathbf{ā}(k-j|k-1) + ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k̄}(k+j|k-1)`` and ``\mathbf{ā}(k-j|k-1)`` include the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at the last control period. """ From 48b2503f8bb0157a1345a6bdec9af981f44f81bc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 15:12:53 -0400 Subject: [PATCH 17/28] added: default estimator with `NonLinModelDAE` in `NonLinMPC` The estimation horizon `He` still needs to be explicitly defined in the keyword arguments. --- src/controller/nonlinmpc.jl | 11 ++++++----- src/estimator/execute.jl | 1 + test/2_test_state_estim.jl | 2 +- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 0750d6a35..4fd1a194d 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -154,9 +154,9 @@ struct NonLinMPC{ end @doc raw""" - NonLinMPC(model::SimModelODE; ) + NonLinMPC(model::SimModel; ) -Construct a nonlinear predictive controller based on [`SimModelODE`](@ref) `model`. +Construct a nonlinear predictive controller based on [`SimModel`](@ref) `model`. Both [`NonLinModel`](@ref) and [`LinModel`](@ref) are supported (see Extended Help). The controller minimizes the following objective function at each discrete time ``k``: @@ -197,7 +197,8 @@ definition of the other variables. This method uses the default state estimator: - if `model` is a [`LinModel`](@ref), a [`SteadyKalmanFilter`](@ref) with default arguments; -- else, an [`UnscentedKalmanFilter`](@ref) with default arguments. +- if `model` is a [`NonLinModel`](@ref), an [`UnscentedKalmanFilter`](@ref) with default arguments; +- else, a [`MovingHorizonEstimator`](@ref) with default arguments. This controller allocates memory at each time step for the optimization. @@ -206,7 +207,7 @@ This controller allocates memory at each time step for the optimization. `MethodError: no method matching Float64(::ForwardDiff.Dual)`. # Arguments -- `model::SimModelODE` : model used for controller predictions and state estimations. +- `model::SimModel` : model used for controller predictions and state estimations. - `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays if `model` is a [`LinModel`](@ref) (must be specified otherwise). - `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is @@ -346,7 +347,7 @@ NonLinMPC controller with a sample time Ts = 5.0 s: `10/Cwt` (if not already set), to scale the small values of ``ϵ``. """ function NonLinMPC( - model::SimModelODE; + model::SimModel; Hp::Int = default_Hp(model), Hc::IntVectorOrInt = DEFAULT_HC, Mwt = fill(DEFAULT_MWT, model.ny), diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 4b59ce8ea..58e2e488a 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -153,6 +153,7 @@ function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) mul!(ŷ0, estim.Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end +ĥ!(ŷ0, ::InternalModel, model::SimModel, x̂0, â0, d0) = h_dae!(ŷ0, model, x̂0, â0, d0) """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 91a386e0f..80b05cd94 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1465,7 +1465,7 @@ end preparestate!(mhe, [7.0], [0]) @test mhe([0]) ≈ [7.0] atol=1e-3 - # h=1 & f_threads=false options test the branch with reuse of k̇1 & q1 from prev. iter: + # h=1 & f_threads=false options test the branch with reuse of k̇ & q from prev. iter: transcription = TrapezoidalCollocation(1, f_threads=false, h_threads=true) mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) preparestate!(mhe2, [0.0], [0.0]) From 6e4e0a4042e9211fe1d54fbd336dc628d14c9c03 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:17:22 -0400 Subject: [PATCH 18/28] debug: correctly reset warm-starting for `NonLinMPC` --- src/controller/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5cebee1df..04a6d54fa 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1016,7 +1016,7 @@ function reset_warmstart!(mpc::PredictiveController, ::TrapezoidalCollocation) a0s = as_0 mpc.Z̃ .= 0 for j in 1:Hp - mpc.Z̃[(nΔU + nx*(j-1) + 1):(nΔU + nx*j)] .= x0s + mpc.Z̃[(nΔU + nx̂*(j-1) + 1):(nΔU + nx̂*(j-1) + nx)] .= x0s mpc.Z̃[(nΔU + nX̂ + na*(j-1) + 1):(nΔU + nX̂ + na*j)] .= a0s mpc.Z̃[(nΔU + nX̂ + n + na*(j-1) + 1):(nΔU + nX̂ + n + na*j)] .= a0s end @@ -1034,7 +1034,7 @@ function reset_warmstart!(mpc::PredictiveController, transcription::OrthogonalCo a0s = as_0 mpc.Z̃ .= 0 for j in 1:Hp - mpc.Z̃[(nΔU + nx*(j-1) + 1):(nΔU + nx*j)] .= x0s + mpc.Z̃[(nΔU + nx̂*(j-1) + 1):(nΔU + nx̂*(j-1) + nx)] .= x0s mpc.Z̃[(nΔU + nX̂ + na*(j-1) + 1):(nΔU + nX̂ + na*j)] .= a0s end K̄ = @views mpc.Z̃[(nΔU + nX̂ + n + 1):(nΔU + nX̂ + n + nK̄)] From 88912cc06daa6b5baf8c0922d8d02ce7e557e27e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:26:09 -0400 Subject: [PATCH 19/28] test: `NonLinMPC` with `NonLinModelDAE` and `TrapezoidalCollocation` --- test/2_test_state_estim.jl | 2 +- test/3_test_predictive_control.jl | 43 +++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 80b05cd94..644866ae7 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1476,7 +1476,7 @@ end @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 - @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 # a1 in Ā + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ as_0 # a⁺ in Ā end @testitem "MHE estim. & getinfo (NonLinModelDAE, OC)" setup=[SetupMPCtests] begin diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 83ba626e7..41fd67b34 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1119,6 +1119,49 @@ end @test nmpc6_1.con.FS ≈ nmpc6_1.con.KS*nmpc6_1.estim.x̂0 # OC + IMC: special lin. eq. method end +@testitem "NonLinMPC moves & getinfo (NonLinModelDAE, TC)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = x[] + a[] + 0.1 * d[] + end + Kp = 2*0.2 # static gain for input, factor 2 since the output is 2*x (a==x) + Ts, p = 100.0, [0.01] + as_0, xs_0 = [-1.0], [1.0] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + + transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) + nmpc = NonLinMPC(dae; Hp=100, Hc=1, He=2, Nwt=[0.0], transcription) + preparestate!(nmpc, [0.0], [0.0]) + u = moveinput!(nmpc, [Kp], [0.0]) + @test u ≈ [1.0] atol=5e-2 + info = getinfo(nmpc) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ Kp atol=5e-3 + + # h=1 & f_threads=false options test the branch with reuse of k̇ & q from prev. iter: + transcription = TrapezoidalCollocation(1, f_threads=false, h_threads=true) + nmpc2 = NonLinMPC(dae; Hp=100, Hc=1, He=2, Nwt=[0.0], transcription) + preparestate!(nmpc2, [0.0], [0.0]) + u = moveinput!(nmpc2, [Kp], [0.0]) + @test u ≈ [1.0] atol=5e-2 + + initstate!(nmpc2, [0], [0], [0]) + nX̂, n = nmpc2.estim.nx̂*100, dae.na*100 + @test nmpc2.Z̃[1:1] ≈ [0.0] # Δu in ΔU + @test nmpc2.Z̃[(1 + 1):(1 + nX̂)] ≈ repeat([xs_0; 0.0], 100) # x̂d and x̂s in X̂0 + @test nmpc2.Z̃[(1 + nX̂ + 1):(1 + nX̂ + nÂ)] ≈ repeat(as_0, 100) # â0 in Â0 + @test nmpc2.Z̃[(1 + nX̂ + n + 1):end-1] ≈ repeat(as_0, 100) # a⁺ in Ā + +end + @testitem "NonLinMPC step disturbance rejection" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) From c84cacfb93fc2d39b6b1ba74e2d6aa8a9aaffbdb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:38:06 -0400 Subject: [PATCH 20/28] test: `NonLinMPC` with `NonLinModelDAE` and `OrthogonalCollocation` --- test/2_test_state_estim.jl | 1 - test/3_test_predictive_control.jl | 45 +++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 644866ae7..90a66d324 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1521,7 +1521,6 @@ end @test mhe([0]) ≈ [7.0] atol=5e-3 no, roots = 2, :gaussradau - # h=1 & f_threads=false options test the branch with reuse of q̄ from prev. iter: transcription = OrthogonalCollocation(1, no; roots, f_threads=false, h_threads=true) mhe2 = MovingHorizonEstimator(dae; He=2, Cwt=1e4, direct=false, transcription) preparestate!(mhe2, [0.0], [0.0]) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 41fd67b34..ee9eaa004 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1162,6 +1162,51 @@ end end +@testitem "NonLinMPC moves & getinfo (NonLinModelDAE, OC)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = x[] + a[] + 0.1 * d[] + end + Kp = 2*0.2 # static gain for input, factor 2 since the output is 2*x (a==x) + Ts, p = 100.0, [0.01] + as_0, xs_0 = [-1.0], [1.0] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + + no, roots = 3, :gausslegendre + transcription = OrthogonalCollocation(0, no; roots, f_threads=true, h_threads=false) + nmpc = NonLinMPC(dae; Hp=100, Hc=1, He=2, Nwt=[0.0], transcription) + preparestate!(nmpc, [0.0], [0.0]) + u = moveinput!(nmpc, [Kp], [0.0]) + @test u ≈ [1.0] atol=5e-2 + info = getinfo(nmpc) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ Kp atol=5e-3 + + no, roots = 2, :gaussradau + transcription = OrthogonalCollocation(1, no; roots, f_threads=false, h_threads=true) + nmpc2 = NonLinMPC(dae; Hp=100, Hc=1, He=2, Nwt=[0.0], transcription) + preparestate!(nmpc2, [0.0], [0.0]) + u = moveinput!(nmpc2, [Kp], [0.0]) + @test u ≈ [1.0] atol=5e-2 + + initstate!(nmpc2, [0], [0], [0]) + nX̂, nÂ, nK̄, nĀ = nmpc2.estim.nx̂*100, dae.na*100, dae.nx*no*100, dae.na*no*100 + @test nmpc2.Z̃[1:1] ≈ [0.0] # Δu in ΔU + @test nmpc2.Z̃[(1 + 1):(1 + nX̂)] ≈ repeat([xs_0; 0.0], 100) # x̂d and x̂s in X̂0 + @test nmpc2.Z̃[(1 + nX̂ + 1):(1 + nX̂ + nÂ)] ≈ repeat(as_0, 100) # â0 in Â0 + @test nmpc2.Z̃[(1 + nX̂ + n + 1):(1 + nX̂ + n + nK̄)] ≈ repeat(xs_0, no*100) # k̄ in K̄ + @test nmpc2.Z̃[(1 + nX̂ + n + nK̄ + 1):end-1] ≈ repeat(as_0, no*100) # ā in Ā + +end + @testitem "NonLinMPC step disturbance rejection" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) From 78b96c3e1f030c9d14d6aeaa1d575918b9d257f3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:38:21 -0400 Subject: [PATCH 21/28] test: `NonLinMPC` construction with `NonLinModelDAE` --- test/3_test_predictive_control.jl | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index ee9eaa004..8fa2ee1a5 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -905,6 +905,34 @@ end @test_throws ArgumentError OrthogonalCollocation(2) end +@testitem "NonLinMPC construction (NonLinModelDAE)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, d, _ ) + y[] = 2 * x[] + a[] + 0.1 * d[] + end + Ts, p = 100.0, [0.01] + dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) + + transcription = TrapezoidalCollocation() + nmpc = NonLinMPC(dae; He=3, Hp=3, transcription) + @test mhe.transcription isa TrapezoidalCollocation + + nmpc2 = NonLinMPC(dae; He=3, Hp=3, hessian=true) + @test nmpc2.transcription isa OrthogonalCollocation + + @test_throws ArgumentError NonLinMPC(dae; Hp=3, He=3, transcription=SingleShooting()) + @test_throws ArgumentError NonLinMPC(dae; Hp=3, He=3, transcription=MultipleShooting()) + +end + @testitem "NonLinMPC moves & getinfo (LinModel, SS)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface From e45dac1611365b5578de4a2df7f77b8ebea96198 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:39:44 -0400 Subject: [PATCH 22/28] test: debug tests --- test/3_test_predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 8fa2ee1a5..ad00956dd 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -923,7 +923,7 @@ end transcription = TrapezoidalCollocation() nmpc = NonLinMPC(dae; He=3, Hp=3, transcription) - @test mhe.transcription isa TrapezoidalCollocation + @test nmpc.transcription isa TrapezoidalCollocation nmpc2 = NonLinMPC(dae; He=3, Hp=3, hessian=true) @test nmpc2.transcription isa OrthogonalCollocation From a64730580c447ccfd7a8930ea73afefd31c9f5e6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 16:44:17 -0400 Subject: [PATCH 23/28] debug: move method definition in `internal_model.jl` --- src/estimator/execute.jl | 1 - src/estimator/internal_model.jl | 11 ++++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 58e2e488a..4b59ce8ea 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -153,7 +153,6 @@ function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, â0, d0) mul!(ŷ0, estim.Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end -ĥ!(ŷ0, ::InternalModel, model::SimModel, x̂0, â0, d0) = h_dae!(ŷ0, model, x̂0, â0, d0) """ disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0 diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 3c9c8c270..74512c459 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -187,9 +187,14 @@ end Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls [`h!`](@ref). """ -function ĥ!(ŷ0, ::InternalModel, model::NonLinModel, x̂0, d0) - return h!(ŷ0, model, x̂0, d0, model.p) -end +ĥ!(ŷ0, ::InternalModel, model::NonLinModel, x̂0, d0) = h!(ŷ0, model, x̂0, d0, model.p) + +@doc raw""" + ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, â0, d0) + +Same, but calls [`h_dae!`](@ref) instead. +""" +ĥ!(ŷ0, ::InternalModel, model::SimModel, x̂0, â0, d0) = h_dae!(ŷ0, model, x̂0, â0, d0) "No input disturbances for [`InternalModel`](@ref), return `Û0 .= U0`." disturbedinput!(Û0, ::InternalModel, _ , _ , U0) = (Û0 .= U0) From f26ee643d2de19a9f8ad8acc37307d1ce2e1f2c3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 15 Oct 2026 17:06:37 -0400 Subject: [PATCH 24/28] added: `NonLinMPC` with `NonLinModelDAE` precompilation --- src/precompile.jl | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/precompile.jl b/src/precompile.jl index d569cb199..f2efad265 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -91,25 +91,20 @@ R̂y = repeat([55; 30], 3) ) sim!(nmpc_ukf, 2, [55, 30]) - transcription = OrthogonalCollocation(0, 2) - nmpc_mhe = setconstraint!(NonLinMPC( - MovingHorizonEstimator(nlmodel; He=2, transcription); transcription, Hp=3, Cwt=Inf), - ymin=[45, -Inf] - ) - setconstraint!(nmpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50]) - initstate!(nmpc_mhe, nlmodel.uop, y) - preparestate!(nmpc_mhe, [55, 30]) - nmpc_mhe([55, 30]) - linearizemodel = linearize(nlmodel) setmodel!(mpc_kf, linearizemodel) + transcription = OrthogonalCollocation(0, 2) daemodel = NonLinModelDAE(fq_dae!, h_dae!, 100.0, 1, 1, 1, 1; transcription) sim!(daemodel, 2, [10]) mhe_dae = MovingHorizonEstimator(daemodel, He=2, direct=false) - preparestate!(mhe_dae, [5]) - updatestate!(mhe_dae, [2], [5]) + setconstraint!(mhe_dae, x̂min=[-50, 50], x̂max=[50, 50]) + nmpc_dae = NonLinMPC(mhe_dae, Hp=3) + setconstraint!(nmpc_dae, ymin=[-50], ymax=[50]) + preparestate!(nmpc_dae, [5]) + moveinput!(nmpc_dae, [0.1]) + updatestate!(nmpc_dae, [2], [5]) end From 8098f7ef1560de38976de65ca46816fe4a3c6577 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 16 Oct 2026 10:10:21 -0400 Subject: [PATCH 25/28] =?UTF-8?q?Modifications=20de=20l=E2=80=99h=C3=B4te?= =?UTF-8?q?=20de=20l=E2=80=99agent=20pour=20dae=5Fmpc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 04a6d54fa..d8aa47c94 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -415,6 +415,17 @@ function init_defectmat( return ES, GS, JS, KS, VS, BS end +function init_defectmat( + model::LinModel, estim::InternalModel{NT}, transcription::MultipleShooting, + Hp, Hc, nb, Co=nothing, λo=nothing +) where {NT<:Real} + return invoke( + init_defectmat, + Tuple{LinModel, StateEstimator{NT}, MultipleShooting, Any, Any, Any, Any, Any}, + model, estim, transcription, Hp, Hc, nb, Co, λo + ) +end + @doc raw""" init_defectmat( model::SimModel, estim::StateEstimator, transcription::TranscriptionMethod, @@ -610,7 +621,7 @@ end Return empty matrices for [`InternalModel`](@ref) (the state vector is not augmented). """ function init_defectmat( - ::SimModel, estim::InternalModel, transcription::TranscriptionMethod, + ::SimModel, estim::InternalModel, transcription::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) @@ -941,6 +952,17 @@ function linconstrainteq!( return nothing end +function linconstrainteq!( + mpc::PredictiveController, model::LinModel, estim::InternalModel, + transcription::MultipleShooting +) + return invoke( + linconstrainteq!, + Tuple{PredictiveController, LinModel, StateEstimator, MultipleShooting}, + mpc, model, estim, transcription + ) +end + """ linconstrainteq!( mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod From 4f05328c7ce0d73013967af4ef758b93dbacf8b4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 16 Oct 2026 10:38:58 -0400 Subject: [PATCH 26/28] changed: avoid explicit `invoke` calls It's better to be explicit on the multiple dispatch. --- src/controller/transcription.jl | 48 ++++++++++++++++----------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d8aa47c94..d0890442a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -373,9 +373,21 @@ matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended H (see [`move_blocking`](@ref)). """ function init_defectmat( - model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, + ::LinModel, estim::StateEstimator, ::MultipleShooting, Hp, Hc, nb, ::Any=nothing, ::Any=nothing -) where {NT<:Real} +) + return init_defectmat_lin(estim, Hp, Hc, nb) +end + +function init_defectmat( + ::LinModel, estim::InternalModel, ::MultipleShooting, + Hp, Hc, nb, ::Any=nothing, ::Any=nothing +) + return init_defectmat_lin(estim, Hp, Hc, nb) +end + +function init_defectmat_lin(estim::StateEstimator{NT}, Hp, Hc, nb) where {NT<:Real} + model = estim.model nu, nx̂, nd = model.nu, estim.nx̂, model.nd Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d # helper function to be similar to eqs. in docstring: @@ -415,17 +427,6 @@ function init_defectmat( return ES, GS, JS, KS, VS, BS end -function init_defectmat( - model::LinModel, estim::InternalModel{NT}, transcription::MultipleShooting, - Hp, Hc, nb, Co=nothing, λo=nothing -) where {NT<:Real} - return invoke( - init_defectmat, - Tuple{LinModel, StateEstimator{NT}, MultipleShooting, Any, Any, Any, Any, Any}, - model, estim, transcription, Hp, Hc, nb, Co, λo - ) -end - @doc raw""" init_defectmat( model::SimModel, estim::StateEstimator, transcription::TranscriptionMethod, @@ -938,6 +939,16 @@ Also init ``\mathbf{F_S} = \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + \mathbf{K_ function linconstrainteq!( mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting ) + return linconstrainteq_lin!(mpc, model) +end + +function linconstrainteq!( + mpc, model::LinModel, ::InternalModel, ::MultipleShooting +) + return linconstrainteq_lin!(mpc, model) +end + +function linconstrainteq_lin!(mpc::PredictiveController, model::LinModel) FS = mpc.con.FS FS .= mpc.con.BS mul!(FS, mpc.con.KS, mpc.estim.x̂0, 1, 1) @@ -952,17 +963,6 @@ function linconstrainteq!( return nothing end -function linconstrainteq!( - mpc::PredictiveController, model::LinModel, estim::InternalModel, - transcription::MultipleShooting -) - return invoke( - linconstrainteq!, - Tuple{PredictiveController, LinModel, StateEstimator, MultipleShooting}, - mpc, model, estim, transcription - ) -end - """ linconstrainteq!( mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod From f9d513a16afe4b46028b66678ef33fa12a645c19 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 16 Oct 2026 11:25:37 -0400 Subject: [PATCH 27/28] debug: dispatch pattern --- src/controller/transcription.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d0890442a..c3d8959a0 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -373,16 +373,16 @@ matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended H (see [`move_blocking`](@ref)). """ function init_defectmat( - ::LinModel, estim::StateEstimator, ::MultipleShooting, + ::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc, nb, ::Any=nothing, ::Any=nothing -) +) where NT <: Real return init_defectmat_lin(estim, Hp, Hc, nb) end function init_defectmat( - ::LinModel, estim::InternalModel, ::MultipleShooting, + ::LinModel, estim::InternalModel{NT}, ::MultipleShooting, Hp, Hc, nb, ::Any=nothing, ::Any=nothing -) +) where NT <: Real return init_defectmat_lin(estim, Hp, Hc, nb) end @@ -943,7 +943,7 @@ function linconstrainteq!( end function linconstrainteq!( - mpc, model::LinModel, ::InternalModel, ::MultipleShooting + mpc::PredictiveController, model::LinModel, ::InternalModel, ::MultipleShooting ) return linconstrainteq_lin!(mpc, model) end From c98c3ac011d3cd7dcf8e9c4d937e06d8c321cd39 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 16 Oct 2026 12:42:54 -0400 Subject: [PATCH 28/28] debug: do not update `beq` if no linear eq. constraints --- src/controller/transcription.jl | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c3d8959a0..31b979af8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -958,8 +958,10 @@ function linconstrainteq_lin!(mpc::PredictiveController, model::LinModel) mul!(FS, mpc.con.JS, mpc.D̂0, 1, 1) end mpc.con.beq .= @. -FS - linconeq = mpc.optim[:linconstrainteq] - JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + if length(mpc.con.beq) > 0 + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + end return nothing end @@ -980,8 +982,10 @@ function linconstrainteq!( FS = mpc.con.FS mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS mpc.con.beq .= @. -FS - linconeq = mpc.optim[:linconstrainteq] - JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + if length(mpc.con.beq) > 0 + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + end return nothing end @@ -1000,8 +1004,10 @@ function linconstrainteq!( FS = mpc.con.FS mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS mpc.con.beq .= @. -FS - linconeq = mpc.optim[:linconstrainteq] - JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + if length(mpc.con.beq) > 0 + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + end return nothing end "No linear equality constraints for other cases of [`InternalModel`](@ref)."