From e3627347694f2b8976437bf6f6e9f3a1d61a5ed2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 7 Mar 2026 16:39:00 -0500 Subject: [PATCH 01/18] wip: linear constraint for stochastic defect --- src/controller/transcription.jl | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5deecdbf4..bdf9053b1 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -704,6 +704,33 @@ function init_defectmat( return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end +function init_defectmat( + model::NonLinModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ +) + +end + +""" + init_defectmat( + model::NonLinModel, estim::IntenalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + +Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented). +""" +function init_defectmat( + model::NonLinModel, estim::IntenalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ +) where {NT<:Real} + nx̂, nu, nd = estim.nx̂, model.nu, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + Eŝ = zeros(NT, 0, nZ) + Gŝ = zeros(NT, 0, nd) + Jŝ = zeros(NT, 0, nd*Hp) + Kŝ = zeros(NT, 0, nx̂) + Vŝ = zeros(NT, 0, nu) + Bŝ = zeros(NT, 0) + return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ +end + """ init_defectmat( model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb From 95208ad2d0c3026c305d67d772c14f03665ccbb8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 9 Mar 2026 14:03:31 -0400 Subject: [PATCH 02/18] changed: rename to upper case (for consistency) --- src/controller/construct.jl | 18 +++++++++--------- src/controller/execute.jl | 6 +++--- src/controller/transcription.jl | 30 +++++++++++++++++++----------- 3 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 588d93f9c..1e85626f5 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -176,7 +176,7 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # indices of finite numbers in the b vector (linear inequality constraints): i_b ::BitVector # A matrices for the linear equality constraints: - A_ŝ ::Matrix{NT} + A_Ŝ ::Matrix{NT} Aeq ::Matrix{NT} # b vector for the linear equality constraints: beq ::Vector{NT} @@ -518,7 +518,7 @@ function setconstraint!( con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, con.A_x̂min, con.A_x̂max, - con.A_ŝ + con.A_Ŝ ) A = con.A[con.i_b, :] b = con.b[con.i_b] @@ -910,7 +910,7 @@ function init_defaultcon_mpc( A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) + A_Ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) @@ -920,7 +920,7 @@ function init_defaultcon_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, - A_ŝ + A_Ŝ ) # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) @@ -935,7 +935,7 @@ function init_defaultcon_mpc( A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , A_Ymin , A_Ymax , A_Wmin , A_Wmax , A_x̂min , A_x̂max , A , b , i_b , - A_ŝ , + A_Ŝ , Aeq , beq , neq , C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , @@ -1194,14 +1194,14 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where end @doc raw""" - augmentdefect(Eŝ, nϵ) -> A_ŝ, Ẽŝ + augmentdefect(Eŝ, nϵ) -> A_Ŝ, Ẽŝ Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the defect equation ``\mathbf{Ŝ = Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: ```math -\mathbf{A_ŝ Z̃} = - \mathbf{F_ŝ} +\mathbf{A_Ŝ Z̃} = - \mathbf{F_ŝ} ``` """ function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real @@ -1210,8 +1210,8 @@ function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real else # Z̃ = Z (only hard constraints) Ẽŝ = Eŝ end - A_ŝ = Ẽŝ - return A_ŝ, Ẽŝ + A_Ŝ = Ẽŝ + return A_Ŝ, Ẽŝ end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 2164255ad..089c9b6ea 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -691,7 +691,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.bx̂ .= bx̂ # --- defect matrices --- Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) + A_Ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) con.Ẽŝ .= Ẽŝ con.Gŝ .= Gŝ con.Jŝ .= Jŝ @@ -718,8 +718,8 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.A_x̂max ] # --- linear equality constraints --- - con.A_ŝ .= A_ŝ - con.Aeq .= A_ŝ + con.A_Ŝ .= A_Ŝ + con.Aeq .= A_Ŝ # --- operating points --- con.U0min .+= mpc.Uop # convert U0 to U with the old operating point con.U0max .+= mpc.Uop # convert U0 to U with the old operating point diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index dff5c8f1d..fc7681199 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -709,19 +709,27 @@ end function init_defectmat( model::NonLinModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ -) - +) where {NT<:Real} + nx̂, nu, nd = estim.nx̂, model.nu, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + Eŝ = zeros(NT, 0, nZ) + Gŝ = zeros(NT, 0, nd) + Jŝ = zeros(NT, 0, nd*Hp) + Kŝ = zeros(NT, 0, nx̂) + Vŝ = zeros(NT, 0, nu) + Bŝ = zeros(NT, 0) + return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end """ init_defectmat( - model::NonLinModel, estim::IntenalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + model::NonLinModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented). """ function init_defectmat( - model::NonLinModel, estim::IntenalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + model::NonLinModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) @@ -778,7 +786,7 @@ The argument `nc` is the number of custom nonlinear inequality constraints in finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a case, `args` needs to contain all the inequality and equality constraint matrices: -`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ`. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ`. The integer `neq` is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( @@ -795,7 +803,7 @@ function init_matconstraint_mpc( A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, - A_ŝ + A_Ŝ ) = args A = [ A_Umin; A_Umax; @@ -804,7 +812,7 @@ function init_matconstraint_mpc( A_Wmin; A_Wmax A_x̂min; A_x̂max; ] - Aeq = A_ŝ + Aeq = A_Ŝ neq = 0 end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] @@ -821,9 +829,9 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_Ŝ = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] - Aeq = A_ŝ + Aeq = A_Ŝ neq = 0 end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] @@ -840,9 +848,9 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max] - Aeq = A_ŝ + Aeq = A_Ŝ nΔŨ, nZ̃ = size(A_ΔŨmin) neq = nZ̃ - nΔŨ end From 78f50ba9d3809f5eb9ddb7a76d5c71a5d526304d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 9 Mar 2026 15:18:30 -0400 Subject: [PATCH 03/18] =?UTF-8?q?changed:=20replaced=20`f=CC=82=5Finput!`?= =?UTF-8?q?=20with=20`disturbedinput!`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/src/internals/state_estim.md | 1 - src/controller/execute.jl | 31 ++++++++++++++++++++++++++++++- src/controller/transcription.jl | 20 ++++++-------------- src/estimator/execute.jl | 19 ------------------- src/estimator/internal_model.jl | 7 ------- 5 files changed, 36 insertions(+), 42 deletions(-) diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 2a8df67d0..6b92cc2d7 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -27,7 +27,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! -ModelPredictiveControl.f̂_input! ``` ## Update Quadratic Optimization diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 089c9b6ea..09b9179dd 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -312,7 +312,36 @@ function predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel) return nothing end "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." -predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) +predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) + +""" + disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) -> nothing + +Fill disturbed inputs of the augmented model `Û0` in-place with stochastic states in `X̂0` + +Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+H_p-1``. The +predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+H_p-1`` (the current one +is stored in `estim.x̂0`). + +This function is used for the collocation methods that directly call the state derivative +function `estim.model.f!` with the manipulated inputs augmented with the estimated +disturbances at model input (see [`init_estimstoch`](@ref)). It's also necessary to prefill +the `Û0` vector before anything since both `û0` and `û0next` are needed at each stage with +hold order `h>0`, thus potential race conditions with multi-threading. +""" +function disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) + nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂ + Cs_u = estim.Cs_u + Û0 .= U0 + for j=0:mpc.Hp-1 + xs = @views j < 1 ? estim.x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] + û0 = @views Û0[(1+nu*j):(nu*(j+1))] + mul!(û0, Cs_u, xs, 1, 1) # û0 = u0 + Cs_u*xs + end + return nothing +end +"No input disturbances for [`InternalModel`](@ref), hence do `Û0 .= U0`." +disturbedinput!(Û0, ::PredictiveController, ::InternalModel, U0, _) = (Û0 .= U0; nothing) @doc raw""" linconstraint_custom!(mpc::PredictiveController, model::SimModel) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fc7681199..a616b19cc 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1397,7 +1397,7 @@ deterministic and stochastic states extracted from the decision variables `Z̃`. \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂_input!`](@ref). +is defined in [`f̂!`](@ref) documentation. """ function con_nonlinprogeq!( geq, X̂0, Û0, K̇, @@ -1412,11 +1412,7 @@ function con_nonlinprogeq!( nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): - x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - end + disturbedinput!(Û0, mpc, estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1503,9 +1499,9 @@ and disturbances are piecewise constant or linear: \mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) \end{aligned} ``` -The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for -the stochastic states ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) -method, and the ones for the continuity constraint of the deterministic states are: +The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The stochastic state +defects ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) method, and +the ones for the continuity constraint of the deterministic states are: ```math \mathbf{s_c}(k+j+1) = \mathbf{C_o} \begin{bmatrix} @@ -1535,11 +1531,7 @@ function con_nonlinprogeq!( 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̂ - for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): - x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - end + disturbedinput!(Û0, mpc, estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 717b66b15..d06cc06c2 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -121,25 +121,6 @@ function fs!(x̂0next, estim::StateEstimator, model::SimModel, x̂0) return nothing end -@doc raw""" - f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) -> nothing - -Compute the disturbed input ``\mathbf{û_0}`` of the augmented model from `x̂0` and `u0`. - -It mutates `û0` in place with the following equation: -```math -\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) -``` -where ``\mathbf{C_{s_u}}`` is defined in [`init_estimstoch`](@ref), and ``\mathbf{x_s}`` is -extracted from `x̂0` as the last `estim.nxs` elements. -""" -function f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) - xs = @views x̂0[model.nx+1:end] - mul!(û0, estim.Cs_u, xs) # ys_u = Cs_u*xs - û0 .+= u0 # û0 = u0 + ys_u - return nothing -end - @doc raw""" ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 24418af80..afc9285ac 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -189,13 +189,6 @@ Does nothing since [`InternalModel`](@ref) does not augment the state vector. """ fs!( _ , ::InternalModel, ::SimModel, _ ) = nothing -@doc raw""" - f̂_input!(û0, estim::InternalModel, model::SimModel, x̂0, u0) -> nothing - -Compute `û0 .= u0` since [`InternalModel`](@ref) does not augment the state vector. -""" -f̂_input!(û0, ::InternalModel, ::SimModel, _ , u0) = (û0 .= u0; nothing) - @doc raw""" ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0) From af6f2e67cde8872e29d1ac68502ae2adf5d803d0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 9 Mar 2026 15:21:53 -0400 Subject: [PATCH 04/18] debug: correct arg in `disturbedinput!` --- 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 a616b19cc..6abd3d042 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1412,7 +1412,7 @@ function con_nonlinprogeq!( nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - disturbedinput!(Û0, mpc, estim, U0, X̂0_Z̃) + disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1531,7 +1531,7 @@ function con_nonlinprogeq!( 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̂ - disturbedinput!(Û0, mpc, estim, U0, X̂0_Z̃) + disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] From 075f955c753ec546e9c20862cd0c1cbf8e53e39b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 9 Mar 2026 17:36:09 -0400 Subject: [PATCH 05/18] =?UTF-8?q?changed:=20removed=20`A=5FS=CC=82`=20matr?= =?UTF-8?q?ix=20It=20is=20never=20used,=20since=20the=20same=20as=20`Aeq`?= =?UTF-8?q?=20matrix,=20and=20it=20cause=20confusion=20with=20the=20linear?= =?UTF-8?q?=20defect=20constraint=20of=20`CollocationMethod`s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 20 +++++++++----------- src/controller/execute.jl | 5 ++--- src/controller/transcription.jl | 11 ++++------- 3 files changed, 15 insertions(+), 21 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 1e85626f5..0c93b7b9e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -176,7 +176,6 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # indices of finite numbers in the b vector (linear inequality constraints): i_b ::BitVector # A matrices for the linear equality constraints: - A_Ŝ ::Matrix{NT} Aeq ::Matrix{NT} # b vector for the linear equality constraints: beq ::Vector{NT} @@ -518,7 +517,7 @@ function setconstraint!( con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, con.A_x̂min, con.A_x̂max, - con.A_Ŝ + con.Aeq ) A = con.A[con.i_b, :] b = con.b[con.i_b] @@ -910,7 +909,7 @@ function init_defaultcon_mpc( A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - A_Ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) + Aeq, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) @@ -920,7 +919,7 @@ function init_defaultcon_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, - A_Ŝ + Aeq ) # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) @@ -935,7 +934,6 @@ function init_defaultcon_mpc( A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , A_Ymin , A_Ymax , A_Wmin , A_Wmax , A_x̂min , A_x̂max , A , b , i_b , - A_Ŝ , Aeq , beq , neq , C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , @@ -1194,14 +1192,14 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where end @doc raw""" - augmentdefect(Eŝ, nϵ) -> A_Ŝ, Ẽŝ + augmentdefect(Eŝ, nϵ) -> Aeq, Ẽŝ Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. -It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the defect equation -``\mathbf{Ŝ = Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: +It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the linear defect equation +``\mathbf{Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: ```math -\mathbf{A_Ŝ Z̃} = - \mathbf{F_ŝ} +\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_ŝ} ``` """ function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real @@ -1210,8 +1208,8 @@ function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real else # Z̃ = Z (only hard constraints) Ẽŝ = Eŝ end - A_Ŝ = Ẽŝ - return A_Ŝ, Ẽŝ + Aeq = Ẽŝ + return Aeq, Ẽŝ end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 09b9179dd..3a2d421bc 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -720,7 +720,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.bx̂ .= bx̂ # --- defect matrices --- Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - A_Ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) + Aeq, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) con.Ẽŝ .= Ẽŝ con.Gŝ .= Gŝ con.Jŝ .= Jŝ @@ -747,8 +747,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.A_x̂max ] # --- linear equality constraints --- - con.A_Ŝ .= A_Ŝ - con.Aeq .= A_Ŝ + con.Aeq .= Aeq # --- operating points --- con.U0min .+= mpc.Uop # convert U0 to U with the old operating point con.U0max .+= mpc.Uop # convert U0 to U with the old operating point diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6abd3d042..a9b7f58de 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -786,7 +786,7 @@ The argument `nc` is the number of custom nonlinear inequality constraints in finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a case, `args` needs to contain all the inequality and equality constraint matrices: -`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ`. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, Aeq`. The integer `neq` is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( @@ -803,7 +803,7 @@ function init_matconstraint_mpc( A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, - A_Ŝ + Aeq ) = args A = [ A_Umin; A_Umax; @@ -812,7 +812,6 @@ function init_matconstraint_mpc( A_Wmin; A_Wmax A_x̂min; A_x̂max; ] - Aeq = A_Ŝ neq = 0 end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] @@ -829,9 +828,8 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_Ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] - Aeq = A_Ŝ neq = 0 end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] @@ -848,9 +846,8 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max] - Aeq = A_Ŝ nΔŨ, nZ̃ = size(A_ΔŨmin) neq = nZ̃ - nΔŨ end From b6f0928ca577a3b2b2b129052ae273a809fa69b0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 10:29:31 -0400 Subject: [PATCH 06/18] added: linear eq. constraints with `CollocationMethod` --- src/controller/construct.jl | 3 +- src/controller/transcription.jl | 89 +++++++++++++++++++-------------- 2 files changed, 54 insertions(+), 38 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 0c93b7b9e..c0fa417db 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -881,6 +881,7 @@ function init_defaultcon_mpc( nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ nw = size(Wy, 1) nW = nw*(Hp+1) + nFŝ = size(Eŝ, 1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) @@ -922,7 +923,7 @@ function init_defaultcon_mpc( Aeq ) # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) - fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) + fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nFŝ) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a9b7f58de..e1b4a05c9 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -684,7 +684,7 @@ function init_defectmat( Vŝ = repeat(B̂u, Hp) # --- decision variables Z --- nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) - Eŝ = [zeros(nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] + Eŝ = [zeros(NT, nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] for j=1:Hc iCol = (1:nu) .+ nu*(j-1) for i=j:Hc @@ -700,36 +700,50 @@ function init_defectmat( Eŝ[iRow, iCol] = Â end # --- current measured disturbances d0 and predictions D̂0 --- - Gŝ = [B̂d; zeros(NT, (Hp-1)*nx̂, nd)] - Jŝ = [zeros(nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] + Gŝ = [B̂d; zeros(NT, nx̂*(Hp-1), nd)] + Jŝ = [zeros(NT, nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] # --- state x̂op and state update f̂op operating points --- Bŝ = repeat(estim.f̂op - estim.x̂op, Hp) return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end function init_defectmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ + model::SimModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ ) where {NT<:Real} - nx̂, nu, nd = estim.nx̂, model.nu, model.nd + nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs nZ = get_nZ(estim, transcription, Hp, Hc) - Eŝ = zeros(NT, 0, nZ) - Gŝ = zeros(NT, 0, nd) - Jŝ = zeros(NT, 0, nd*Hp) - Kŝ = zeros(NT, 0, nx̂) - Vŝ = zeros(NT, 0, nu) - Bŝ = zeros(NT, 0) + nK = nZ - nu*Hc - nx̂*Hp + As = estim.As + # --- current state estimates x̂0 --- + Kŝ = zeros(NT, nxs*Hp, nx̂) + Kŝ[1:nxs, nx+1:end] = As + # --- previous manipulated inputs lastu0 --- + Vŝ = zeros(nxs*Hp, nu) + # --- decision variables Z --- + zeros_nI = [zeros(NT, nxs, nx) -I] + Eŝ = [zeros(NT, nxs*Hp, nu*Hc) repeatdiag(zeros_nI, Hp) zeros(NT, nxs*Hp, nK)] + for j=1:Hp-1 + iRow = (1:nxs) .+ nxs*j + iCol = (nx+1:nx̂) .+ nx̂*(j-1) .+ nu*Hc + Eŝ[iRow, iCol] = As + end + # --- current measured disturbances d0 and predictions D̂0 --- + Gŝ = zeros(NT, nxs*Hp, nd) + Jŝ = zeros(NT, nxs*Hp, nd*Hp) + # --- state x̂op and state update f̂op operating points --- + Bŝ = zeros(NT, nxs*Hp) return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end """ init_defectmat( - model::NonLinModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + model::SimModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented). """ function init_defectmat( - model::NonLinModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + model::SimModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) @@ -812,7 +826,7 @@ function init_matconstraint_mpc( A_Wmin; A_Wmax A_x̂min; A_x̂max; ] - neq = 0 + neq = 0 # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = trues(nc) @@ -830,7 +844,7 @@ function init_matconstraint_mpc( else A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] - neq = 0 + neq = 0 # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] @@ -849,7 +863,7 @@ function init_matconstraint_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max] nΔŨ, nZ̃ = size(A_ΔŨmin) - neq = nZ̃ - nΔŨ + neq = nZ̃ - nΔŨ - size(Aeq, 1) # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = [i_Ymin; i_Ymax; trues(nc)] @@ -960,7 +974,7 @@ end @doc raw""" linconstrainteq!( - mpc::PredictiveController, model::LinModel, transcription::MultipleShooting + mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting ) Set `beq` vector for the linear model equality constraints (``\mathbf{A_{eq} Z̃ = b_{eq}}``). @@ -968,7 +982,9 @@ Set `beq` vector for the linear model equality constraints (``\mathbf{A_{eq} Z̃ Also init ``\mathbf{F_ŝ} = \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} + \mathbf{K_ŝ x̂_0}(k) + \mathbf{V_ŝ u_0}(k-1) + \mathbf{B_ŝ}``, see [`init_defectmat`](@ref). """ -function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::MultipleShooting) +function linconstrainteq!( + mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting +) Fŝ = mpc.con.Fŝ Fŝ .= mpc.con.Bŝ mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1) @@ -982,7 +998,20 @@ function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::Multiple JuMP.set_normalized_rhs(linconeq, mpc.con.beq) return nothing end -linconstrainteq!(::PredictiveController, ::SimModel, ::TranscriptionMethod) = nothing + +function linconstrainteq!( + mpc::PredictiveController, ::SimModel, ::StateSpace, ::CollocationMethod +) + Fŝ = mpc.con.Fŝ + # the only non-zeros matrices are Eŝ and Kŝ: + mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0) + mpc.con.beq .= @. -Fŝ + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + return nothing +end +linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::TranscriptionMethod) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod) = nothing @doc raw""" set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s @@ -1420,18 +1449,11 @@ function con_nonlinprogeq!( end k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - sdnext = @views geq[(1 + nx̂*(j-1) ):(nx̂*(j-1) + nx)] - ssnext = @views geq[(1 + nx̂*(j-1) + nx):(nx̂*j )] + sdnext = @views geq[(1 + nx*(j-1) ):(nx*(j-1) + nx)] x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] - # ----------------- stochastic defects ----------------------------------------- - xsnext = @views x̂0next[nx+1:end] - xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] - fs!(x̂0next, mpc.estim, model, x̂0_Z̃) - ssnext .= @. xsnext - xsnext_Z̃ # ----------------- deterministic defects: trapezoidal collocation ------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] if f_threads || h < 1 || j < 2 @@ -1524,7 +1546,7 @@ function con_nonlinprogeq!( no, τ = transcription.no, transcription.τ Mo, Co, λo = mpc.Mo, mpc.Co, mpc.λo nk = get_nk(model, transcription) - nx̂_nk = nx̂ + nk + nx_nk = nx + nk 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̂ @@ -1540,18 +1562,11 @@ function con_nonlinprogeq!( k̇ = @views K̇[(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)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - scnext = @views geq[(1 + nx̂_nk*(j-1) ):(nx̂_nk*(j-1) + nx)] - ssnext = @views geq[(1 + nx̂_nk*(j-1) + nx):(nx̂_nk*(j-1) + nx̂)] - sk = @views geq[(1 + nx̂_nk*(j-1) + nx̂):(nx̂_nk*j )] + scnext = @views geq[(1 + nx_nk*(j-1) ):(nx_nk*(j-1) + nx)] + sk = @views geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j )] x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] - # ----------------- stochastic defects ----------------------------------------- - xsnext = @views x̂0next[nx+1:end] - xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] - fs!(x̂0next, mpc.estim, model, x̂0_Z̃) - ssnext .= @. xsnext - xsnext_Z̃ # ----------------- collocation constraint defects ----------------------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ From 10930e928f21394ad56dc2bf71335822cb61c2b3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 10:35:06 -0400 Subject: [PATCH 07/18] debug: correct signature --- src/controller/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 3a2d421bc..56592b89a 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -74,7 +74,7 @@ function moveinput!( validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) initpred!(mpc, mpc.estim.model, ry, d, lastu, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model, mpc.transcription) - linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) + linconstrainteq!(mpc, mpc.estim.model, mpc.estim, mpc.transcription) Z̃ = optim_objective!(mpc) return getinput!(mpc, Z̃) end From fd86391f6e63592590fa8a3b7cf3ae6cb4edefb7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 10:37:21 -0400 Subject: [PATCH 08/18] added: linear eq. constraint method for `ExplicitMPC` --- src/controller/explicitmpc.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 414155724..8e98ae3b3 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -192,6 +192,7 @@ function Base.show(io::IO, mpc::ExplicitMPC) end linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing +linconstrainteq!(::ExplicitMPC, ::LinMode, ::StateEstimator, ::TranscriptionMethod) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ From 2ba31f9fc47c078b55df16c53e84e4dd56aed6c8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 10:44:50 -0400 Subject: [PATCH 09/18] debug: correct type name --- src/controller/explicitmpc.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 8e98ae3b3..98472134e 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -192,7 +192,7 @@ function Base.show(io::IO, mpc::ExplicitMPC) end linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing -linconstrainteq!(::ExplicitMPC, ::LinMode, ::StateEstimator, ::TranscriptionMethod) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::TranscriptionMethod) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ From 61d335656bec8e5f71767c5346417164e29cb37d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 12:26:07 -0400 Subject: [PATCH 10/18] debug: avoid abiguous methods --- src/controller/explicitmpc.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 98472134e..693b21621 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -191,8 +191,8 @@ function Base.show(io::IO, mpc::ExplicitMPC) print_estim_dim(io, mpc.estim, n) end -linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing -linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::TranscriptionMethod) = nothing +linconstraint!(::ExplicitMPC, ::LinModel, ::SingleShooting) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::SingleShooting) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ From f26d18443252aa92d5d7b34d9b88a35059eb960a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 12:35:10 -0400 Subject: [PATCH 11/18] debug: idem --- src/controller/explicitmpc.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 693b21621..2cf4eec8f 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -193,6 +193,7 @@ end linconstraint!(::ExplicitMPC, ::LinModel, ::SingleShooting) = nothing linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::SingleShooting) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::InternalModel, ::SingleShooting) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ From 84318bf7644b71c12f647a719f06760319651083 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 14:47:52 -0400 Subject: [PATCH 12/18] =?UTF-8?q?changed:=20uppercase=20"S"=20as=20subscri?= =?UTF-8?q?pt=20for=20defect=20matrices=20To=20avoid=20confusion=20with=20?= =?UTF-8?q?the=20matrix=20related=20to=20the=20stochastic=20model=20(lower?= =?UTF-8?q?case=20"s")=20It's=20not=20easy=20avoiding=20greek=20letters=20?= =?UTF-8?q?for=20matrix=20and=20vector=20=F0=9F=AB=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 46 +++++------ src/controller/execute.jl | 16 ++-- src/controller/explicitmpc.jl | 2 +- src/controller/linmpc.jl | 4 +- src/controller/nonlinmpc.jl | 4 +- src/controller/transcription.jl | 139 +++++++++++++------------------- 6 files changed, 92 insertions(+), 119 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index c0fa417db..d9002cd3c 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -133,13 +133,13 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} vx̂ ::Matrix{NT} bx̂ ::Vector{NT} # matrices for the zero defect constraints (N/A for single shooting transcriptions): - Ẽŝ ::Matrix{NT} - Fŝ ::Vector{NT} - Gŝ ::Matrix{NT} - Jŝ ::Matrix{NT} - Kŝ ::Matrix{NT} - Vŝ ::Matrix{NT} - Bŝ ::Vector{NT} + ẼS ::Matrix{NT} + FS ::Vector{NT} + GS ::Matrix{NT} + JS ::Matrix{NT} + KS ::Matrix{NT} + VS ::Matrix{NT} + BS ::Vector{NT} # custom linear equality constraints: Ẽw ::Matrix{NT} Fw ::Vector{NT} @@ -857,7 +857,7 @@ verify_cond(::TranscriptionMethod,_,_) = nothing Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -873,7 +873,7 @@ function init_defaultcon_mpc( Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} @@ -881,7 +881,7 @@ function init_defaultcon_mpc( nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ nw = size(Wy, 1) nW = nw*(Hp+1) - nFŝ = size(Eŝ, 1) + nS = size(ES, 1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) @@ -910,7 +910,7 @@ function init_defaultcon_mpc( A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - Aeq, Ẽŝ = augmentdefect(Eŝ, nϵ) + Aeq, ẼS = augmentdefect(ES, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) @@ -922,13 +922,13 @@ function init_defaultcon_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, Aeq ) - # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) - fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nFŝ) + # dummy fx̂, Fw and FS vectors (updated just before optimization) + fx̂, Fw, FS = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nS) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , - Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + ẼS , FS , GS , JS , KS , VS , BS , Ẽw , Fw , W̄y , W̄u , W̄d , W̄r , nw , U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , @@ -1193,24 +1193,24 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where end @doc raw""" - augmentdefect(Eŝ, nϵ) -> Aeq, Ẽŝ + augmentdefect(ES, nϵ) -> Aeq, ẼS Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. -It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the linear defect equation -``\mathbf{Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: +It returns the ``\mathbf{Ẽ_S}`` matrix that appears in the linear defect equation +``\mathbf{Ẽ_S Z̃ + F_S}`` and the ``\mathbf{A}`` matrix for the equality constraints: ```math -\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_ŝ} +\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S} ``` """ -function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real +function augmentdefect(ES::AbstractMatrix{NT}, nϵ) where NT<:Real if nϵ == 1 # Z̃ = [Z; ϵ] - Ẽŝ = [Eŝ zeros(NT, size(Eŝ, 1), 1)] + ẼS = [ES zeros(NT, size(ES, 1), 1)] else # Z̃ = Z (only hard constraints) - Ẽŝ = Eŝ + ẼS = ES end - Aeq = Ẽŝ - return Aeq, Ẽŝ + Aeq = ẼS + return Aeq, ẼS end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 56592b89a..cf49cfa58 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -719,14 +719,14 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.vx̂ .= vx̂ con.bx̂ .= bx̂ # --- defect matrices --- - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - Aeq, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) - con.Ẽŝ .= Ẽŝ - con.Gŝ .= Gŝ - con.Jŝ .= Jŝ - con.Kŝ .= Kŝ - con.Vŝ .= Vŝ - con.Bŝ .= Bŝ + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) + Aeq, ẼS = augmentdefect(ES, mpc.nϵ) + con.ẼS .= ẼS + con.GS .= GS + con.JS .= JS + con.KS .= KS + con.VS .= VS + con.BS .= BS # --- custom linear constraints --- con.Ẽw .= Ẽw # --- linear inequality constraints --- diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 2cf4eec8f..eff16ef88 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -193,7 +193,7 @@ end linconstraint!(::ExplicitMPC, ::LinModel, ::SingleShooting) = nothing linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::SingleShooting) = nothing -linconstrainteq!(::ExplicitMPC, ::LinModel, ::InternalModel, ::SingleShooting) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::InternalModel, ::SingleShooting) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 47424d799..a394ab56b 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -73,13 +73,13 @@ struct LinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 41f481a81..519d31abc 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -103,13 +103,13 @@ struct NonLinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!, nc ) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e1b4a05c9..f4f3c2914 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -600,24 +600,24 @@ end @doc raw""" init_defectmat( model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb - ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ) -> ES, GS, JS, KS, VS, BS Init the matrices for computing the defects over the predicted states. Knowing that the decision vector ``\mathbf{Z}`` contains both ``\mathbf{ΔU}`` and ``\mathbf{X̂_0}`` vectors (with a [`MultipleShooting`](@ref) transcription), an equation -similar to the prediction matrices (see [`init_predmat`](@ref)) computes the defects over -the predicted states: +similar to the prediction matrices (see [`init_predmat`](@ref)) computes the defects of +the estimated states of ``H_p``: ```math \begin{aligned} - \mathbf{Ŝ} &= \mathbf{E_ŝ Z} + \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} - + \mathbf{K_ŝ x̂_0}(k) + \mathbf{V_ŝ u_0}(k-1) - + \mathbf{B_ŝ} \\ - &= \mathbf{E_ŝ Z} + \mathbf{F_ŝ} + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + + \mathbf{K_S x̂_0}(k) + \mathbf{V_S u_0}(k-1) + + \mathbf{B_S} \\ + &= \mathbf{E_S Z} + \mathbf{F_S} \end{aligned} ``` They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality constraints. The -matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in the Extended Help section. +matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended Help section. # Extended Help !!! details "Extended Help" @@ -634,34 +634,34 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th The defect matrices are computed with: ```math \begin{aligned} - \mathbf{E_ŝ} &= \begin{bmatrix} - \mathbf{E_{ŝ}^{Δu}} & \mathbf{E_{ŝ}^{x̂}} \end{bmatrix} \\ - \mathbf{E_{ŝ}^{Δu}} &= \begin{bmatrix} + \mathbf{E_S} &= \begin{bmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{bmatrix} \\ + \mathbf{E_{S}^{Δu}} &= \begin{bmatrix} \mathbf{Q}(n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Q}(n_2) & \mathbf{Q}(n_2) & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ - \mathbf{E_{ŝ}^{x̂}} &= \begin{bmatrix} + \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ - \mathbf{G_ŝ} &= \begin{bmatrix} + \mathbf{G_S} &= \begin{bmatrix} \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ - \mathbf{J_ŝ} &= \begin{bmatrix} + \mathbf{J_S} &= \begin{bmatrix} \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ - \mathbf{K_ŝ} &= \begin{bmatrix} + \mathbf{K_S} &= \begin{bmatrix} \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ - \mathbf{V_ŝ} &= \begin{bmatrix} + \mathbf{V_S} &= \begin{bmatrix} \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \vdots \\ \mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{B_ŝ} &= \begin{bmatrix} + \mathbf{B_S} &= \begin{bmatrix} \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} \end{aligned} ``` - The ``\mathbf{E_ŝ^{Δu}}`` matrix structure is due to the move blocking implementation: + The ``\mathbf{E_S^{Δu}}`` matrix structure is due to the move blocking implementation: the ``\mathbf{ΔU}`` vector only contains the input increment of the free moves (see [`move_blocking`](@ref)). """ @@ -679,66 +679,39 @@ function init_defectmat( return Q end # --- current state estimates x̂0 --- - Kŝ = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] + KS = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] # --- previous manipulated inputs lastu0 --- - Vŝ = repeat(B̂u, Hp) + VS = repeat(B̂u, Hp) # --- decision variables Z --- nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) - Eŝ = [zeros(NT, nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] + ES = [zeros(NT, nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] for j=1:Hc iCol = (1:nu) .+ nu*(j-1) for i=j:Hc ni = nb[i] iRow = (1:nx̂*ni) .+ nx̂*sum(nb[1:i-1]) - Q = @views Eŝ[iRow, iCol] + Q = @views ES[iRow, iCol] Q!(Q, ni) end end for j=1:Hp-1 iRow = (1:nx̂) .+ nx̂*j iCol = (1:nx̂) .+ nx̂*(j-1) .+ nu*Hc - Eŝ[iRow, iCol] = Â + ES[iRow, iCol] = Â end # --- current measured disturbances d0 and predictions D̂0 --- - Gŝ = [B̂d; zeros(NT, nx̂*(Hp-1), nd)] - Jŝ = [zeros(NT, nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] + GS = [B̂d; zeros(NT, nx̂*(Hp-1), nd)] + JS = [zeros(NT, nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] # --- state x̂op and state update f̂op operating points --- - Bŝ = repeat(estim.f̂op - estim.x̂op, Hp) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + BS = repeat(estim.f̂op - estim.x̂op, Hp) + return ES, GS, JS, KS, VS, BS end -function init_defectmat( - model::SimModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ -) where {NT<:Real} - nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs - nZ = get_nZ(estim, transcription, Hp, Hc) - nK = nZ - nu*Hc - nx̂*Hp - As = estim.As - # --- current state estimates x̂0 --- - Kŝ = zeros(NT, nxs*Hp, nx̂) - Kŝ[1:nxs, nx+1:end] = As - # --- previous manipulated inputs lastu0 --- - Vŝ = zeros(nxs*Hp, nu) - # --- decision variables Z --- - zeros_nI = [zeros(NT, nxs, nx) -I] - Eŝ = [zeros(NT, nxs*Hp, nu*Hc) repeatdiag(zeros_nI, Hp) zeros(NT, nxs*Hp, nK)] - for j=1:Hp-1 - iRow = (1:nxs) .+ nxs*j - iCol = (nx+1:nx̂) .+ nx̂*(j-1) .+ nu*Hc - Eŝ[iRow, iCol] = As - end - # --- current measured disturbances d0 and predictions D̂0 --- - Gŝ = zeros(NT, nxs*Hp, nd) - Jŝ = zeros(NT, nxs*Hp, nd*Hp) - # --- state x̂op and state update f̂op operating points --- - Bŝ = zeros(NT, nxs*Hp) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ -end """ init_defectmat( model::SimModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ - ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ) -> ES, GS, JS, KS, VS, BS Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented). """ @@ -747,19 +720,19 @@ function init_defectmat( ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) - Eŝ = zeros(NT, 0, nZ) - Gŝ = zeros(NT, 0, nd) - Jŝ = zeros(NT, 0, nd*Hp) - Kŝ = zeros(NT, 0, nx̂) - Vŝ = zeros(NT, 0, nu) - Bŝ = zeros(NT, 0) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ES = zeros(NT, 0, nZ) + GS = zeros(NT, 0, nd) + JS = zeros(NT, 0, nd*Hp) + KS = zeros(NT, 0, nx̂) + VS = zeros(NT, 0, nu) + BS = zeros(NT, 0) + return ES, GS, JS, KS, VS, BS end """ init_defectmat( model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb - ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ) -> ES, GS, JS, KS, VS, BS Return empty matrices for all other cases (N/A). """ @@ -768,13 +741,13 @@ function init_defectmat( ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) - Eŝ = zeros(NT, 0, nZ) - Gŝ = zeros(NT, 0, nd) - Jŝ = zeros(NT, 0, nd*Hp) - Kŝ = zeros(NT, 0, nx̂) - Vŝ = zeros(NT, 0, nu) - Bŝ = zeros(NT, 0) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ES = zeros(NT, 0, nZ) + GS = zeros(NT, 0, nd) + JS = zeros(NT, 0, nd*Hp) + KS = zeros(NT, 0, nx̂) + VS = zeros(NT, 0, nu) + BS = zeros(NT, 0) + return ES, GS, JS, KS, VS, BS end @doc raw""" @@ -979,21 +952,21 @@ end Set `beq` vector for the linear model equality constraints (``\mathbf{A_{eq} Z̃ = b_{eq}}``). -Also init ``\mathbf{F_ŝ} = \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} + \mathbf{K_ŝ x̂_0}(k) + -\mathbf{V_ŝ u_0}(k-1) + \mathbf{B_ŝ}``, see [`init_defectmat`](@ref). +Also init ``\mathbf{F_S} = \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + \mathbf{K_S x̂_0}(k) + +\mathbf{V_S u_0}(k-1) + \mathbf{B_S}``, see [`init_defectmat`](@ref). """ function linconstrainteq!( mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting ) - Fŝ = mpc.con.Fŝ - Fŝ .= mpc.con.Bŝ - mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1) - mul!(Fŝ, mpc.con.Vŝ, mpc.lastu0, 1, 1) + FS = mpc.con.FS + FS .= mpc.con.BS + mul!(FS, mpc.con.KS, mpc.estim.x̂0, 1, 1) + mul!(FS, mpc.con.VS, mpc.lastu0, 1, 1) if model.nd > 0 - mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1) - mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1) + mul!(FS, mpc.con.GS, mpc.d0, 1, 1) + mul!(FS, mpc.con.JS, mpc.D̂0, 1, 1) end - mpc.con.beq .= @. -Fŝ + mpc.con.beq .= @. -FS linconeq = mpc.optim[:linconstrainteq] JuMP.set_normalized_rhs(linconeq, mpc.con.beq) return nothing @@ -1002,10 +975,10 @@ end function linconstrainteq!( mpc::PredictiveController, ::SimModel, ::StateSpace, ::CollocationMethod ) - Fŝ = mpc.con.Fŝ - # the only non-zeros matrices are Eŝ and Kŝ: - mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0) - mpc.con.beq .= @. -Fŝ + FS = mpc.con.FS + # the only non-zeros matrices are ES and KS: + mul!(FS, mpc.con.KS, mpc.estim.x̂0) + mpc.con.beq .= @. -FS linconeq = mpc.optim[:linconstrainteq] JuMP.set_normalized_rhs(linconeq, mpc.con.beq) return nothing From c9df7fbed657cf17a4b8f19d1819b7ce1756cabc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 15:39:04 -0400 Subject: [PATCH 13/18] doc: remove stochastic defects in `con_nonlinprogeq!` --- src/controller/transcription.jl | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f4f3c2914..c12e8486c 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1375,20 +1375,16 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). The method mutates the `geq`, `X̂0`, `Û0` and `K̇` vectors in argument. The nonlinear equality -constraints `geq` only includes the state defects. The deterministic and stochastic states -are handled separately since collocation methods require continuous-time state-space models, -and the stochastic model of the unmeasured disturbances is discrete-time. The deterministic -and stochastic defects are respectively computed with: +constraints `geq` includes the defects of the deterministic states only. The stochastic +states are handled seperatly as linear equality constraints, see [`init_defectmat`](@ref). +The deterministic state defects are computed with: ```math -\begin{aligned} -\mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] - - \mathbf{x_0}(k+j+1) \\ -\mathbf{s_s}(k+j+1) &= \mathbf{A_s x_s}(k+j) - \mathbf{x_s}(k+j+1) -\end{aligned} +\mathbf{s_d}(k+j+1) = \mathbf{x_0}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] + - \mathbf{x_0}(k+j+1) ``` -for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x_0}`` and ``\mathbf{x_s}`` are the -deterministic and stochastic states extracted from the decision variables `Z̃`. The -``\mathbf{k̇}`` coefficients are evaluated from the continuous-time function `model.f!` and: +for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x_0}`` is the deterministic state +extracted from the decision variables `Z̃`. The ``\mathbf{k̇}`` coefficients are evaluated +from the continuous-time function `model.f!` and: ```math \begin{aligned} \mathbf{k̇}_1(k+j) &= \mathbf{f}\Big(\mathbf{x_0}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ @@ -1427,7 +1423,6 @@ function con_nonlinprogeq!( x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] - # ----------------- deterministic defects: trapezoidal collocation ------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the @@ -1491,9 +1486,9 @@ and disturbances are piecewise constant or linear: \mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) \end{aligned} ``` -The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The stochastic state -defects ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) method, and -the ones for the continuity constraint of the deterministic states are: +The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The defects of the +stochastic states are linear equality constraints (see [`init_defectmat`](@ref)), and the +ones for the continuity constraint of the deterministic states are: ```math \mathbf{s_c}(k+j+1) = \mathbf{C_o} \begin{bmatrix} From d81dca9500b08636e04da4d69cecc095248f616e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 16:21:44 -0400 Subject: [PATCH 14/18] doc: stochastic defect matrices --- src/controller/transcription.jl | 78 +++++++++++++++++++++++++++++++-- 1 file changed, 74 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c12e8486c..a66aecd61 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -642,10 +642,10 @@ matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended H \vdots & \vdots & \ddots & \vdots \\ \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} - -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ + -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ \mathbf{G_S} &= \begin{bmatrix} \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ \mathbf{J_S} &= \begin{bmatrix} @@ -707,6 +707,76 @@ function init_defectmat( return ES, GS, JS, KS, VS, BS end +@doc raw""" + init_defectmat( + model::SimModel, estim::StateEstimator, transcription::CollocationMethod, Hp, Hc, _ + ) + +Init the matrices for computing the defects of the stochastic states only. + +The documentation of [`init_estimstoch`](@ref) shows that the stochastic model of the +unmeasured disturbances is linear and discrete-time. The defect of the stochastic states +over ``H_p`` is therefore: +```math +\begin{aligned} + \mathbf{Ŝ_s} &= \mathbf{E_S Z} + \mathbf{K_S x̂_0}(k) \\ + &= \mathbf{E_S Z} + \mathbf{F_S} +\end{aligned} +``` +The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + Using the stochastic matrix ``\mathbf{A_s}`` in `estim` (see [`init_estimstoch`](@ref)), + the defect matrices are computed with: + ```math + \begin{aligned} + \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}^{k}} &= \mathbf{0} \\ + \mathbf{K_S} &= \begin{bmatrix} + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} + \end{aligned} + ``` + and: + - if `transcription` is an [`OrthogonalCollocation`](@ref), ``\mathbf{E_S} = [\begin{smallmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k}} \end{smallmatrix}]`` + - else ``\mathbf{E_S} = [\begin{smallmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{smallmatrix}]`` +""" +function init_defectmat( + model::SimModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ +) where {NT<:Real} + nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs + nZ = get_nZ(estim, transcription, Hp, Hc) + nK = nZ - nu*Hc - nx̂*Hp + As = estim.As + # --- current state estimates x̂0 --- + KS = zeros(NT, nxs*Hp, nx̂) + KS[1:nxs, nx+1:end] = As + # --- previous manipulated inputs lastu0 --- + VS = zeros(nxs*Hp, nu) + # --- decision variables Z --- + zeros_nI = [zeros(NT, nxs, nx) -I] + ES = [zeros(NT, nxs*Hp, nu*Hc) repeatdiag(zeros_nI, Hp) zeros(NT, nxs*Hp, nK)] + for j=1:Hp-1 + iRow = (1:nxs) .+ nxs*j + iCol = (nx+1:nx̂) .+ nx̂*(j-1) .+ nu*Hc + ES[iRow, iCol] = As + end + # --- current measured disturbances d0 and predictions D̂0 --- + GS = zeros(NT, nxs*Hp, nd) + JS = zeros(NT, nxs*Hp, nd*Hp) + # --- state x̂op and state update f̂op operating points --- + BS = zeros(NT, nxs*Hp) + return ES, GS, JS, KS, VS, BS +end """ init_defectmat( From 061c1b0e35bb49ecda8c531ef251bf76263ca150 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 16:24:26 -0400 Subject: [PATCH 15/18] test: debug nonlineal eq. constraint checkup --- 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 4d7196f46..e14358983 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -853,7 +853,7 @@ end nmpc16 = NonLinMPC(nonlinmodel_c, Hp=10, transcription=TrapezoidalCollocation(), nc=10, gc=gc!) @test nmpc16.transcription == TrapezoidalCollocation() @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ - @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp + @test nmpc16.con.neq == nmpc16.estim.nx*nmpc16.Hp @test nmpc16.con.nc == 10 nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), From 13fbba7bef66ec6018d588377629a87b40ebf63d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 17:09:20 -0400 Subject: [PATCH 16/18] debug: idem --- 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 e14358983..31b6a46bf 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -853,7 +853,7 @@ end nmpc16 = NonLinMPC(nonlinmodel_c, Hp=10, transcription=TrapezoidalCollocation(), nc=10, gc=gc!) @test nmpc16.transcription == TrapezoidalCollocation() @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ - @test nmpc16.con.neq == nmpc16.estim.nx*nmpc16.Hp + @test nmpc16.con.neq == nmpc16.estim.model.nx*nmpc16.Hp @test nmpc16.con.nc == 10 nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), From 3e82e47c7579e0e1db0993f183b8a370259be033 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 17:52:27 -0400 Subject: [PATCH 17/18] debug: correct dispathc type in `linconstrainteq!` --- 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 a66aecd61..266e83a82 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1043,7 +1043,7 @@ function linconstrainteq!( end function linconstrainteq!( - mpc::PredictiveController, ::SimModel, ::StateSpace, ::CollocationMethod + mpc::PredictiveController, ::SimModel, ::StateEstimator, ::CollocationMethod ) FS = mpc.con.FS # the only non-zeros matrices are ES and KS: From c970f137510d7b994138a0d0c053d0358eeddb65 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Mar 2026 19:51:38 -0400 Subject: [PATCH 18/18] debug: avoid abiguous methods --- 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 266e83a82..e53b07d38 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1053,7 +1053,7 @@ function linconstrainteq!( JuMP.set_normalized_rhs(linconeq, mpc.con.beq) return nothing end -linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::TranscriptionMethod) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::CollocationMethod ) = nothing linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod) = nothing @doc raw"""