From 9b0bda70fe018070950ec7a2da7a356a0e2588f7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Feb 2026 14:40:30 -0500 Subject: [PATCH 01/49] added: `OrthogonalCollocation` type --- docs/src/public/predictive_control.md | 8 +++++++- src/controller/transcription.jl | 21 +++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/docs/src/public/predictive_control.md b/docs/src/public/predictive_control.md index 4250a4b14..6cbcb8945 100644 --- a/docs/src/public/predictive_control.md +++ b/docs/src/public/predictive_control.md @@ -120,4 +120,10 @@ MultipleShooting ```@docs TrapezoidalCollocation -``` \ No newline at end of file +``` + +### OrthogonalCollocation + +```@docs +OrthogonalCollocation +``` diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d18d4365d..0db66dcaf 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -116,6 +116,27 @@ struct TrapezoidalCollocation <: CollocationMethod end end + +@doc raw""" + OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) + +Construct an orthogonal collocation [`TranscriptionMethod`](@ref) with `h`th order hold. + + +""" +struct OrthogonalCollocation <: CollocationMethod + h:Int + nc::Int + f_threads::Bool + h_threads::Bool + function OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_thread=false) + if !(h == 0 || h == 1) + throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + end + return new(h, nc, ) + end +end + function validate_transcription(::LinModel, ::CollocationMethod) throw(ArgumentError("Collocation methods are not supported for LinModel.")) return nothing From 5a66aaeb3bb9f570d919d2e0a723d4b6973bf4a3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Feb 2026 14:57:24 -0500 Subject: [PATCH 02/49] debug: syntax for `OrthogonalCollocation` type --- src/ModelPredictiveControl.jl | 3 ++- src/controller/transcription.jl | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 61fdcf537..c8fe2c32b 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -48,7 +48,8 @@ export MovingHorizonEstimator export ManualEstimator export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! -export TranscriptionMethod, SingleShooting, MultipleShooting, TrapezoidalCollocation +export TranscriptionMethod, SingleShooting, MultipleShooting +export TrapezoidalCollocation, OrthogonalCollocation export SimResult, getinfo, sim! include("general.jl") diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0db66dcaf..52d789c64 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -125,15 +125,17 @@ Construct an orthogonal collocation [`TranscriptionMethod`](@ref) with `h`th ord """ struct OrthogonalCollocation <: CollocationMethod - h:Int + h::Int nc::Int f_threads::Bool h_threads::Bool - function OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_thread=false) + function OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) end - return new(h, nc, ) + if nc>5 + throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + return new(h, nc, f_threads, h_threads) end end From 5376c1b48fa0e911dabcdde3f7e38dd9eb1a6a58 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 10 Feb 2026 16:34:58 -0500 Subject: [PATCH 03/49] debug: forgot `end` --- src/controller/transcription.jl | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 52d789c64..cc0fb2f68 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -120,9 +120,7 @@ end @doc raw""" OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) -Construct an orthogonal collocation [`TranscriptionMethod`](@ref) with `h`th order hold. - - +Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). """ struct OrthogonalCollocation <: CollocationMethod h::Int @@ -131,10 +129,11 @@ struct OrthogonalCollocation <: CollocationMethod h_threads::Bool function OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) - throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end if nc>5 - throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + throw(ArgumentError("nc argument must be ≤ 5 for OrthogonalCollocation.")) + end return new(h, nc, f_threads, h_threads) end end From 0cb98e677d1fa478ea4caab95d801b72a6517af9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 11 Feb 2026 09:19:17 -0500 Subject: [PATCH 04/49] added: `FastGaussQuadrature` dependency --- Project.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Project.toml b/Project.toml index b69146f17..a0b06cedf 100644 --- a/Project.toml +++ b/Project.toml @@ -6,6 +6,7 @@ authors = ["Francis Gagnon"] [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DifferentiationInterface = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" +FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" @@ -33,6 +34,7 @@ ControlSystemsBase = "1.18.2" DAQP = "0.6, 0.7.1" DifferentiationInterface = "0.7.11" Documenter = "1" +FastGaussQuadrature = "1.1.0" FiniteDiff = "2" ForwardDiff = "0.10, 1" Ipopt = "1" From 64e9adff96e170e4ddda5ed95b40fc4adbb150e8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 12 Feb 2026 13:10:45 -0500 Subject: [PATCH 05/49] wip: `OrthgonalCollation` --- src/controller/transcription.jl | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index cc0fb2f68..18c340014 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -121,6 +121,13 @@ end OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). + +The decision variable includes the collocations points (excluding ``ϵ``): + +``math +\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} +`` + """ struct OrthogonalCollocation <: CollocationMethod h::Int From 7e965756725352d4d79b4f6c34e7350bd14de650 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Feb 2026 11:39:40 -0500 Subject: [PATCH 06/49] changed: renamed `nc` to `np` `nc` was the number of collocation points. I renamed it to `np` since there is confusion with `mpc.con.nc`, the number of custom inequality constraints --- src/controller/transcription.jl | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 18c340014..6cf60e1d1 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -104,44 +104,42 @@ transcription method. """ struct TrapezoidalCollocation <: CollocationMethod h::Int - nc::Int + np::Int f_threads::Bool h_threads::Bool function TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) end - nc = 2 # 2 collocation points per interval for trapezoidal rule - return new(h, nc, f_threads, h_threads) + np = 2 # 2 collocation points per intervals for trapezoidal rule + return new(h, np, f_threads, h_threads) end end @doc raw""" - OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) + OrthogonalCollocation(h::Int=0, np=5; f_threads=false, h_threads=false) Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). The decision variable includes the collocations points (excluding ``ϵ``): -``math +```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} -`` +``` +where ``\mathbf{K}`` includes all the (intermediate) collocation points. """ struct OrthogonalCollocation <: CollocationMethod h::Int - nc::Int + np::Int f_threads::Bool h_threads::Bool - function OrthogonalCollocation(h::Int=0, nc=5; f_threads=false, h_threads=false) + function OrthogonalCollocation(h::Int=0, np=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end - if nc>5 - throw(ArgumentError("nc argument must be ≤ 5 for OrthogonalCollocation.")) - end - return new(h, nc, f_threads, h_threads) + return new(h, np, f_threads, h_threads) end end @@ -165,7 +163,7 @@ end "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." get_nk(model::SimModel, ::ShootingMethod) = model.nk -get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.nc +get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.np @doc raw""" init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu From dc4f555043edf0c5a98799210a2397891eddab5f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 09:41:32 -0500 Subject: [PATCH 07/49] changed: renamed `np` to `no` The subscript p is already used for `Hp` i.e. "prediction". --- src/controller/transcription.jl | 43 ++++++++++++++++++++++++--------- 1 file changed, 32 insertions(+), 11 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6cf60e1d1..d568dd8c3 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -104,42 +104,60 @@ transcription method. """ struct TrapezoidalCollocation <: CollocationMethod h::Int - np::Int + no::Int f_threads::Bool h_threads::Bool function TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) end - np = 2 # 2 collocation points per intervals for trapezoidal rule - return new(h, np, f_threads, h_threads) + no = 2 # 2 collocation points per intervals for trapezoidal rule + return new(h, no, f_threads, h_threads) end end @doc raw""" - OrthogonalCollocation(h::Int=0, np=5; f_threads=false, h_threads=false) + OrthogonalCollocation(h::Int=0, no=5; f_threads=false, h_threads=false) Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). -The decision variable includes the collocations points (excluding ``ϵ``): - +The `h` argument is the hold order for ``\mathbf{u}``, and `no`, the number of collocation +points. The decision variable is similar to [`MultipleShooting`](@ref), but it also includes +the collocation points (excluding ``ϵ``): ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` -where ``\mathbf{K}`` includes all the (intermediate) collocation points. +where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic state only: +```math +\mathbf{K} = \begin{bmatrix} + \mathbf{k}_{1}(k+0) \\ + \mathbf{k}_{2}(k+0) \\ + \vdots \\ + \mathbf{k}_{n_p}(k+0) \\ + \mathbf{k}_{1}(k+1) \\ + \mathbf{k}_{2}(k+1) \\ + \vdots \\ + \mathbf{k}_{n_p}(k+H_p) \end{bmatrix} +``` +and ``\mathbf{k}_p(k+j)`` is the deterministic state prediction for the ``p``th collocation +point at the ``j``th stage/iterval (details in Extended Help). +# Extended Help +!!! details "Extended Help" + See the Extended Help of [`TrapezoidalCollocation`](@ref) to understand why the + stochastic states are left out of the ``\mathbf{K}`` vector. """ struct OrthogonalCollocation <: CollocationMethod h::Int - np::Int + no::Int f_threads::Bool h_threads::Bool - function OrthogonalCollocation(h::Int=0, np=5; f_threads=false, h_threads=false) + function OrthogonalCollocation(h::Int=0, no=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end - return new(h, np, f_threads, h_threads) + return new(h, no, f_threads, h_threads) end end @@ -160,10 +178,13 @@ end function get_nZ(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp end +function get_nZ(estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc) + return estim.model.nu*Hc + estim.nx̂*Hp + estim.model.nx*transcription.no*Hp +end "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." get_nk(model::SimModel, ::ShootingMethod) = model.nk -get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.np +get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no @doc raw""" init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu From 9b620d1f3da3bb9df050c947e007779716890290 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 09:53:32 -0500 Subject: [PATCH 08/49] =?UTF-8?q?changed:=20moving=20`init=5FZto=CE=94U`?= =?UTF-8?q?=20to=20`construct.jl`=20A=20dispatch=20was=20not=20necessary?= =?UTF-8?q?=20for=20this.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 31 +++++++++++++++++++++++ src/controller/transcription.jl | 44 +++------------------------------ 2 files changed, 35 insertions(+), 40 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 8b32bac3b..e8abf3a2f 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -692,6 +692,37 @@ function validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) end +@doc raw""" + init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu + +Init decision variables to input increments over ``H_c`` conversion matrix `PΔu`. + +The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input +increments over ``H_c``, is computed by: +```math +\mathbf{ΔU} = \mathbf{P_{Δu}} \mathbf{Z} +``` + +in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + Following the decision variable definition of the [`TranscriptionMethod`](@ref), the + conversion matrix ``\mathbf{P_{Δu}}``, we have: + - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref) + - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise. + The matrix is store as as `SparseMatrixCSC` to support both cases efficiently. +""" +function init_ZtoΔU( + estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc +) where {NT<:Real} + I_nu_Hc = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)) + nZ = get_nZ(estim, transcription, Hp, Hc) + nΔU = estim.model.nu*Hc + PΔu = [I_nu_Hc spzeros(NT, nΔU, nZ - nΔU)] + return PΔu +end + @doc raw""" init_quadprog( model::LinModel, transcriptions::TranscriptionMethod, weights::ControllerWeights, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d568dd8c3..0ec15ac7c 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -186,44 +186,6 @@ end get_nk(model::SimModel, ::ShootingMethod) = model.nk get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no -@doc raw""" - init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu - -Init decision variables to input increments over ``H_c`` conversion matrix `PΔu`. - -The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input -increments over ``H_c``, is computed by: -```math -\mathbf{ΔU} = \mathbf{P_{Δu}} \mathbf{Z} -``` - -in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. - -# Extended Help -!!! details "Extended Help" - Following the decision variable definition of the [`TranscriptionMethod`](@ref), the - conversion matrix ``\mathbf{P_{Δu}}``, we have: - - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref) - - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise. - The matrix is store as as `SparseMatrixCSC` to support both cases efficiently. -""" -function init_ZtoΔU end - -function init_ZtoΔU( - estim::StateEstimator{NT}, ::SingleShooting, _ , Hc -) where {NT<:Real} - PΔu = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)) - return PΔu -end - -function init_ZtoΔU( - estim::StateEstimator{NT}, ::TranscriptionMethod, Hp, Hc -) where {NT<:Real} - I_nu_Hc = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)) - PΔu = [I_nu_Hc spzeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)] - return PΔu -end - @doc raw""" init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu @@ -295,9 +257,11 @@ function init_PUmat(_,::SingleShooting,_,_,PuDagger::AbstractMatrix{NT}) where N return PuDagger end function init_PUmat( - estim, ::TranscriptionMethod, Hp, _ , PuDagger::AbstractMatrix{NT} + estim, transcription::TranscriptionMethod, Hp, Hc, PuDagger::AbstractMatrix{NT} ) where NT<:Real - return [PuDagger spzeros(NT, estim.model.nu*Hp, estim.nx̂*Hp)] + nΔU = estim.model.nu*Hc + nZ = get_nZ(estim, transcription, Hp, Hc) + return [PuDagger spzeros(NT, nΔU, nZ - nΔU)] end @doc raw""" From 907d3ec83975c10fcc95a0b0e3f4996101eac323 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 09:55:27 -0500 Subject: [PATCH 09/49] changed: same for `init_ZtoU` --- src/controller/construct.jl | 69 +++++++++++++++++++++++++++++ src/controller/transcription.jl | 78 --------------------------------- 2 files changed, 69 insertions(+), 78 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index e8abf3a2f..dcbf20a48 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -723,6 +723,75 @@ function init_ZtoΔU( return PΔu end +@doc raw""" + init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu + +Init decision variables to inputs over ``H_p`` conversion matrices. + +The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{U}``, the manipulated +inputs over ``H_p``, is computed by: +```math +\mathbf{U} = \mathbf{P_u} \mathbf{Z} + \mathbf{T_u} \mathbf{u}(k-1) +``` +The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + With ``n_i``, the ``i``th element of the ``\mathbf{n_b}`` vector defined in [`move_blocking`](@ref) + documentation, we introduce the ``\mathbf{Q}(n_i)`` matrix of size `(nu*ni, nu)`: + ```math + \mathbf{Q}(n_i) = \begin{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \end{bmatrix} + ``` + The ``\mathbf{U}`` vector and the conversion matrices are defined as: + ```math + \mathbf{U} = \begin{bmatrix} + \mathbf{u}(k + 0) \\ + \mathbf{u}(k + 1) \\ + \vdots \\ + \mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad + \mathbf{P_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} , \quad + \mathbf{T_u} = \begin{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \end{bmatrix} + ``` + and, depending on the transcription method, we have: + - ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref) + - ``\mathbf{P_u} = [\begin{smallmatrix}\mathbf{P_u^†} & \mathbf{0} \end{smallmatrix}]`` + if `transcription` is a [`MultipleShooting`](@ref) + The conversion matrices are stored as `SparseMatrixCSC` since it was benchmarked that it + is generally more performant than normal dense matrices, even for small `nu`, `Hp` and + `Hc` values. Using `Bool` element type and `BitMatrix` is also slower. +""" +function init_ZtoU( + estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb +) where {NT<:Real} + nu = estim.model.nu + I_nu = sparse(Matrix{NT}(I, nu, nu)) + PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc) + for i=1:Hc + ni = nb[i] + Q_ni = repeat(I_nu, ni, 1) + iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1]) + PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))] + end + PuDagger = sparse(PuDagger) + nΔU = estim.model.nu*Hc + nZ = get_nZ(estim, transcription, Hp, Hc) + Pu = [PuDagger spzeros(NT, nΔU, nZ - nΔU)] + Tu = repeat(I_nu, Hp) + return Pu, Tu +end + @doc raw""" init_quadprog( model::LinModel, transcriptions::TranscriptionMethod, weights::ControllerWeights, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0ec15ac7c..c4804a467 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -186,84 +186,6 @@ end get_nk(model::SimModel, ::ShootingMethod) = model.nk get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no -@doc raw""" - init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu - -Init decision variables to inputs over ``H_p`` conversion matrices. - -The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{U}``, the manipulated -inputs over ``H_p``, is computed by: -```math -\mathbf{U} = \mathbf{P_u} \mathbf{Z} + \mathbf{T_u} \mathbf{u}(k-1) -``` -The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended Help section. - -# Extended Help -!!! details "Extended Help" - With ``n_i``, the ``i``th element of the ``\mathbf{n_b}`` vector defined in [`move_blocking`](@ref) - documentation, we introduce the ``\mathbf{Q}(n_i)`` matrix of size `(nu*ni, nu)`: - ```math - \mathbf{Q}(n_i) = \begin{bmatrix} - \mathbf{I} \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \end{bmatrix} - ``` - The ``\mathbf{U}`` vector and the conversion matrices are defined as: - ```math - \mathbf{U} = \begin{bmatrix} - \mathbf{u}(k + 0) \\ - \mathbf{u}(k + 1) \\ - \vdots \\ - \mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad - \mathbf{P_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} , \quad - \mathbf{T_u} = \begin{bmatrix} - \mathbf{I} \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \end{bmatrix} - ``` - and, depending on the transcription method, we have: - - ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref) - - ``\mathbf{P_u} = [\begin{smallmatrix}\mathbf{P_u^†} & \mathbf{0} \end{smallmatrix}]`` - if `transcription` is a [`MultipleShooting`](@ref) - The conversion matrices are stored as `SparseMatrixCSC` since it was benchmarked that it - is generally more performant than normal dense matrices, even for small `nu`, `Hp` and - `Hc` values. Using `Bool` element type and `BitMatrix` is also slower. -""" -function init_ZtoU( - estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb -) where {NT<:Real} - nu = estim.model.nu - I_nu = sparse(Matrix{NT}(I, nu, nu)) - PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc) - for i=1:Hc - ni = nb[i] - Q_ni = repeat(I_nu, ni, 1) - iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1]) - PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))] - end - PuDagger = sparse(PuDagger) - Pu = init_PUmat(estim, transcription, Hp, Hc, PuDagger) - Tu = repeat(I_nu, Hp) - return Pu, Tu -end - -function init_PUmat(_,::SingleShooting,_,_,PuDagger::AbstractMatrix{NT}) where NT<:Real - return PuDagger -end -function init_PUmat( - estim, transcription::TranscriptionMethod, Hp, Hc, PuDagger::AbstractMatrix{NT} -) where NT<:Real - nΔU = estim.model.nu*Hc - nZ = get_nZ(estim, transcription, Hp, Hc) - return [PuDagger spzeros(NT, nΔU, nZ - nΔU)] -end - @doc raw""" init_predmat( model::LinModel, estim, transcription::SingleShooting, Hp, Hc, nb From a73dee07caa92adc33521247bc366328dcc59659 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 10:57:13 -0500 Subject: [PATCH 10/49] wip: orthogonal collocation --- src/controller/transcription.jl | 57 ++++++++++++++++++++++++++++----- 1 file changed, 49 insertions(+), 8 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c4804a467..0158dd16a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -123,8 +123,8 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). The `h` argument is the hold order for ``\mathbf{u}``, and `no`, the number of collocation -points. The decision variable is similar to [`MultipleShooting`](@ref), but it also includes -the collocation points (excluding ``ϵ``): +points ``n_o``. The decision variable is similar to [`MultipleShooting`](@ref), but it also +includes the collocation points (excluding ``ϵ``): ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -134,11 +134,11 @@ where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic \mathbf{k}_{1}(k+0) \\ \mathbf{k}_{2}(k+0) \\ \vdots \\ - \mathbf{k}_{n_p}(k+0) \\ + \mathbf{k}_{n_o}(k+0) \\ \mathbf{k}_{1}(k+1) \\ \mathbf{k}_{2}(k+1) \\ \vdots \\ - \mathbf{k}_{n_p}(k+H_p) \end{bmatrix} + \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} ``` and ``\mathbf{k}_p(k+j)`` is the deterministic state prediction for the ``p``th collocation point at the ``j``th stage/iterval (details in Extended Help). @@ -835,8 +835,7 @@ Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) tra If supported by `mpc.optim`, it warm-starts the solver at: ```math -\mathbf{Z̃_s} = -\begin{bmatrix} +\mathbf{Z̃_s} = \begin{bmatrix} \mathbf{Δu}(k+0|k-1) \\ \mathbf{Δu}(k+1|k-1) \\ \vdots \\ @@ -859,6 +858,49 @@ function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) return Z̃s end +@doc raw""" + set_warmstart!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s + +Set and return the warm-start value of `Z̃var` for other [`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-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-1|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{k}_{1}(k+1|k-1) \\ + \mathbf{k}_{2}(k+1|k-1) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+1|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}}``. +""" +function set_warmstart!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) + nu, nx̂, Hp, Hc, Z̃s = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ + # --- input increments ΔU --- + Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] + Z̃s[(Hc*nu-nu+1):(Hc*nu)] .= 0 + # --- predicted states X̂0 --- + Z̃s[(Hc*nu+1):(Hc*nu+Hp*nx̂-nx̂)] .= @views mpc.Z̃[(Hc*nu+nx̂+1):(Hc*nu+Hp*nx̂)] + Z̃s[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] .= @views mpc.Z̃[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] + # --- slack variable ϵ --- + mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) + JuMP.set_start_value.(Z̃var, Z̃s) + return Z̃s +end + @doc raw""" set_warmstart!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s @@ -866,8 +908,7 @@ Set and return the warm-start value of `Z̃var` for other [`TranscriptionMethod` It warm-starts the solver at: ```math -\mathbf{Z̃_s} = -\begin{bmatrix} +\mathbf{Z̃_s} = \begin{bmatrix} \mathbf{Δu}(k+0|k-1) \\ \mathbf{Δu}(k+1|k-1) \\ \vdots \\ From a812b0d5b95732a3ba390c22e933d1592b9a07ee Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 16:15:30 -0500 Subject: [PATCH 11/49] debug: correct `spzeros` size in `init_ZtoU` --- src/controller/construct.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index dcbf20a48..97520bb2c 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -785,9 +785,8 @@ function init_ZtoU( PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))] end PuDagger = sparse(PuDagger) - nΔU = estim.model.nu*Hc nZ = get_nZ(estim, transcription, Hp, Hc) - Pu = [PuDagger spzeros(NT, nΔU, nZ - nΔU)] + Pu = [PuDagger spzeros(NT, nu*Hp, nZ - nu*Hc)] Tu = repeat(I_nu, Hp) return Pu, Tu end From b63f57d6f798d66f805ae669540fd5294d67ce75 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 22 Feb 2026 09:42:53 -0500 Subject: [PATCH 12/49] debug: correct prediction matrices for `OrthogonalCollocation` --- src/ModelPredictiveControl.jl | 2 ++ src/controller/transcription.jl | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index c8fe2c32b..72aabe892 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -37,6 +37,8 @@ import JuMP: @variable, @operator, @constraint, @objective import OSQP, Ipopt +import FastGaussQuadrature + export SimModel, LinModel, NonLinModel export DiffSolver, RungeKutta, ForwardEuler export setop!, setname! diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0158dd16a..b7d4e737f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -153,11 +153,15 @@ struct OrthogonalCollocation <: CollocationMethod no::Int f_threads::Bool h_threads::Bool + τ::Vector{Float64} function OrthogonalCollocation(h::Int=0, no=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) end - return new(h, no, f_threads, h_threads) + x, _ = FastGaussQuadrature.gaussradau(no) + # we reverse the nodes to include the τ=1.0 node: + τ = (reverse(-x) .+ 1) ./ 2 + return new(h, no, f_threads, h_threads, τ) end end @@ -452,11 +456,15 @@ given in the Extended Help section. !!! details "Extended Help" The terminal state matrices all appropriately sized zero matrices ``\mathbf{0}``, except for ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]`` + if `transcription` is a [`MultipleShooting`](@ref), and ``\mathbf{e_x̂} = + [\begin{smallmatrix}\mathbf{0} & \mathbf{I} & \mathbf{0}\end{smallmatrix}]`` otherwise. """ function init_predmat( model::NonLinModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd + nΔU = nu*Hc + nX̂0 = nx̂*Hp nZ = get_nZ(estim, transcription, Hp, Hc) E = zeros(NT, 0, nZ) G = zeros(NT, 0, nd) @@ -464,7 +472,8 @@ function init_predmat( K = zeros(NT, 0, nx̂) V = zeros(NT, 0, nu) B = zeros(NT, 0) - ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I] + myzeros = zeros(nx̂, nZ - nΔU - nX̂0) + ex̂ = [zeros(NT, nx̂, nΔU + nX̂0 - nx̂) I myzeros] gx̂ = zeros(NT, nx̂, nd) jx̂ = zeros(NT, nx̂, nd*Hp) kx̂ = zeros(NT, nx̂, nx̂) From ea6447be4d6cf0a41d924b93f6f5b8b710f5527d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 22 Feb 2026 10:36:36 -0500 Subject: [PATCH 13/49] doc: warning admonition for collocation and state estimators --- src/controller/transcription.jl | 81 +++++++++++++++++++++++++++++++-- 1 file changed, 77 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index b7d4e737f..26ac982bf 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -86,10 +86,12 @@ simulated with these solvers. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle -moderately stiff systems and is A-stable. Note that the built-in [`StateEstimator`](@ref) -will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to -estimate the plant states, not the trapezoidal rule (see `supersample` option of -[`RungeKutta`](@ref) for stiff systems). See Extended Help for more details. +moderately stiff systems and is A-stable. See Extended Help for more details. + +!!! warning +The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the +construction of the [`NonLinModel`](@ref) to estimate the plant states, not the trapezoidal +rule (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this transcription method. @@ -143,6 +145,14 @@ where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic and ``\mathbf{k}_p(k+j)`` is the deterministic state prediction for the ``p``th collocation point at the ``j``th stage/iterval (details in Extended Help). +!!! warning + The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the + construction of the [`NonLinModel`](@ref) to estimate the plant states, not orthogonal + collocation (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). + +Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this +transcription method. + # Extended Help !!! details "Extended Help" See the Extended Help of [`TrapezoidalCollocation`](@ref) to understand why the @@ -161,6 +171,7 @@ struct OrthogonalCollocation <: CollocationMethod x, _ = FastGaussQuadrature.gaussradau(no) # we reverse the nodes to include the τ=1.0 node: τ = (reverse(-x) .+ 1) ./ 2 + return new(h, no, f_threads, h_threads, τ) end end @@ -1300,5 +1311,67 @@ function con_nonlinprogeq!( return geq end +function con_nonlinprogeq!( + geq, X̂0, Û0, K0, + mpc::PredictiveController, model::NonLinModel, 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 + Ts, p = model.Ts, model.p + As, Cs_u = mpc.estim.As, mpc.estim.Cs_u + nk = get_nk(model, transcription) + D̂0 = mpc.D̂0 + X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] + @threadsif f_threads for j=1:Hp + if j < 2 + x̂0 = @views mpc.estim.x̂0[1:nx̂] + d̂0 = @views mpc.d0[1:nd] + else + x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] + d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] + end + k0 = @views K0[(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)] + ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] + x0, xs = @views x̂0[1:nx], x̂0[nx+1:end] + x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] + sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] + k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] + # ----------------- stochastic defects ----------------------------------------- + xsnext = @views x̂0next[nx+1:end] + mul!(xsnext, As, xs) + ssnext .= @. xsnext - xsnext_Z̃ + # ----------------- deterministic defects -------------------------------------- + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + mul!(û0, Cs_u, xs) # ys_u(k) = Cs_u*xs(k) + û0 .+= u0 # û0(k) = u0(k) + ys_u(k) + 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!(k1, x0, û0, d̂0, p) + else + k1 .= @views K0[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + end + if h < 1 || j ≥ Hp + # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 + û0next = û0 + else + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + mul!(û0next, Cs_u, xsnext_Z̃) # ys_u(k+1) = Cs_u*xs(k+1) + û0next .+= u0next # û0(k+1) = u0(k+1) + ys_u(k+1) + end + model.f!(k2, x0next_Z̃, û0next, d̂0next, p) + sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2) + end + return geq +end + "No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq From 78ad00775045b27a4ca3556190788090e12264ea Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 22 Feb 2026 11:15:26 -0500 Subject: [PATCH 14/49] doc: idem --- src/controller/transcription.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 26ac982bf..596850db3 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -89,9 +89,9 @@ equality constraint function and by using the implicit trapezoidal rule. It can moderately stiff systems and is A-stable. See Extended Help for more details. !!! warning -The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the -construction of the [`NonLinModel`](@ref) to estimate the plant states, not the trapezoidal -rule (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). + The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the + construction of the [`NonLinModel`](@ref) to estimate the plant states, not the + trapezoidal rule (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this transcription method. From eb91f873df4f9f1728776b77ca3adb5df35da20d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 23 Feb 2026 09:24:53 -0500 Subject: [PATCH 15/49] doc: minor details --- src/controller/transcription.jl | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 596850db3..a490380e5 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -124,13 +124,14 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). -The `h` argument is the hold order for ``\mathbf{u}``, and `no`, the number of collocation -points ``n_o``. The decision variable is similar to [`MultipleShooting`](@ref), but it also -includes the collocation points (excluding ``ϵ``): +Also known as pseudo-spectral method. The `h` argument is the hold order for ``\mathbf{u}``, +and `no`, the number of collocation points ``n_o``. The decision variable is similar to +[`MultipleShooting`](@ref), but it also includes the collocation points (excluding ``ϵ``): ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` -where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic state only: +where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic state only +(the first `nx` elements of ``\mathbf{x̂}``): ```math \mathbf{K} = \begin{bmatrix} \mathbf{k}_{1}(k+0) \\ @@ -143,15 +144,15 @@ where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} ``` and ``\mathbf{k}_p(k+j)`` is the deterministic state prediction for the ``p``th collocation -point at the ``j``th stage/iterval (details in Extended Help). +point at the ``j``th stage/interval/finite element (details in Extended Help). !!! warning The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to estimate the plant states, not orthogonal collocation (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). -Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this -transcription method. +Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for +this transcription method (sparser formulation than [`MultipleShooting`](@ref)). # Extended Help !!! details "Extended Help" From d416675e17354302f8f0445a451f049e06a0d5a2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 25 Feb 2026 11:07:32 -0500 Subject: [PATCH 16/49] changed: renamed `K0` to `K` and `k0` to `k` It's not necessary to differentiate with a `k` vector that would include the operating point (there is never operating point in this vector). And it now follows the nomenclature of `OrthogonalCollocation` --- src/controller/execute.jl | 4 +- src/controller/legacy.jl | 20 ++++----- src/controller/nonlinmpc.jl | 74 +++++++++++++++--------------- src/controller/transcription.jl | 79 ++++++++++++++++++--------------- src/estimator/execute.jl | 16 +++---- src/estimator/internal_model.jl | 10 ++--- src/estimator/kalman.jl | 18 ++++---- src/estimator/mhe/construct.jl | 22 ++++----- src/estimator/mhe/execute.jl | 46 +++++++++---------- src/estimator/mhe/legacy.jl | 14 +++--- src/model/linearization.jl | 4 +- src/model/nonlinmodel.jl | 8 ++-- src/sim_model.jl | 4 +- test/1_test_sim_model.jl | 14 +++--- 14 files changed, 171 insertions(+), 162 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index bb059ca4e..d9b24929d 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -142,7 +142,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real info = Dict{Symbol, Any}() ΔŨ = Vector{NT}(undef, nΔŨ) x̂0end = similar(mpc.estim.x̂0) - K0 = 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) @@ -150,7 +150,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real D̂ = buffer.D̂ U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop diff --git a/src/controller/legacy.jl b/src/controller/legacy.jl index 2892154a8..e093cbf33 100644 --- a/src/controller/legacy.jl +++ b/src/controller/legacy.jl @@ -23,21 +23,21 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT J::Vector{JNT} = zeros(JNT, 1) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K0::Vector{JNT} = zeros(JNT, nK) + K::Vector{JNT} = zeros(JNT, nK) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û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) # ---------------------- objective function ------------------------------------------- - function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function Jfunc!(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, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) @@ -64,14 +64,14 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end end # --------------------- inequality constraint functions ------------------------------- - function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return g end Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), ) # temporarily enable all the inequality constraints for sparsity detection: @@ -109,14 +109,14 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇g_funcs![i] = ∇gfuncs_i! end # --------------------- equality constraint functions --------------------------------- - function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return geq end Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 35d39e69a..1e27e6154 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -572,7 +572,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny ΔŨ = zeros(NT, nΔŨ) x̂0end = zeros(NT, nx̂) - K0 = zeros(NT, nK) + K = zeros(NT, nK) Ue, Ŷe = zeros(NT, nUe), zeros(NT, nŶe) U0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) Û0, X̂0 = zeros(NT, nU), zeros(NT, nX̂) @@ -580,11 +580,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real geq = zeros(NT, neq) J_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) - function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, 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, model, Ue, Ŷe, ΔŨ) end if !isnothing(mpc.hessian) @@ -597,11 +597,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), ) - function g!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function g!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return nothing end g, ∇g = value_and_jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) @@ -624,11 +624,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real λ[old_i_g] = λi ∇²g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) - function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return dot(λ, g) end ∇²ℓg = hessian(ℓ_g, mpc.hessian, mpc.Z̃, Constant(λ), ∇²g_cache...) @@ -639,11 +639,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real # --- equality constraint derivatives --- geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return nothing end geq, ∇geq = value_and_jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) @@ -664,11 +664,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) - function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return dot(λeq, geq) end ∇²ℓgeq = hessian(ℓ_geq, mpc.hessian, mpc.Z̃, Constant(λeq), ∇²geq_cache...) @@ -792,20 +792,20 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J J::Vector{JNT} = zeros(JNT, 1) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K0::Vector{JNT} = zeros(JNT, nK) + K::Vector{JNT} = zeros(JNT, nK) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û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, K0, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, 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, model, Ue, Ŷe, ΔŨ) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update at first call J_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(J!, grad, Z̃_J, J_cache...; strict) @@ -918,7 +918,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K0::Vector{JNT} = zeros(JNT, nK) + K::Vector{JNT} = zeros(JNT, nK) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) @@ -926,20 +926,20 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN gi::Vector{JNT}, geq::Vector{JNT} = zeros(JNT, ngi), zeros(JNT, neq) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ----------------------------- - function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g, gi) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g, gi) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update at first call ∇gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) @@ -948,7 +948,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g), Cache(gi) ) ∇²gi_prep = prepare_hessian( @@ -991,18 +991,18 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return nothing end - function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) return dot(λeq, geq) end Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at first call ∇geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z̃_∇geq, ∇geq_cache...; strict) @@ -1011,7 +1011,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇²geq_prep = prepare_hessian( @@ -1057,7 +1057,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -1066,17 +1066,17 @@ 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, K0, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) model, transcription = mpc.estim.model, mpc.transcription U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) ϵ = getϵ(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) - geq = con_nonlinprogeq!(geq, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃) + geq = con_nonlinprogeq!(geq, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index cddd55a75..185af5f5f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -126,7 +126,7 @@ Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ Also known as pseudo-spectral method. The `h` argument is the hold order for ``\mathbf{u}``, and `no`, the number of collocation points ``n_o``. The decision variable is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points (excluding ``ϵ``): +[`MultipleShooting`](@ref), but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -143,7 +143,7 @@ where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic \vdots \\ \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} ``` -and ``\mathbf{k}_p(k+j)`` is the deterministic state prediction for the ``p``th collocation +and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation point at the ``j``th stage/interval/finite element (details in Extended Help). !!! warning @@ -165,16 +165,29 @@ struct OrthogonalCollocation <: CollocationMethod f_threads::Bool h_threads::Bool τ::Vector{Float64} - function OrthogonalCollocation(h::Int=0, no=5; f_threads=false, h_threads=false) + Dτ::Matrix{Float64} + function OrthogonalCollocation(h::Int=0, no::Int=5; f_threads=false, h_threads=false) if !(h == 0 || h == 1) throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) - end - x, _ = FastGaussQuadrature.gaussradau(no) + end + # TODO: move τ and Dτ to NonLinMPC object to construct with adequate types (e.g. Float32) + Dτ, τ = init_diffmatrix(Float64, no) + return new(h, no, f_threads, h_threads, τ, Dτ) + end +end + +function init_diffmatrix(T, no, quadrature=:gaussradau) + if quadrature == :gaussradau + x, _ = FastGaussQuadrature.gaussradau(no, T) # we reverse the nodes to include the τ=1.0 node: τ = (reverse(-x) .+ 1) ./ 2 - - return new(h, no, f_threads, h_threads, τ) + A = τ.^(0:no-1)'.*(1:no)' + B = τ.^(1:no)' + Dτ = (A/B) + else + throw(ArgumentError("Only :gaussradau scheme is currently implemented.")) end + return Dτ, τ end function validate_transcription(::LinModel, ::CollocationMethod) @@ -1003,14 +1016,14 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, X̂0, Û0, K0, + Ŷ0, x̂0end, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) -> Ŷ0, x̂0end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). -The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augmented model of [`f̂!`](@ref) and [`ĥ!`](@ref) functions is called recursively in a `for` loop: ```math \begin{aligned} @@ -1020,7 +1033,7 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The aug ``` """ function predict!( - Ŷ0, x̂0end, X̂0, Û0, K0, + Ŷ0, x̂0end, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, U0, _ ) @@ -1031,9 +1044,9 @@ function predict!( for j=1:Hp u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + k = @views K[(1 + nk*(j-1)):(nk*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d̂0) + f̂!(x̂0next, û0, k, mpc.estim, model, x̂0, u0, d̂0) x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d̂0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1167,14 +1180,14 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K0 + geq, X̂0, Û0, K mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The nonlinear +The method mutates the `geq`, `X̂0`, `Û0` and `K` vectors in argument. The nonlinear equality constraints `geq` only includes the augmented state defects, computed with: ```math \mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d̂_0}(k)\Big) @@ -1184,7 +1197,7 @@ in which the augmented state ``\mathbf{x̂_0}`` are extracted from the decision `Z̃`, and ``\mathbf{f̂}`` is the augmented state function defined in [`f̂!`](@ref). """ function con_nonlinprogeq!( - geq, X̂0, Û0, K0, + geq, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) nu, nx̂, nd, nk = model.nu, mpc.estim.nx̂, model.nd, model.nk @@ -1203,11 +1216,11 @@ function con_nonlinprogeq!( end u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + k = @views K[(1 + nk*(j-1)):(nk*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)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d̂0) + f̂!(x̂0next, û0, k, mpc.estim, model, x̂0, u0, d̂0) ŝnext .= x̂0next .- x̂0next_Z̃ end return geq @@ -1215,14 +1228,14 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K0 + geq, X̂0, Û0, K mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. +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- @@ -1251,7 +1264,7 @@ in which ``h`` is the hold order `transcription.h` and the disturbed input is: the ``\mathbf{A_s, C_{s_u}}`` matrices are defined in [`init_estimstoch`](@ref) doc. """ function con_nonlinprogeq!( - geq, X̂0, Û0, K0, + geq, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) @@ -1271,7 +1284,7 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + 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)] @@ -1280,7 +1293,7 @@ function con_nonlinprogeq!( xsnext = @views x̂0next[nx+1:end] x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] + k1, k2 = @views k[1:nx], k[nx+1:2*nx] # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ @@ -1293,7 +1306,7 @@ function con_nonlinprogeq!( # last iteration (j-1) may not be executed (iterations are re-orderable) model.f!(k1, x0, û0, d̂0, p) else - k1 .= @views K0[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + k1 .= @views K[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end if h < 1 || j ≥ Hp # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 @@ -1310,7 +1323,7 @@ function con_nonlinprogeq!( end function con_nonlinprogeq!( - geq, X̂0, Û0, K0, + geq, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) @@ -1319,7 +1332,6 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads Ts, p = model.Ts, model.p - As, Cs_u = mpc.estim.As, mpc.estim.Cs_u nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] @@ -1331,30 +1343,28 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + 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)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - x0, xs = @views x̂0[1:nx], x̂0[nx+1:end] + x0 = @views x̂0[1:nx] + xsnext = @views x̂0next[nx+1:end] x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] # ----------------- stochastic defects ----------------------------------------- - xsnext = @views x̂0next[nx+1:end] - mul!(xsnext, As, xs) + fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ # ----------------- deterministic defects -------------------------------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - mul!(û0, Cs_u, xs) # ys_u(k) = Cs_u*xs(k) - û0 .+= u0 # û0(k) = u0(k) + ys_u(k) + f̂_input!(û0, mpc.estim, model, x̂0, u0) 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!(k1, x0, û0, d̂0, p) else - k1 .= @views K0[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + k1 .= @views K[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end if h < 1 || j ≥ Hp # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 @@ -1362,8 +1372,7 @@ function con_nonlinprogeq!( else u0next = @views U0[(1 + nu*j):(nu*(j+1))] û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - mul!(û0next, Cs_u, xsnext_Z̃) # ys_u(k+1) = Cs_u*xs(k+1) - û0next .+= u0next # û0(k+1) = u0(k+1) + ys_u(k+1) + f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) end model.f!(k2, x0next_Z̃, û0next, d̂0next, p) sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index e1fa8b076..06b6e13d6 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state function ``\mathbf{f̂}`` of the augmented model. @@ -27,8 +27,8 @@ the function returns the next state of the augmented model, as deviation vectors \end{aligned} ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, -`û0` and `k0` in place. The argument `û0` stores the disturbed input of the augmented model -``\mathbf{û_0}``, and `k0`, the intermediate stage values of `model.solver`, when applicable. +`û0` and `k` in place. The argument `û0` stores the disturbed input of the augmented model +``\mathbf{û_0}``, and `k`, the intermediate stage values of `model.solver`, when applicable. The model parameter `model.p` is not included in the function signature for conciseness. The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. @@ -61,8 +61,8 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for are computed by [`augment_model`](@ref) (almost always zeros in practice for [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, k, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end @doc raw""" @@ -92,17 +92,17 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - f!(xdnext, k0, model, xd, û0, d0, model.p) + f!(xdnext, k, model, xd, û0, d0, model.p) mul!(xsnext, As, xs) x̂0next .+= f̂op .- x̂op return nothing diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 0a5e04524..8eb1eb9d8 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -168,14 +168,14 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real end @doc raw""" - f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) + f̂!(x̂0next, _ , k, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref). It calls [`f!`](@ref) directly since this estimator does not augment the states. """ -function f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) - f!(x̂0next, k0, model, x̂0, u0, d0, model.p) +function f̂!(x̂0next, _ , k, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) + f!(x̂0next, k, model, x̂0, u0, d0, model.p) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end @@ -307,8 +307,8 @@ function update_estimate!(estim::InternalModel, _ , d0, u0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext, k0 = estim.buffer.x̂, estim.buffer.k - f!(x̂dnext, k0, model, x̂d, u0, d0, model.p) + x̂dnext, k = estim.buffer.x̂, estim.buffer.k + f!(x̂dnext, k, model, x̂d, u0, d0, model.p) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 26eae242b..d08e11b4c 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -862,7 +862,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k + x̂0next, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: P̂corr_temp = Hermitian(estim.buffer.P̂, :L) P̂corr_temp .= P̂corr @@ -875,7 +875,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) X̂0next = X̂0corr for j in axes(X̂0next, 2) @views x̂0corr .= X̂0corr[:, j] - @views f̂!(X̂0next[:, j], û0, k0, estim, estim.model, x̂0corr, u0, d0) + @views f̂!(X̂0next[:, j], û0, k, estim, estim.model, x̂0corr, u0, d0) end x̂0next .= mul!(x̂0corr, X̂0next, m̂) X̄0next = estim.X̄0 @@ -1101,8 +1101,8 @@ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nx̂ = model.nx + size(As, 1) x̂op = f̂op = zeros(nx̂) # not important for Jacobian computations - function f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) - return f̂!(x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + function f̂_ekf!(x̂0next, x̂0, û0, k, u0, d0) + return f̂!(x̂0next, û0, k, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) end ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) strict = Val(true) @@ -1110,15 +1110,15 @@ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) û0 = Cache(zeros(NT, nu)) - k0 = Cache(zeros(NT, nk)) + k = Cache(zeros(NT, nk)) cst_u0 = Constant(rand(NT, nu)) cst_d0 = Constant(rand(NT, nd)) F̂prep = prepare_jacobian( - f̂_ekf!, x̂0next, jacobian, x̂0, û0, k0, cst_u0, cst_d0; strict + f̂_ekf!, x̂0next, jacobian, x̂0, û0, k, cst_u0, cst_d0; strict ) Ĥprep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) function linfuncF̂!(F̂, x̂0next, backend, x̂0, cst_u0, cst_d0) - return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k0, cst_u0, cst_d0) + return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k, cst_u0, cst_d0) end function linfuncĤ!(Ĥ, ŷ0, backend, x̂0, cst_d0) return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥprep, backend, x̂0, cst_d0) @@ -1253,9 +1253,9 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ Q̂ = estim.cov.Q̂ - x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k + x̂0next, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: - f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0) + f̂!(x̂0next, û0, k, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ mul!(P̂corr_Âᵀ, P̂corr, Â') Â_P̂corr_Âᵀ = estim.buffer.Q̂ diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index a29a08d85..e452cb686 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1385,17 +1385,17 @@ function get_nonlinobj_op( myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k0::Vector{JNT} = zeros(JNT, nk) + k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) - function J!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call J_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), ) @@ -1490,23 +1490,23 @@ function get_nonlincon_oracle( strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k0::Vector{JNT} = zeros(JNT, nk) + k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT}, gi::Vector{JNT} = zeros(JNT, ng), zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, V̂, X̂0, û0, k0, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function gi!(gi, Z̃, V̂, X̂0, û0, k, ŷ0, g) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, V̂, X̂0, û0, k0, ŷ0, g, gi) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function ℓ_gi(Z̃, λi, V̂, X̂0, û0, k, ŷ0, g, gi) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call - ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) + ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) @@ -1515,7 +1515,7 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g), Cache(gi) + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(gi) ) estim.Nk[] = He # see comment above ∇²gi_prep = prepare_hessian( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index b558f3df0..58e1ca468 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -122,8 +122,8 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̃ = nε + nx̂ info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) - û0, k0, ŷ0 = buffer.û, buffer.k, buffer.ŷ - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + û0, k, ŷ0 = buffer.û, buffer.k, buffer.ŷ + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] x̄ = estim.x̂0arr_old - x̂0arr X̂0 = [x̂0arr; X̂0] @@ -190,17 +190,17 @@ function addinfo!( He = estim.He nV̂, nX̂, ng = He*nym, He*nx̂, length(con.i_g) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) - k0 = zeros(NT, nk) + k = zeros(NT, nk) û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) g = zeros(NT, ng) x̄ = zeros(NT, nx̂) J_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), ) - function J!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end if !isnothing(estim.hessian) @@ -211,9 +211,9 @@ function addinfo!( # --- inequality constraint derivatives --- old_i_g = copy(estim.con.i_g) estim.con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed - ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0)) - function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0)) + function g!(g, Z̃, V̂, X̂0, û0, k, ŷ0) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) return nothing end g, ∇g = value_and_jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) @@ -234,9 +234,9 @@ function addinfo!( end λ = zeros(NT, ng) λ[old_i_g] .= λi - ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) - function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) + function ℓ_g(Z̃, λ, V̂, X̂0, û0, k, ŷ0, g) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) return dot(λ, g) end ∇²ℓg = hessian(ℓ_g, estim.hessian, estim.Z̃, Constant(λ), ∇²g_cache...) @@ -513,9 +513,9 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real estim.Z̃ .= JuMP.value.(Z̃var) end # --------- update estimate ----------------------- - û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k + û0, ŷ0, k = buffer.û, buffer.ŷ, buffer.k estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next return estim.Z̃ @@ -550,7 +550,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var nε, nx̂, nŵ, nZ̃, Nk = estim.nε, estim.nx̂, estim.nx̂, length(estim.Z̃), estim.Nk[] nx̃ = nε + nx̂ Z̃s = Vector{NT}(undef, nZ̃) # TODO: remove this allocation - û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k + û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k # --- slack variable ε --- estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) # --- arrival state estimate x̂0arr --- @@ -558,7 +558,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var # --- process noise estimates Ŵ --- Z̃s[nx̃+1:end] = estim.Ŵ # verify definiteness of objective function: - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s) Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] = 0 @@ -725,7 +725,7 @@ function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::Li end @doc raw""" - predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 Compute the vectors when `model` is *not* a [`LinModel`](@ref). @@ -733,7 +733,7 @@ The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The augm [`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated process noise ``\mathbf{ŵ}``. """ -function predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) +function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) nε, Nk = estim.nε, estim.Nk[] nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nε + nx̂ @@ -745,7 +745,7 @@ function predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) x̂0next .+= ŵ y0nextm = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] @@ -764,7 +764,7 @@ function predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, ŷ0m = @views ŷ0[estim.i_ym] V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) x̂0next .+= ŵ x̂0 = x̂0next end @@ -774,15 +774,15 @@ end """ - update_predictions!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + update_predictions!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) +function update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) model = estim.model - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃) ε = getε(estim, Z̃) g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ε) return nothing diff --git a/src/estimator/mhe/legacy.jl b/src/estimator/mhe/legacy.jl index 31041867c..5cc1d2f0d 100644 --- a/src/estimator/mhe/legacy.jl +++ b/src/estimator/mhe/legacy.jl @@ -20,18 +20,18 @@ function get_optim_functions( myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k0::Vector{JNT} = zeros(JNT, nk) + k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) # --------------------- objective functions ------------------------------------------- - function Jfunc!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function Jfunc!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇J_context = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), ) @@ -57,12 +57,12 @@ function get_optim_functions( return ∇Jarg .= ∇J end # --------------------- inequality constraint functions ------------------------------- - function gfunc!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) - return update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + function gfunc!(g, Z̃, V̂, X̂0, û0, k, ŷ0) + return update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇g_context = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), ) # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 5b05d6267..7d0aad91b 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -163,7 +163,7 @@ function linearize!( nonlinmodel = model buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- - x0, u0, d0, k0 = buffer.x, buffer.u, buffer.d, buffer.k + x0, u0, d0, k = buffer.x, buffer.u, buffer.d, buffer.k x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop @@ -175,7 +175,7 @@ function linearize!( y0 .+= nonlinmodel.yop y = y0 # --- compute the nonlinear model next state at operating points --- - f!(x0next, k0, nonlinmodel, x0, u0, d0, model.p) + f!(x0next, k, nonlinmodel, x0, u0, d0, model.p) x0next .+= nonlinmodel.fop xnext = x0next # xnext = f(x0,u0,d0) + fop - xop + xop = f(x0,u0,d0) + fop # --- recompute x since it was modified in buffer.x --- diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 37f0b2e0c..4877a0f6a 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -298,15 +298,15 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, k0, model::NonLinModel, x0, u0, d0, p) + f!(x0next, k, model::NonLinModel, x0, u0, d0, p) Compute `x0next` using the [`DiffSolver`](@ref) in `model.solver` and `model.f!`. -The method mutates `x0next` and `k0` arguments in-place. The latter is used to store the +The method mutates `x0next` and `k` arguments in-place. The latter is used to store the intermediate stage values of the solver. """ -function f!(x0next, k0, model::NonLinModel, x0, u0, d0, p) - return solver_f!(x0next, k0, model.f!, model.Ts, model.solver, x0, u0, d0, p) +function f!(x0next, k, model::NonLinModel, x0, u0, d0, p) + return solver_f!(x0next, k, model.f!, model.Ts, model.solver, x0, u0, d0, p) end """ diff --git a/src/sim_model.jl b/src/sim_model.jl index 26b638b69..c02d87826 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -247,10 +247,10 @@ julia> x = updatestate!(model, [1]) """ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0, x0next, k0 = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k + u0, d0, x0next, k = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k u0 .= u .- model.uop d0 .= d .- model.dop - f!(x0next, k0, model, model.x0, u0, d0, model.p) + f!(x0next, k, model, model.x0, u0, d0, model.p) x0next .+= model.fop .- model.xop model.x0 .= x0next xnext = x0next diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 1639460f1..e4c4fe8d5 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -327,16 +327,16 @@ end nonlinmodel3 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=RungeKutta()) linmodel3 = linearize(nonlinmodel3; x, u, d) x0, u0, d0 = x - nonlinmodel3.xop, u - nonlinmodel3.uop, d - nonlinmodel3.dop - x0next, k0, y0 = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k, nonlinmodel3.buffer.y + x0next, k, y0 = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k, nonlinmodel3.buffer.y backend = AutoForwardDiff() - f_A(x0next, x0, k0) = ModelPredictiveControl.f!(x0next, k0, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) - f_Bu(x0next, u0, k0) = ModelPredictiveControl.f!(x0next, k0, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) - f_Bd(x0next, d0, k0) = ModelPredictiveControl.f!(x0next, k0, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) + f_A(x0next, x0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) + f_Bu(x0next, u0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) + f_Bd(x0next, d0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) h_C(y0, x0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0, nonlinmodel3.p) h_Dd(y0, d0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0, nonlinmodel3.p) - A = jacobian(f_A, x0next, backend, x0, Cache(k0)) - Bu = jacobian(f_Bu, x0next, backend, u0, Cache(k0)) - Bd = jacobian(f_Bd, x0next, backend, d0, Cache(k0)) + A = jacobian(f_A, x0next, backend, x0, Cache(k)) + Bu = jacobian(f_Bu, x0next, backend, u0, Cache(k)) + Bd = jacobian(f_Bd, x0next, backend, d0, Cache(k)) C = jacobian(h_C, y0, backend, x0) Dd = jacobian(h_Dd, y0, backend, d0) @test linmodel3.A ≈ A From 66c88c1ff7231fdc306586320cd320ab4b4640b6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 25 Feb 2026 13:33:24 -0500 Subject: [PATCH 17/49] changed: minor formatting modification --- src/controller/execute.jl | 2 +- src/controller/transcription.jl | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index d9b24929d..80134fffa 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -142,7 +142,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real 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) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 185af5f5f..d96729de3 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1044,7 +1044,7 @@ function predict!( for j=1:Hp u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k = @views K[(1 + nk*(j-1)):(nk*j)] + k = @views K[(1 + nk*(j-1)):(nk*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] f̂!(x̂0next, û0, k, mpc.estim, model, x̂0, u0, d̂0) x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] @@ -1216,7 +1216,7 @@ function con_nonlinprogeq!( end u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k = @views K[(1 + nk*(j-1)):(nk*j)] + k = @views K[(1 + nk*(j-1)):(nk*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)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] @@ -1284,7 +1284,7 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k = @views K[(1 + nk*(j-1)):(nk*j)] + 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)] @@ -1343,7 +1343,7 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k = @views K[(1 + nk*(j-1)):(nk*j)] + 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)] From fef9bfac98d86eb6da50854dc6341bccb74aeff4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 26 Feb 2026 18:24:07 -0500 Subject: [PATCH 18/49] wip: `OrthogonalCollocation` --- src/controller/nonlinmpc.jl | 4 + src/controller/transcription.jl | 130 +++++++++++++++++++++++--------- 2 files changed, 99 insertions(+), 35 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1e27e6154..6259c7709 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -42,6 +42,8 @@ struct NonLinMPC{ weights::CW JE::JEfunc p::PT + Mo::SparseMatrixCSC{NT, Int} + Co::SparseMatrixCSC{NT, Int} R̂u::Vector{NT} R̂y::Vector{NT} lastu0::Vector{NT} @@ -120,6 +122,7 @@ struct NonLinMPC{ d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) + Mo, Co = init_orthocolloc(model, transcription) nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) @@ -130,6 +133,7 @@ struct NonLinMPC{ Hp, Hc, nϵ, nb, weights, JE, p, + Mo, Co, R̂u, R̂y, lastu0, P̃Δu, P̃u, Tu, Tu_lastu0, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d96729de3..f0af638ab 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1,3 +1,5 @@ +const COLLOCATION_NODE_TYPE = Float64 + """ Abstract supertype of all transcription methods of [`PredictiveController`](@ref). @@ -125,8 +127,9 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). Also known as pseudo-spectral method. The `h` argument is the hold order for ``\mathbf{u}``, -and `no`, the number of collocation points ``n_o``. The decision variable is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points: +and `no`, the number of collocation points ``n_o``. Only zero-order hold is currently +implemented, so `h` must be `0`. The decision variable is similar to [`MultipleShooting`](@ref), +but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -164,30 +167,70 @@ struct OrthogonalCollocation <: CollocationMethod no::Int f_threads::Bool h_threads::Bool - τ::Vector{Float64} - Dτ::Matrix{Float64} - function OrthogonalCollocation(h::Int=0, no::Int=5; f_threads=false, h_threads=false) - if !(h == 0 || h == 1) - throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) - end - # TODO: move τ and Dτ to NonLinMPC object to construct with adequate types (e.g. Float32) - Dτ, τ = init_diffmatrix(Float64, no) - return new(h, no, f_threads, h_threads, τ, Dτ) + τ::Vector{COLLOCATION_NODE_TYPE} + function OrthogonalCollocation( + h::Int=0, no::Int=5; f_threads=false, h_threads=false, roots=:gaussradau + ) + if !(h == 0) + throw(ArgumentError("Only the zero-order hold (h=0) is currently implemented.")) + end + if roots==:gaussradau + x, _ = FastGaussQuadrature.gaussradau(no, COLLOCATION_NODE_TYPE) + # we reverse the nodes to include the τ=1.0 node: + τ = (reverse(-x) .+ 1) ./ 2 + elseif roots==:gausslegendre + x, _ = FastGaussQuadrature.gausslegendre(no) + # converting [-1, 1] to [0, 1] (see + # https://en.wikipedia.org/wiki/Gaussian_quadrature#Change_of_interval): + τ = (x .+ 1) ./ 2 + else + throw(ArgumentError("roots argument must be :gaussradau or :gausslegendre.")) + end + return new(h, no, f_threads, h_threads, τ) end end -function init_diffmatrix(T, no, quadrature=:gaussradau) - if quadrature == :gaussradau - x, _ = FastGaussQuadrature.gaussradau(no, T) - # we reverse the nodes to include the τ=1.0 node: - τ = (reverse(-x) .+ 1) ./ 2 - A = τ.^(0:no-1)'.*(1:no)' - B = τ.^(1:no)' - Dτ = (A/B) - else - throw(ArgumentError("Only :gaussradau scheme is currently implemented.")) +""" + init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, o + +Init the differentiation `Mo` and continuity `Co` matrices of [`OrthogonalCollocation`](@ref). +""" +function init_orthocolloc( + model::SimModel{NT}, transcription::OrthogonalCollocation +) where {NT<:Real} + nx, no = model.nx, transcription.no + τ = transcription.τ + Po = Matrix{NT}(undef, nx*no, nx*no) # polynomial matrix (w/o the Y-intercept term) + Ṗo = Matrix{NT}(undef, nx*no, nx*no) # polynomial derivative matrix + for j=1:no, i=1:no + iRows = (1:nx) .+ nx*(i-1) + iCols = (1:nx) .+ nx*(j-1) + Po[iRows, iCols] = (τ[i]^j)*I(nx) + Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I(nx) + end + Mo = sparse((Ṗo/Po)/model.Ts) + Co = Matrix{NT}(undef, nx, nx*no) + for j=1:no + iCols = (1:nx) .+ nx*(j-1) + Co[:, iCols] = lagrange_end(j, τ)*I(nx) end - return Dτ, τ + Co = sparse(Co) + return Mo, Co +end +"Return empty sparse matrices for other [`TranscriptionMethod`](@ref)" +init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0) + +"Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." +function lagrange_end(j, τ_values) + τ_val = 1 # evaluating the Lagrange polynomial at τ=1 (the end of the interval) + τj = τ_values[j] + Lj = 1 + for i in eachindex(τ_values) + i == j && continue + τi = τ_values[i] + Lj *= (τ_val - τi)/(τj - τi) + end + return Lj end function validate_transcription(::LinModel, ::CollocationMethod) @@ -1297,7 +1340,7 @@ function con_nonlinprogeq!( # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ - # ----------------- deterministic defects -------------------------------------- + # ----------------- deterministic defects: trapezoidal collocation ------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0, u0) @@ -1331,10 +1374,11 @@ function con_nonlinprogeq!( Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads - Ts, p = model.Ts, model.p + p = model.p + no, Mo = transcription.no, mpc.Mo nk = get_nk(model, transcription) D̂0 = mpc.D̂0 - X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] + X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] @threadsif f_threads for j=1:Hp if j < 2 x̂0 = @views mpc.estim.x̂0[1:nx̂] @@ -1344,6 +1388,7 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end 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)] @@ -1355,17 +1400,13 @@ function con_nonlinprogeq!( # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ - # ----------------- deterministic defects -------------------------------------- + # ----------------- deterministic defects: orthogonal collocation -------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0, u0) - 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!(k1, x0, û0, d̂0, p) - else - k1 .= @views K[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 - end + + + if h < 1 || j ≥ Hp # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 û0next = û0 @@ -1374,8 +1415,27 @@ function con_nonlinprogeq!( û0next = @views Û0[(1 + nu*j):(nu*(j+1))] f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) end - model.f!(k2, x0next_Z̃, û0next, d̂0next, p) - sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2) + + # TODO: remove this allocation + Δk = similar(k) + for o=1:no + ko = @views k[(1 + (o-1)*nx):(o*nx)] + ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] + Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] + Δko .= ko_Z̃ .- x0 + if o < no + model.f!(ko, ko_Z̃, û0, d̂0, p) + else + model.f!(ko, ko_Z̃, û0next, d̂0next, p) + end + end + # TODO: remove this allocation + display(Δk) + ẋ0 = Mo*Δk + display(ẋ0) + + sdnext .= @. ẋ0 - k + end return geq end From 8c719f8cda714320f4813d38c73cae2113932c26 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 27 Feb 2026 00:00:42 -0500 Subject: [PATCH 19/49] wip --- src/controller/transcription.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f0af638ab..6e5edbb37 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1375,7 +1375,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads p = model.p - no, Mo = transcription.no, mpc.Mo + no, Mo, Co = transcription.no, mpc.Mo, mpc.Co nk = get_nk(model, transcription) 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)] @@ -1404,7 +1404,8 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0, u0) - + # something like this: + # sdnext .= @. x0 - x0next_Z̃ if h < 1 || j ≥ Hp From 4d39181309445172fecea2d07afb3a1cea28bbae Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 27 Feb 2026 00:05:39 -0500 Subject: [PATCH 20/49] doc --- src/controller/transcription.jl | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6e5edbb37..96f04353e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -161,6 +161,25 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). !!! details "Extended Help" See the Extended Help of [`TrapezoidalCollocation`](@ref) to understand why the stochastic states are left out of the ``\mathbf{K}`` vector. + + The collocation points are the roots of orthogonal polynomials, which are optimal for + approximating the state trajectories with polynomials of degree ``n_o``. The method then + enforces the system dynamics at these collocation points, which leads to a more accurate + solution than the trapezoidal rule for the same number of collocation points. + + The differentiation ``\mathbf{M_o}`` and continuity ``\mathbf{C_o}`` matrices are: + ```math + \begin{aligned} + \mathbf{M_o} &= \begin{bmatrix} + \mathbf{Ṗ_o} \mathbf{P_o}^{-1} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{Ṗ_o} \mathbf{P_o}^{-1} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \end{bmatrix} \\ + \mathbf{C_o} &= [\begin{smallmatrix}\mathbf{C_o^{x̂}} & \mathbf{C_o^{x̂}} & ... & \mathbf{C_o^{x̂}}\end{smallmatrix}] + \end{aligned} + ``` + where ``\mathbf{P_o}`` is the polynomial matrix (w/o the Y-intercept term) and + ``\mathbf{Ṗ_o}`` is its derivative matrix, both evaluated at the collocation points. """ struct OrthogonalCollocation <: CollocationMethod h::Int From ebe9df5de422382668f312a006f6827e3c43f2d8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 27 Feb 2026 15:16:03 -0500 Subject: [PATCH 21/49] doc: cleanup in `OrthogonalCollocation` equations --- src/controller/transcription.jl | 131 +++++++++++++++++++++----------- 1 file changed, 88 insertions(+), 43 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 96f04353e..28b982ac2 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -133,21 +133,22 @@ but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` -where ``\mathbf{K}`` comprises all the intermediate stages of the deterministic state only +where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic state only (the first `nx` elements of ``\mathbf{x̂}``): ```math \mathbf{K} = \begin{bmatrix} - \mathbf{k}_{1}(k+0) \\ - \mathbf{k}_{2}(k+0) \\ + \mathbf{k}(k+0) \\ + \mathbf{k}(k+1) \\ \vdots \\ - \mathbf{k}_{n_o}(k+0) \\ - \mathbf{k}_{1}(k+1) \\ - \mathbf{k}_{2}(k+1) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} + \mathbf{k}(k+H_p-1) \end{bmatrix} ``` -and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation -point at the ``j``th stage/interval/finite element (details in Extended Help). +and ``\mathbf{k}(k+j)`` comprises the deterministic state predictions for the ``n_o`` +collocation points at the ``j``th stage/interval/finite element (details in Extended Help). + +This transcription computes the predictions by enforcing the collocation and continuity +constraints at the collocation points. It is efficient for highly stiff systems, but +generally more expensive than the other methods for non-stiff systems. See Extended Help for +more details. !!! warning The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the @@ -164,22 +165,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). The collocation points are the roots of orthogonal polynomials, which are optimal for approximating the state trajectories with polynomials of degree ``n_o``. The method then - enforces the system dynamics at these collocation points, which leads to a more accurate - solution than the trapezoidal rule for the same number of collocation points. - - The differentiation ``\mathbf{M_o}`` and continuity ``\mathbf{C_o}`` matrices are: - ```math - \begin{aligned} - \mathbf{M_o} &= \begin{bmatrix} - \mathbf{Ṗ_o} \mathbf{P_o}^{-1} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{0} & \mathbf{Ṗ_o} \mathbf{P_o}^{-1} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \end{bmatrix} \\ - \mathbf{C_o} &= [\begin{smallmatrix}\mathbf{C_o^{x̂}} & \mathbf{C_o^{x̂}} & ... & \mathbf{C_o^{x̂}}\end{smallmatrix}] - \end{aligned} - ``` - where ``\mathbf{P_o}`` is the polynomial matrix (w/o the Y-intercept term) and - ``\mathbf{Ṗ_o}`` is its derivative matrix, both evaluated at the collocation points. + enforces the system dynamics at these collocation points. See [`con_nonlinprogeq!`](@ref) + for details on the implementation. """ struct OrthogonalCollocation <: CollocationMethod h::Int @@ -1089,10 +1076,11 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augm [`f̂!`](@ref) and [`ĥ!`](@ref) functions is called recursively in a `for` loop: ```math \begin{aligned} -\mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d̂_0}(k) \Big) \\ -\mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d̂_0}(k) \Big) +\mathbf{x̂_0}(k+j+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k+j), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j) \Big) \\ +\mathbf{ŷ_0}(k+j) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k+j), \mathbf{d̂_0}(k+j) \Big) \end{aligned} ``` +for ``j = 0, 1, ... , H_p``. """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K, @@ -1131,9 +1119,10 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionM The method mutates `Ŷ0` and `x̂0end` arguments. The augmented output function [`ĥ!`](@ref) is called multiple times in a `for` loop: ```math -\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d̂_0}(k) \Big) +\mathbf{ŷ_0}(k+j) = \mathbf{ĥ}\Big(\mathbf{x̂_0}(k+j), \mathbf{d̂_0}(k+j) \Big) ``` -in which ``\mathbf{x̂_0}`` is the augmented state extracted from the decision variable `Z̃`. +for ``j = 1, 2, ... , H_p``, and in which ``\mathbf{x̂_0}`` is the augmented state extracted +from the decision variable `Z̃`. """ function predict!( Ŷ0, x̂0end, _, _, _, @@ -1245,18 +1234,19 @@ end geq, X̂0, Û0, K mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ - ) + ) -> geq Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). The method mutates the `geq`, `X̂0`, `Û0` and `K` vectors in argument. The nonlinear equality constraints `geq` only includes the augmented state defects, computed with: ```math -\mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d̂_0}(k)\Big) - - \mathbf{x̂_0}(k+1) +\mathbf{ŝ}(k+j+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k+j), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j)\Big) + - \mathbf{x̂_0}(k+j+1) ``` -in which the augmented state ``\mathbf{x̂_0}`` are extracted from the decision variables -`Z̃`, and ``\mathbf{f̂}`` is the augmented state function defined in [`f̂!`](@ref). +for ``j = 0, 1, ... , H_p-1``, and in which the augmented state ``\mathbf{x̂_0}`` are +extracted from the decision variables `Z̃`, and ``\mathbf{f̂}`` is the augmented state +function defined in [`f̂!`](@ref). """ function con_nonlinprogeq!( geq, X̂0, Û0, K, @@ -1293,7 +1283,7 @@ end geq, X̂0, Û0, K mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ - ) + ) -> geq Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). @@ -1305,18 +1295,18 @@ time state-space models, and the stochastic model of the unmeasured disturbances is discrete-time. The deterministic and stochastic defects are respectively computed with: ```math \begin{aligned} -\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0}(k+1) +\mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1) + 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\ -\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s}(k+1) +\mathbf{s_s}(k+j+1) &= \mathbf{A_s x_s}(k+j) - \mathbf{x_s}(k+j+1) \end{aligned} ``` -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}`` and ``\mathbf{x_s}`` are the +deterministic and stochastic states extracted from the decision variables `Z̃`. The +``\mathbf{k}`` coefficients are evaluated from the continuous-time function `model.f!` and: ```math \begin{aligned} -\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d̂_0}(k) \Big) \\ -\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k+h), \mathbf{d̂_0}(k+1)\Big) +\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d̂_0}(k), \mathbf{p}\Big) \\ +\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k+h), \mathbf{d̂_0}(k+1), \mathbf{p}\Big) \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input is: @@ -1384,6 +1374,61 @@ function con_nonlinprogeq!( return geq end + +@doc raw""" + con_nonlinprogeq!( + geq, X̂0, Û0, K, + mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, + U0, Z̃ + ) -> geq + +Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). + +Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the +interval ``[0, 1]``, each state trajectories are approximated by a distinct polynomial +of degree ``n_o``. + +The defects between the deterministic state derivative at the collocation points and the +model dynamics are computed by: +```math +\begin{aligned} +\mathbf{s_k}(k+j+1) + & = \mathbf{M_o} [\mathbf{k}(k+j+1) - \mathbf{x_0}(k+j+1)] + & - \begin{bmatrix} + \mathbf{f}\Big(\mathbf{k}_1(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ + \mathbf{f}\Big(\mathbf{k}_2(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ + \vdots \\ + \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} +``` +for ``j = 0, 1, ... , H_p-1``, and knowing that ``\mathbf{k}(k+j+1)`` include all the +``\mathbf{k}_i(k+j+1)`` collocation points. The defects related to the continuity of +the deterministic state trajectories are given by: +```math + \mathbf{s_c}(k+j+1) = \mathbf{C_o} \mathbf{k}(k+j) - \mathbf{x_0}(k+j+1) +``` +for ``j = 0, 1, ... , H_p-1``. The defects of the stochastic states ``\mathbf{s_s}`` are +computed as in the [`TrapezoidalCollocation`](@ref) method. The differentiation +``\mathbf{M_o}`` and continuity ``\mathbf{C_o}`` matrices +```math +\begin{aligned} + \mathbf{P_o} &= \begin{bmatrix} + τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ + τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ + \mathbf{Ṗ_o} &= \begin{bmatrix} + 1*τ_1^0 \mathbf{I} & 2*τ_1^1 \mathbf{I} & \cdots & n_o*τ_1^{n_o-1} \mathbf{I} \\ + 1*τ_2^0 \mathbf{I} & 2*τ_2^1 \mathbf{I} & \cdots & n_o*τ_2^{n_o-1} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + 1*τ_{n_o}^0 \mathbf{I} & 2*τ_{n_o}^1 \mathbf{I} & \cdots & n_o*τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ + \mathbf{M_o} &= \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ + \mathbf{C_o} &= \begin{bmatrix} + L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} +\end{aligned} +``` +where ``\mathbf{P_o}`` is the polynomial matrix (w/o the Y-intercept term), +``\mathbf{Ṗ_o}``, its derivative matrix, and ``L_o(τ)``, the Lagrange basis polynomials. +""" function con_nonlinprogeq!( geq, X̂0, Û0, K, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, From ea5c1e5ac2135c85bdccc1e231105364a15ecd7f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 27 Feb 2026 15:39:27 -0500 Subject: [PATCH 22/49] doc: idem --- src/controller/transcription.jl | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 28b982ac2..4e1c85e42 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -122,7 +122,9 @@ end @doc raw""" - OrthogonalCollocation(h::Int=0, no=5; f_threads=false, h_threads=false) + OrthogonalCollocation( + h::Int=0, no=5; f_threads=false, h_threads=false, roots=:gaussradau + ) Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). @@ -133,7 +135,7 @@ but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` -where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic state only +where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic states (the first `nx` elements of ``\mathbf{x̂}``): ```math \mathbf{K} = \begin{bmatrix} @@ -142,8 +144,10 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi \vdots \\ \mathbf{k}(k+H_p-1) \end{bmatrix} ``` -and ``\mathbf{k}(k+j)`` comprises the deterministic state predictions for the ``n_o`` +and ``\mathbf{k}(k+j)`` includes the deterministic state predictions for the ``n_o`` collocation points at the ``j``th stage/interval/finite element (details in Extended Help). +The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for the roots of +the Gauss-Radau or Gauss-Legendre quadrature, respectively. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocation points. It is efficient for highly stiff systems, but @@ -165,8 +169,8 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). The collocation points are the roots of orthogonal polynomials, which are optimal for approximating the state trajectories with polynomials of degree ``n_o``. The method then - enforces the system dynamics at these collocation points. See [`con_nonlinprogeq!`](@ref) - for details on the implementation. + enforces the system dynamics at these points. See [`con_nonlinprogeq!`](@ref) for + details on the implementation. """ struct OrthogonalCollocation <: CollocationMethod h::Int @@ -1399,6 +1403,7 @@ model dynamics are computed by: \mathbf{f}\Big(\mathbf{k}_2(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ \vdots \\ \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} +\end{aligned} ``` for ``j = 0, 1, ... , H_p-1``, and knowing that ``\mathbf{k}(k+j+1)`` include all the ``\mathbf{k}_i(k+j+1)`` collocation points. The defects related to the continuity of From d0789afa1673be219d2cbbb51d6a1c8cdc6182be Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 28 Feb 2026 02:34:27 -0500 Subject: [PATCH 23/49] doc: implicit product --- src/controller/transcription.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 4e1c85e42..9fc03684a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1422,11 +1422,11 @@ computed as in the [`TrapezoidalCollocation`](@ref) method. The differentiation \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ \mathbf{Ṗ_o} &= \begin{bmatrix} - 1*τ_1^0 \mathbf{I} & 2*τ_1^1 \mathbf{I} & \cdots & n_o*τ_1^{n_o-1} \mathbf{I} \\ - 1*τ_2^0 \mathbf{I} & 2*τ_2^1 \mathbf{I} & \cdots & n_o*τ_2^{n_o-1} \mathbf{I} \\ + 1τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ + 1τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ \vdots & \vdots & \ddots & \vdots \\ - 1*τ_{n_o}^0 \mathbf{I} & 2*τ_{n_o}^1 \mathbf{I} & \cdots & n_o*τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ - \mathbf{M_o} &= \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ + 1τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ + \mathbf{M_o} &= \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ \mathbf{C_o} &= \begin{bmatrix} L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \end{aligned} From 8e6ac65d0e5e6c04039ae44a2bbbf48d9be74903 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 16:43:41 -0500 Subject: [PATCH 24/49] doc: cleanup `OrthogonalCollocation` --- docs/src/internals/state_estim.md | 1 + src/controller/nonlinmpc.jl | 5 +- src/controller/transcription.jl | 142 +++++++++++++++--------------- src/estimator/execute.jl | 11 ++- 4 files changed, 82 insertions(+), 77 deletions(-) diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 6b92cc2d7..2a8df67d0 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -27,6 +27,7 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! +ModelPredictiveControl.f̂_input! ``` ## Update Quadratic Optimization diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6259c7709..0c4e7ebf2 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -44,6 +44,7 @@ struct NonLinMPC{ p::PT Mo::SparseMatrixCSC{NT, Int} Co::SparseMatrixCSC{NT, Int} + λo::NT R̂u::Vector{NT} R̂y::Vector{NT} lastu0::Vector{NT} @@ -122,7 +123,7 @@ struct NonLinMPC{ d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) - Mo, Co = init_orthocolloc(model, transcription) + Mo, Co, λo = init_orthocolloc(model, transcription) nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) @@ -133,7 +134,7 @@ struct NonLinMPC{ Hp, Hc, nϵ, nb, weights, JE, p, - Mo, Co, + Mo, Co, λo, R̂u, R̂y, lastu0, P̃Δu, P̃u, Tu, Tu_lastu0, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9fc03684a..899006a7d 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -167,10 +167,11 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). See the Extended Help of [`TrapezoidalCollocation`](@ref) to understand why the stochastic states are left out of the ``\mathbf{K}`` vector. - The collocation points are the roots of orthogonal polynomials, which are optimal for - approximating the state trajectories with polynomials of degree ``n_o``. The method then - enforces the system dynamics at these points. See [`con_nonlinprogeq!`](@ref) for - details on the implementation. + The collocation points are located at the roots of orthogonal polynomials, which is + "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. + The method then enforces the system dynamics at these points. The Gauss-Legendre scheme + is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. + See [`con_nonlinprogeq!`](@ref) for details on the implementation. """ struct OrthogonalCollocation <: CollocationMethod h::Int @@ -201,7 +202,7 @@ struct OrthogonalCollocation <: CollocationMethod end """ - init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, o + init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo Init the differentiation `Mo` and continuity `Co` matrices of [`OrthogonalCollocation`](@ref). """ @@ -222,21 +223,26 @@ function init_orthocolloc( Co = Matrix{NT}(undef, nx, nx*no) for j=1:no iCols = (1:nx) .+ nx*(j-1) - Co[:, iCols] = lagrange_end(j, τ)*I(nx) + Co[:, iCols] = lagrange_end(j, transcription)*I(nx) end Co = sparse(Co) - return Mo, Co + λo = lagrange_end(0, transcription) + display(Co) + display(λo) + return Mo, Co, λo end "Return empty sparse matrices for other [`TranscriptionMethod`](@ref)" -init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0) +init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN "Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." -function lagrange_end(j, τ_values) - τ_val = 1 # evaluating the Lagrange polynomial at τ=1 (the end of the interval) - τj = τ_values[j] +function lagrange_end(j, transcription::OrthogonalCollocation) + τ_val = 1 + τ_values = [0; transcription.τ] # including the τ=0 node for the Lagrange polynomials + j_index = j + 1 # because of the τ=0 node + τj = τ_values[j_index] Lj = 1 for i in eachindex(τ_values) - i == j && continue + i == j_index && continue τi = τ_values[i] Lj *= (τ_val - τi)/(τj - τi) end @@ -1300,7 +1306,7 @@ is discrete-time. The deterministic and stochastic defects are respectively comp ```math \begin{aligned} \mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1) - + 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\ + + 0.5 T_s [\mathbf{k}_1(k+j) + \mathbf{k}_2(k+j)] \\ \mathbf{s_s}(k+j+1) &= \mathbf{A_s x_s}(k+j) - \mathbf{x_s}(k+j+1) \end{aligned} ``` @@ -1309,15 +1315,12 @@ deterministic and stochastic states extracted from the decision variables `Z̃`. ``\mathbf{k}`` coefficients are evaluated from the continuous-time function `model.f!` and: ```math \begin{aligned} -\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d̂_0}(k), \mathbf{p}\Big) \\ -\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k+h), \mathbf{d̂_0}(k+1), \mathbf{p}\Big) +\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) \\ +\mathbf{k}_2(k+j) &= \mathbf{f}\Big(\mathbf{x_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 is: -```math -\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) -``` -the ``\mathbf{A_s, C_{s_u}}`` matrices are defined in [`init_estimstoch`](@ref) doc. +in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` +is defined in [`f̂_input!`](@ref). """ function con_nonlinprogeq!( geq, X̂0, Û0, K, @@ -1388,32 +1391,35 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). -Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the -interval ``[0, 1]``, each state trajectories are approximated by a distinct polynomial -of degree ``n_o``. - -The defects between the deterministic state derivative at the collocation points and the -model dynamics are computed by: +The defects between the deterministic state derivative at the ``n_o`` collocation points and +the model dynamics are computed by: ```math \begin{aligned} -\mathbf{s_k}(k+j+1) - & = \mathbf{M_o} [\mathbf{k}(k+j+1) - \mathbf{x_0}(k+j+1)] - & - \begin{bmatrix} - \mathbf{f}\Big(\mathbf{k}_1(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ - \mathbf{f}\Big(\mathbf{k}_2(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ +\mathbf{s_k}(k+j+1) & + = \mathbf{M_o} [\mathbf{k}(k+j+1) - \mathbf{x_0}(k+j+1)] \\ &\quad + - \begin{bmatrix} + \mathbf{f}\Big(\mathbf{k}_1(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ + \mathbf{f}\Big(\mathbf{k}_2(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ \vdots \\ - \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j+1), \mathbf{u_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} + \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} \end{aligned} ``` -for ``j = 0, 1, ... , H_p-1``, and knowing that ``\mathbf{k}(k+j+1)`` include all the -``\mathbf{k}_i(k+j+1)`` collocation points. The defects related to the continuity of -the deterministic state trajectories are given by: +for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}(k+j+1)`` vectors are +extracted from the decision variable `Z̃` and they include all the ``\mathbf{k}_i(k+j+1)`` +collocation points. The vectors ``\mathbf{x_0}(k+j+1)`` are the deterministic state for time +``k+j+1``, also extracted from `Z̃`. 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 +in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint +of the deterministic state trajectories are given by: ```math - \mathbf{s_c}(k+j+1) = \mathbf{C_o} \mathbf{k}(k+j) - \mathbf{x_0}(k+j+1) +\mathbf{s_c}(k+j+1) = λ_o \mathbf{x_0}(k+j) + \mathbf{C_o k}(k+j) - \mathbf{x_0}(k+j+1) ``` -for ``j = 0, 1, ... , H_p-1``. The defects of the stochastic states ``\mathbf{s_s}`` are -computed as in the [`TrapezoidalCollocation`](@ref) method. The differentiation -``\mathbf{M_o}`` and continuity ``\mathbf{C_o}`` matrices +for ``j = 0, 1, ... , H_p-1``. + +Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the +interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct +polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity +matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: ```math \begin{aligned} \mathbf{P_o} &= \begin{bmatrix} @@ -1422,17 +1428,21 @@ computed as in the [`TrapezoidalCollocation`](@ref) method. The differentiation \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ \mathbf{Ṗ_o} &= \begin{bmatrix} - 1τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ - 1τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ + τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ + τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ \vdots & \vdots & \ddots & \vdots \\ - 1τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ - \mathbf{M_o} &= \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - \mathbf{C_o} &= \begin{bmatrix} + τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ + \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ + \mathbf{λ_o} &= L_0(1) \\ + \mathbf{C_o} &= \begin{bmatrix} L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \end{aligned} ``` -where ``\mathbf{P_o}`` is the polynomial matrix (w/o the Y-intercept term), -``\mathbf{Ṗ_o}``, its derivative matrix, and ``L_o(τ)``, the Lagrange basis polynomials. +where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values, and ``\mathbf{Ṗ_o}``, +to evaluate its derivatives. The Lagrange polynomial basis ``L_j(τ)`` are defined as: +```math +L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} +``` """ function con_nonlinprogeq!( geq, X̂0, Û0, K, @@ -1444,7 +1454,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads p = model.p - no, Mo, Co = transcription.no, mpc.Mo, mpc.Co + no, Mo, Co, λo = transcription.no, transcription.Mo, transcription.Co, transcription.λo nk = get_nk(model, transcription) 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)] @@ -1458,14 +1468,15 @@ function con_nonlinprogeq!( end 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)] - ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] + x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] + ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] + scnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] + sknext = @views geq[(1 + nx̂*Hp + (j-1)*no*nx):(nx̂*Hp + j*no*nx)] x0 = @views x̂0[1:nx] xsnext = @views x̂0next[nx+1:end] x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] - sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] + # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ @@ -1473,18 +1484,7 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0, u0) - # something like this: - # sdnext .= @. x0 - x0next_Z̃ - - - if h < 1 || j ≥ Hp - # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 - û0next = û0 - else - u0next = @views U0[(1 + nu*j):(nu*(j+1))] - û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) - end + # TODO: remove this allocation Δk = similar(k) @@ -1493,19 +1493,15 @@ function con_nonlinprogeq!( ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] Δko .= ko_Z̃ .- x0 - if o < no - model.f!(ko, ko_Z̃, û0, d̂0, p) - else - model.f!(ko, ko_Z̃, û0next, d̂0next, p) - end + model.f!(ko, ko_Z̃, û0, d̂0, p) end - # TODO: remove this allocation - display(Δk) + # TODO: remove the following allocations + #display(Δk) ẋ0 = Mo*Δk - display(ẋ0) + #display(ẋ0) - sdnext .= @. ẋ0 - k - + sknext .= ẋ0 .- k + scnext .= Co*x0 .+ λo*k - x0next_Z̃ end return geq end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 06b6e13d6..717b66b15 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -108,7 +108,7 @@ function f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, return nothing end -#TODO: delete the following two generic functions and replace with linear eq. constraints +#TODO: delete the following generic functions and replace with linear eq. constraints """ fs!(x̂0next, estim::StateEstimator, model::SimModel, x̂0) -> nothing @@ -124,7 +124,14 @@ end @doc raw""" f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) -> nothing -Compute the disturbed input of the augmented model ``\mathbf{û_0}`` from `x̂0` and `u0`. +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] From 8d07cce47ca5ebac6742b2d326e647edbb19a646 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 17:22:38 -0500 Subject: [PATCH 25/49] =?UTF-8?q?changed:=20renaming=20`K`=20arg=20to=20`K?= =?UTF-8?q?=CC=87`=20for=20clarity.=20The=20mutated=20argument=20is=20used?= =?UTF-8?q?=20to=20store=20the=20result=20of=20`model.f!`=20function,=20th?= =?UTF-8?q?us=20it=20is=20a=20derivative=20of=20the=20state.=20The=20disti?= =?UTF-8?q?nction=20is=20more=20important=20for=20code=20readability=20of?= =?UTF-8?q?=20`CollocationMethods`.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 56 ++++++++++++++++----------------- src/estimator/mhe/construct.jl | 4 +-- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 899006a7d..f1c29559d 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1290,40 +1290,39 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K + geq, X̂0, Û0, K̇ mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) -> geq 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: +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: ```math \begin{aligned} \mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1) - + 0.5 T_s [\mathbf{k}_1(k+j) + \mathbf{k}_2(k+j)] \\ + + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] \\ \mathbf{s_s}(k+j+1) &= \mathbf{A_s x_s}(k+j) - \mathbf{x_s}(k+j+1) \end{aligned} ``` 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: +``\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) \\ -\mathbf{k}_2(k+j) &= \mathbf{f}\Big(\mathbf{x_0}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d̂_0}(k+j+1), \mathbf{p}\Big) +\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) \\ +\mathbf{k̇}_2(k+j) &= \mathbf{f}\Big(\mathbf{x_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̂_input!`](@ref). """ function con_nonlinprogeq!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̇, mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) @@ -1343,7 +1342,7 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k = @views K[(1 + nk*(j-1)):(nk*j)] + 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)] @@ -1352,7 +1351,7 @@ function con_nonlinprogeq!( xsnext = @views x̂0next[nx+1:end] x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - k1, k2 = @views k[1:nx], k[nx+1:2*nx] + k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ @@ -1363,9 +1362,9 @@ function con_nonlinprogeq!( 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!(k1, x0, û0, d̂0, p) + model.f!(k̇1, x0, û0, d̂0, p) else - k1 .= @views K[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end if h < 1 || j ≥ Hp # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 @@ -1375,8 +1374,8 @@ function con_nonlinprogeq!( û0next = @views Û0[(1 + nu*j):(nu*(j+1))] f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) end - model.f!(k2, x0next_Z̃, û0next, d̂0next, p) - sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2) + model.f!(k̇2, x0next_Z̃, û0next, d̂0next, p) + sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k̇1 + k̇2) end return geq end @@ -1384,15 +1383,16 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̇, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) -> geq Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocation`](@ref). -The defects between the deterministic state derivative at the ``n_o`` collocation points and -the model dynamics are computed by: +The method mutates the `geq`, `X̂0`, `Û0` and `K̇` vectors in argument. The defects between +the deterministic state derivative at the ``n_o`` collocation points and the model dynamics +are computed by: ```math \begin{aligned} \mathbf{s_k}(k+j+1) & @@ -1445,7 +1445,7 @@ L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` """ function con_nonlinprogeq!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̇, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) @@ -1466,7 +1466,7 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k = @views K[(1 + nk*(j-1)):(nk*j)] + k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*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)] @@ -1487,21 +1487,21 @@ function con_nonlinprogeq!( # TODO: remove this allocation - Δk = similar(k) + Δk = similar(k̇) for o=1:no - ko = @views k[(1 + (o-1)*nx):(o*nx)] + k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] Δko .= ko_Z̃ .- x0 - model.f!(ko, ko_Z̃, û0, d̂0, p) + model.f!(k̇o, ko_Z̃, û0, d̂0, p) end # TODO: remove the following allocations #display(Δk) ẋ0 = Mo*Δk #display(ẋ0) - sknext .= ẋ0 .- k - scnext .= Co*x0 .+ λo*k - x0next_Z̃ + sknext .= ẋ0 .- k̇ + scnext .= Co*x0 .+ λo*k̇ - x0next_Z̃ end return geq end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e452cb686..a0abc1d0e 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1385,7 +1385,7 @@ function get_nonlinobj_op( myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) + k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) @@ -1490,7 +1490,7 @@ function get_nonlincon_oracle( strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) + k::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT}, gi::Vector{JNT} = zeros(JNT, ng), zeros(JNT, ngi) λi::Vector{JNT} = rand(JNT, ngi) From 8b4197c6627dd7f91dd32b2067054d4efa3b1a17 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 17:36:17 -0500 Subject: [PATCH 26/49] debug: correct field location --- 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 f1c29559d..3a15fc831 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1454,7 +1454,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads p = model.p - no, Mo, Co, λo = transcription.no, transcription.Mo, transcription.Co, transcription.λo + no, Mo, Co, λo = transcription.no, mpc.Mo, mpc.Co, mpc.λo nk = get_nk(model, transcription) 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)] From bed52aa0039dcad735559c7040c6788807d26a97 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 17:44:43 -0500 Subject: [PATCH 27/49] =?UTF-8?q?added:=20`OrthogonalCollocation`=20now=20?= =?UTF-8?q?works=20=F0=9F=8D=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 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 3a15fc831..e9b80c9d4 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1433,7 +1433,7 @@ matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed wit \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - \mathbf{λ_o} &= L_0(1) \\ + λ_o &= L_0(1) \\ \mathbf{C_o} &= \begin{bmatrix} L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \end{aligned} @@ -1501,7 +1501,7 @@ function con_nonlinprogeq!( #display(ẋ0) sknext .= ẋ0 .- k̇ - scnext .= Co*x0 .+ λo*k̇ - x0next_Z̃ + scnext .= λo*x0 + Co*k_Z̃ - x0next_Z̃ end return geq end From 0e605cf9ccf4c2eb981786c5e14e87cb0791c279 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 17:45:33 -0500 Subject: [PATCH 28/49] removed: useless print --- src/controller/transcription.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e9b80c9d4..05ee8cb32 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -227,8 +227,6 @@ function init_orthocolloc( end Co = sparse(Co) λo = lagrange_end(0, transcription) - display(Co) - display(λo) return Mo, Co, λo end "Return empty sparse matrices for other [`TranscriptionMethod`](@ref)" From ae0c22f24674dc62ce284ab244edc6ce2fefed37 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Mar 2026 17:47:18 -0500 Subject: [PATCH 29/49] doc: minor correction --- 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 05ee8cb32..c58f5fb43 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1437,7 +1437,7 @@ matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed wit \end{aligned} ``` where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values, and ``\mathbf{Ṗ_o}``, -to evaluate its derivatives. The Lagrange polynomial basis ``L_j(τ)`` are defined as: +to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: ```math L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` From 29e12c4335f952dbfdbb2300285cfbf8cd65ea18 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 10:51:05 -0500 Subject: [PATCH 30/49] doc: moving collocation matrices to seperate docstring --- docs/src/internals/predictive_control.md | 1 + src/controller/transcription.jl | 112 ++++++++++++----------- 2 files changed, 59 insertions(+), 54 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index ef79ef649..7babd1305 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -25,6 +25,7 @@ ModelPredictiveControl.relaxterminal ModelPredictiveControl.augmentdefect ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred +ModelPredictiveControl.init_orthocolloc ModelPredictiveControl.init_matconstraint_mpc ModelPredictiveControl.get_nonlinobj_op(::NonLinMPC, ::ModelPredictiveControl.GenericModel) ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveControl.GenericModel) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c58f5fb43..2e1df6646 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -201,10 +201,38 @@ struct OrthogonalCollocation <: CollocationMethod end end -""" +@doc raw""" init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo -Init the differentiation `Mo` and continuity `Co` matrices of [`OrthogonalCollocation`](@ref). +Init the differentiation and continuity matrices of [`OrthogonalCollocation`](@ref). + +Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the +interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct +polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity +matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: +```math +\begin{aligned} + \mathbf{P_o} &= \begin{bmatrix} + τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ + τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ + \mathbf{Ṗ_o} &= \begin{bmatrix} + τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ + τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ + \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ + λ_o &= L_0(1) \\ + \mathbf{C_o} &= \begin{bmatrix} + L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} +\end{aligned} +``` +where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values, and ``\mathbf{Ṗ_o}``, +to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: +```math +L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} +``` """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation @@ -1403,44 +1431,20 @@ are computed by: \end{aligned} ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}(k+j+1)`` vectors are -extracted from the decision variable `Z̃` and they include all the ``\mathbf{k}_i(k+j+1)`` -collocation points. The vectors ``\mathbf{x_0}(k+j+1)`` are the deterministic state for time -``k+j+1``, also extracted from `Z̃`. 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 -in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint -of the deterministic state trajectories are given by: +extracted from the decision variable `Z̃` and they incorporate all the collocation points +``\mathbf{k}_i(k+j+1)`` for ``i = 1, 2, ..., n_o``, hence `nx*no` elements. The vectors +``\mathbf{x_0}(k+j+1)`` are the deterministic state for time ``k+j+1``, also extracted from +`Z̃`. 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 in the [`TrapezoidalCollocation`](@ref) +method, and the ones for the continuity constraint of the deterministic state trajectories +are given by: ```math \mathbf{s_c}(k+j+1) = λ_o \mathbf{x_0}(k+j) + \mathbf{C_o k}(k+j) - \mathbf{x_0}(k+j+1) ``` for ``j = 0, 1, ... , H_p-1``. -Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the -interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct -polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity -matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: -```math -\begin{aligned} - \mathbf{P_o} &= \begin{bmatrix} - τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ - τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ - \mathbf{Ṗ_o} &= \begin{bmatrix} - τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ - τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ - \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - λ_o &= L_0(1) \\ - \mathbf{C_o} &= \begin{bmatrix} - L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} -\end{aligned} -``` -where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values, and ``\mathbf{Ṗ_o}``, -to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: -```math -L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} -``` +The differentiation matrix ``\mathbf{M_o}`` and the continuity matrix ``\mathbf{C_o}`` and +coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref) documentation. """ function con_nonlinprogeq!( geq, X̂0, Û0, K̇, @@ -1454,6 +1458,7 @@ function con_nonlinprogeq!( p = model.p no, Mo, Co, λo = transcription.no, mpc.Mo, mpc.Co, mpc.λo nk = get_nk(model, transcription) + 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)] @threadsif f_threads for j=1:Hp @@ -1464,26 +1469,24 @@ function con_nonlinprogeq!( x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] - k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*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)] - ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - scnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - sknext = @views geq[(1 + nx̂*Hp + (j-1)*no*nx):(nx̂*Hp + j*no*nx)] - x0 = @views x̂0[1:nx] - xsnext = @views x̂0next[nx+1:end] - x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] - + k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] + k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*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̂)] + sknext = @views geq[(1 + nx̂_nk*(j-1) + nx̂):(nx̂_nk*j )] + x0 = @views x̂0[1:nx] + x0next_Z̃ = @views x̂0next_Z̃[1:nx] + xsnext = @views x̂0next[nx+1:end] + xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] # ----------------- stochastic defects ----------------------------------------- fs!(x̂0next, mpc.estim, model, x̂0) ssnext .= @. xsnext - xsnext_Z̃ - # ----------------- deterministic defects: orthogonal collocation -------------- + # ----------------- collocation point defects ---------------------------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0, u0) - - # TODO: remove this allocation Δk = similar(k̇) for o=1:no @@ -1494,13 +1497,14 @@ function con_nonlinprogeq!( model.f!(k̇o, ko_Z̃, û0, d̂0, p) end # TODO: remove the following allocations - #display(Δk) - ẋ0 = Mo*Δk - #display(ẋ0) - - sknext .= ẋ0 .- k̇ + k̇_Z̃ = Mo*Δk + sknext .= @. k̇_Z̃ - k̇ + # ----------------- continuity constraint defects ------------------------------ scnext .= λo*x0 + Co*k_Z̃ - x0next_Z̃ end + if eltype(geq) == Float64 + println(geq) + end return geq end From 7e3afe6c54e23242c707227855dea6a7164d6b72 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 11:23:48 -0500 Subject: [PATCH 31/49] =?UTF-8?q?changed:=20default=20`no`=20in=20`Orthogo?= =?UTF-8?q?nalCollocation`=20The=20default=20of=203=20collocation=20points?= =?UTF-8?q?=20(the=20internal=20points,=20thus=20it=20exclude=20the=20?= =?UTF-8?q?=CF=84=3D0=20point)=20is=20the=20most=20common=20default=20(e.g?= =?UTF-8?q?.=20do-mpc,=20CasADi,=20etc.)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 2e1df6646..d48d2c6e9 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -123,7 +123,7 @@ end @doc raw""" OrthogonalCollocation( - h::Int=0, no=5; f_threads=false, h_threads=false, roots=:gaussradau + h::Int=0, no=3; f_threads=false, h_threads=false, roots=:gaussradau ) Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). @@ -180,7 +180,7 @@ struct OrthogonalCollocation <: CollocationMethod h_threads::Bool τ::Vector{COLLOCATION_NODE_TYPE} function OrthogonalCollocation( - h::Int=0, no::Int=5; f_threads=false, h_threads=false, roots=:gaussradau + h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) if !(h == 0) throw(ArgumentError("Only the zero-order hold (h=0) is currently implemented.")) @@ -204,7 +204,7 @@ end @doc raw""" init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo -Init the differentiation and continuity matrices of [`OrthogonalCollocation`](@ref). +Init the differentiation and continuity matrices for [`OrthogonalCollocation`](@ref). Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct @@ -223,13 +223,14 @@ matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed wit \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - λ_o &= L_0(1) \\ \mathbf{C_o} &= \begin{bmatrix} - L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} + L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ + λ_o &= L_0(1) \end{aligned} ``` -where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values, and ``\mathbf{Ṗ_o}``, -to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: +where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values w/o the Y-intercept, +and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases +are defined as: ```math L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` @@ -257,7 +258,7 @@ function init_orthocolloc( λo = lagrange_end(0, transcription) return Mo, Co, λo end -"Return empty sparse matrices for other [`TranscriptionMethod`](@ref)" +"Return empty sparse matrices and `NaN` for other [`TranscriptionMethod`](@ref)" init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN "Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." @@ -1502,9 +1503,6 @@ function con_nonlinprogeq!( # ----------------- continuity constraint defects ------------------------------ scnext .= λo*x0 + Co*k_Z̃ - x0next_Z̃ end - if eltype(geq) == Float64 - println(geq) - end return geq end From 530a1be174d160df1331044843b9e09b36dfb1db Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 11:34:34 -0500 Subject: [PATCH 32/49] doc: details on `d` interpolation --- 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 d48d2c6e9..1bf0a4733 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -83,8 +83,8 @@ only. The decision variables are the same as for [`MultipleShooting`](@ref), hen computational costs. See the same docstring for descriptions of `f_threads` and `h_threads` keywords. The `h` argument is `0` or `1`, for piecewise constant or linear manipulated inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various [`DiffSolver`](@ref) -here assume zero-order hold, so `h=1` will induce a plant-model mismatch if the plant is -simulated with these solvers. +here assume zero-order hold, so `h=1` will induce a plant-model mismatch if the plant is +simulated with these solvers. Measured disturbances ``\mathbf{d}`` are piecewise linear. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle From 25105759646cc0c3e33033cbcc32f084c171dbf1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 12:40:54 -0500 Subject: [PATCH 33/49] doc: going back to `k_i` notation --- src/controller/transcription.jl | 36 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 1bf0a4733..4b036d3b8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1331,8 +1331,8 @@ and the stochastic model of the unmeasured disturbances is discrete-time. The de and stochastic defects are respectively computed with: ```math \begin{aligned} -\mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1) - + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] \\ +\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} ``` @@ -1422,23 +1422,25 @@ the deterministic state derivative at the ``n_o`` collocation points and the mod are computed by: ```math \begin{aligned} -\mathbf{s_k}(k+j+1) & - = \mathbf{M_o} [\mathbf{k}(k+j+1) - \mathbf{x_0}(k+j+1)] \\ &\quad - - \begin{bmatrix} - \mathbf{f}\Big(\mathbf{k}_1(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ - \mathbf{f}\Big(\mathbf{k}_2(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ - \vdots \\ - \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j+1), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} +\mathbf{s_k}(k+j+1) & + = \mathbf{M_o} \begin{bmatrix} + \mathbf{k}_1(k+j) - \mathbf{x_0}(k+j) \\ + \mathbf{k}_2(k+j) - \mathbf{x_0}(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) - \mathbf{x_0}(k+j) \\ \end{bmatrix} \\ &\quad + - \begin{bmatrix} + \mathbf{f}\Big(\mathbf{k}_1(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ + \mathbf{f}\Big(\mathbf{k}_2(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ + \vdots \\ + \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} \end{aligned} ``` -for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}(k+j+1)`` vectors are -extracted from the decision variable `Z̃` and they incorporate all the collocation points -``\mathbf{k}_i(k+j+1)`` for ``i = 1, 2, ..., n_o``, hence `nx*no` elements. The vectors -``\mathbf{x_0}(k+j+1)`` are the deterministic state for time ``k+j+1``, also extracted from -`Z̃`. 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 in the [`TrapezoidalCollocation`](@ref) -method, and the ones for the continuity constraint of the deterministic state trajectories -are given by: +for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are +extracted from the decision variable `Z̃`. The vectors ``\mathbf{x_0}(k+j)`` are the +deterministic state for time ``k+j``, also extracted from `Z̃`. 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 in the [`TrapezoidalCollocation`](@ref) method, and +the ones for the continuity constraint of the deterministic state trajectories are given by: ```math \mathbf{s_c}(k+j+1) = λ_o \mathbf{x_0}(k+j) + \mathbf{C_o k}(k+j) - \mathbf{x_0}(k+j+1) ``` From d3cad3794ed173a0f50b332ed331bf9736f39d57 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 12:49:07 -0500 Subject: [PATCH 34/49] doc: `sk` instead of `sknext` --- src/controller/transcription.jl | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 4b036d3b8..d65dca713 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -139,15 +139,19 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi (the first `nx` elements of ``\mathbf{x̂}``): ```math \mathbf{K} = \begin{bmatrix} - \mathbf{k}(k+0) \\ - \mathbf{k}(k+1) \\ + \mathbf{k}_{1}(k+0) \\ + \mathbf{k}_{2}(k+0) \\ \vdots \\ - \mathbf{k}(k+H_p-1) \end{bmatrix} + \mathbf{k}_{n_o}(k+0) \\ + \mathbf{k}_{1}(k+1) \\ + \mathbf{k}_{2}(k+1) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} ``` -and ``\mathbf{k}(k+j)`` includes the deterministic state predictions for the ``n_o`` -collocation points at the ``j``th stage/interval/finite element (details in Extended Help). -The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for the roots of -the Gauss-Radau or Gauss-Legendre quadrature, respectively. +and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation +point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` +keyword argument is either `:gaussradau` or `:gausslegendre`, for the roots of the +Gauss-Radau or Gauss-Legendre quadrature, respectively. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocation points. It is efficient for highly stiff systems, but @@ -164,8 +168,9 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). # Extended Help !!! details "Extended Help" - See the Extended Help of [`TrapezoidalCollocation`](@ref) to understand why the - stochastic states are left out of the ``\mathbf{K}`` vector. + As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic + states are left out of the ``\mathbf{K}`` vector since collocation methods required + continuous-time dynamics and the stochastic model is discrete. The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. @@ -1422,7 +1427,7 @@ the deterministic state derivative at the ``n_o`` collocation points and the mod are computed by: ```math \begin{aligned} -\mathbf{s_k}(k+j+1) & +\mathbf{s_k}(k+j) & = \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(k+j) - \mathbf{x_0}(k+j) \\ \mathbf{k}_2(k+j) - \mathbf{x_0}(k+j) \\ @@ -1435,7 +1440,7 @@ are computed by: \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} \end{aligned} ``` -for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are +for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_o(k+j)`` vectors are extracted from the decision variable `Z̃`. The vectors ``\mathbf{x_0}(k+j)`` are the deterministic state for time ``k+j``, also extracted from `Z̃`. The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for the stochastic @@ -1478,7 +1483,7 @@ function con_nonlinprogeq!( 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̂)] - sknext = @views geq[(1 + nx̂_nk*(j-1) + nx̂):(nx̂_nk*j )] + sk = @views geq[(1 + nx̂_nk*(j-1) + nx̂):(nx̂_nk*j )] x0 = @views x̂0[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] xsnext = @views x̂0next[nx+1:end] @@ -1501,7 +1506,7 @@ function con_nonlinprogeq!( end # TODO: remove the following allocations k̇_Z̃ = Mo*Δk - sknext .= @. k̇_Z̃ - k̇ + sk .= @. k̇_Z̃ - k̇ # ----------------- continuity constraint defects ------------------------------ scnext .= λo*x0 + Co*k_Z̃ - x0next_Z̃ end From b71a2f047545478acb4b59adbcf485f74cb714e8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 13:28:06 -0500 Subject: [PATCH 35/49] changed: clearer code in `TrapezoidalCollocation` --- src/controller/transcription.jl | 79 +++++++++++++++++---------------- 1 file changed, 40 insertions(+), 39 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d65dca713..0b6d1340f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -3,8 +3,8 @@ const COLLOCATION_NODE_TYPE = Float64 """ Abstract supertype of all transcription methods of [`PredictiveController`](@ref). -The module currently supports [`SingleShooting`](@ref), [`MultipleShooting`](@ref) and -[`TrapezoidalCollocation`](@ref) transcription methods. +The module currently supports [`SingleShooting`](@ref), [`MultipleShooting`](@ref), +[`TrapezoidalCollocation`](@ref) and [`OrthogonalCollocation`](@ref) transcription methods. """ abstract type TranscriptionMethod end abstract type ShootingMethod <: TranscriptionMethod end @@ -129,9 +129,9 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). Also known as pseudo-spectral method. The `h` argument is the hold order for ``\mathbf{u}``, -and `no`, the number of collocation points ``n_o``. Only zero-order hold is currently -implemented, so `h` must be `0`. The decision variable is similar to [`MultipleShooting`](@ref), -but it also includes the collocation points: +and `no` argument, the number of collocation points ``n_o``. Only zero-order hold is +currently implemented, so `h` must be `0`. The decision variable is similar to +[`MultipleShooting`](@ref), but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -150,8 +150,8 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi ``` and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` -keyword argument is either `:gaussradau` or `:gausslegendre`, for the roots of the -Gauss-Radau or Gauss-Legendre quadrature, respectively. +keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or +Gauss-Legendre quadrature, respectively. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocation points. It is efficient for highly stiff systems, but @@ -169,7 +169,7 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). # Extended Help !!! details "Extended Help" As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic - states are left out of the ``\mathbf{K}`` vector since collocation methods required + states are left out of the ``\mathbf{K}`` vector since collocation methods require continuous-time dynamics and the stochastic model is discrete. The collocation points are located at the roots of orthogonal polynomials, which is @@ -1302,11 +1302,11 @@ function con_nonlinprogeq!( X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] @threadsif f_threads for j=1:Hp if j < 2 - x̂0 = @views mpc.estim.x̂0[1:nx̂] - d̂0 = @views mpc.d0[1:nd] + x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] + d̂0 = @views mpc.d0[1:nd] else - x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] - d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] + x̂0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] + d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] @@ -1314,8 +1314,8 @@ function con_nonlinprogeq!( x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k, mpc.estim, model, x̂0, u0, d̂0) - ŝnext .= x̂0next .- x̂0next_Z̃ + f̂!(x̂0next, û0, k, mpc.estim, model, x̂0_Z̃, u0, d̂0) + ŝnext .= @. x̂0next - x̂0next_Z̃ end return geq end @@ -1368,33 +1368,34 @@ function con_nonlinprogeq!( X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] @threadsif f_threads for j=1:Hp if j < 2 - x̂0 = @views mpc.estim.x̂0[1:nx̂] - d̂0 = @views mpc.d0[1:nd] + x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] + d̂0 = @views mpc.d0[1:nd] else - x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] - d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] + x̂0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] + d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] 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)] - ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - x0 = @views x̂0[1:nx] - xsnext = @views x̂0next[nx+1:end] - x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] - sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] + sdnext = @views geq[(1 + nx̂*(j-1) ):(nx̂*(j-1) + nx)] + ssnext = @views geq[(1 + nx̂*(j-1) + nx):(nx̂*j )] + 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 ----------------------------------------- - fs!(x̂0next, mpc.estim, model, x̂0) + 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 ------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - f̂_input!(û0, mpc.estim, model, x̂0, u0) + f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) 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, x0, û0, d̂0, p) + model.f!(k̇1, x0_Z̃, û0, d̂0, p) else k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end @@ -1407,7 +1408,7 @@ function con_nonlinprogeq!( f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) end model.f!(k̇2, x0next_Z̃, û0next, d̂0next, p) - sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k̇1 + k̇2) + sdnext .= @. x0_Z̃ - x0next_Z̃ + 0.5*Ts*(k̇1 + k̇2) end return geq end @@ -1471,11 +1472,11 @@ function con_nonlinprogeq!( X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] @threadsif f_threads for j=1:Hp if j < 2 - x̂0 = @views mpc.estim.x̂0[1:nx̂] - d̂0 = @views mpc.d0[1:nd] + x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] + d̂0 = @views mpc.d0[1:nd] else - x̂0 = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] - d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] + x̂0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] + d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] @@ -1484,31 +1485,31 @@ function con_nonlinprogeq!( 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 )] - x0 = @views x̂0[1:nx] + x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] - xsnext = @views x̂0next[nx+1:end] - xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] # ----------------- stochastic defects ----------------------------------------- - fs!(x̂0next, mpc.estim, model, x̂0) + 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 point defects ---------------------------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - f̂_input!(û0, mpc.estim, model, x̂0, u0) + f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) # TODO: remove this allocation Δk = similar(k̇) for o=1:no k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] - Δko .= ko_Z̃ .- x0 + Δko .= ko_Z̃ .- x0_Z̃ model.f!(k̇o, ko_Z̃, û0, d̂0, p) end # TODO: remove the following allocations k̇_Z̃ = Mo*Δk sk .= @. k̇_Z̃ - k̇ # ----------------- continuity constraint defects ------------------------------ - scnext .= λo*x0 + Co*k_Z̃ - x0next_Z̃ + scnext .= λo*x0_Z̃ + Co*k_Z̃ - x0next_Z̃ end return geq end From 17c1f7181e4f6b987c4f6c25e296356626c3e494 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 13:32:42 -0500 Subject: [PATCH 36/49] doc: cleanup --- src/controller/transcription.jl | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0b6d1340f..e3e231822 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1442,18 +1442,16 @@ are computed by: \end{aligned} ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_o(k+j)`` vectors are -extracted from the decision variable `Z̃`. The vectors ``\mathbf{x_0}(k+j)`` are the -deterministic state for time ``k+j``, also extracted from `Z̃`. 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 in the [`TrapezoidalCollocation`](@ref) method, and -the ones for the continuity constraint of the deterministic state trajectories are given by: +extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the +deterministic state extracted from `Z̃`. 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 in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity +constraint of the deterministic state trajectories are given by: ```math \mathbf{s_c}(k+j+1) = λ_o \mathbf{x_0}(k+j) + \mathbf{C_o k}(k+j) - \mathbf{x_0}(k+j+1) ``` -for ``j = 0, 1, ... , H_p-1``. - -The differentiation matrix ``\mathbf{M_o}`` and the continuity matrix ``\mathbf{C_o}`` and -coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref) documentation. +for ``j = 0, 1, ... , H_p-1``. The differentiation matrix ``\mathbf{M_o}``, the continuity +matrix ``\mathbf{C_o}`` and the coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref). """ function con_nonlinprogeq!( geq, X̂0, Û0, K̇, From 94f86e77ef4b78fe7dd69f31eda7ad7c9a581765 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 16:03:59 -0500 Subject: [PATCH 37/49] debug: also warm-start the collocation points --- src/controller/transcription.jl | 77 ++++++++++++++++++++------------- 1 file changed, 46 insertions(+), 31 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e3e231822..4542b9480 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -146,7 +146,7 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi \mathbf{k}_{1}(k+1) \\ \mathbf{k}_{2}(k+1) \\ \vdots \\ - \mathbf{k}_{n_o}(k+H_p) \end{bmatrix} + \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} ``` and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` @@ -974,9 +974,10 @@ last control period ``k-1``, and ``ϵ(k-1)``, the slack variable of the last con """ 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 --- - Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] - Z̃s[(Hc*nu-nu+1):(Hc*nu)] .= 0 + Z̃s[1:(nΔU-nu)] .= @views mpc.Z̃[nu+1:nΔU] + Z̃s[(nΔU-nu+1):(nΔU)] .= 0 # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1001,25 +1002,35 @@ It warm-starts the solver at: \vdots \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ - \mathbf{k}_{1}(k+1|k-1) \\ - \mathbf{k}_{2}(k+1|k-1) \\ + \mathbf{k}(k+0|k-1) \\ + \mathbf{k}(k+1|k-1) \\ \vdots \\ - \mathbf{k}_{n_o}(k+1|k-1) \\ + \mathbf{k}(k+H_p-2|k-1) \\ + \mathbf{k}(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}}``. +``\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. """ -function set_warmstart!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) - nu, nx̂, Hp, Hc, Z̃s = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ +function set_warmstart!( + 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 # --- input increments ΔU --- - Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] - Z̃s[(Hc*nu-nu+1):(Hc*nu)] .= 0 + 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[(Hc*nu+1):(Hc*nu+Hp*nx̂-nx̂)] .= @views mpc.Z̃[(Hc*nu+nx̂+1):(Hc*nu+Hp*nx̂)] - Z̃s[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] .= @views mpc.Z̃[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] + 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)] # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1053,12 +1064,13 @@ last control period ``k-1``, expressed as a deviation from the operating point """ function set_warmstart!(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 # --- input increments ΔU --- - Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] - Z̃s[(Hc*nu-nu+1):(Hc*nu)] .= 0 + 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[(Hc*nu+1):(Hc*nu+Hp*nx̂-nx̂)] .= @views mpc.Z̃[(Hc*nu+nx̂+1):(Hc*nu+Hp*nx̂)] - Z̃s[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] .= @views mpc.Z̃[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] + 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̂)] # --- slack variable ϵ --- mpc.nϵ == 1 && (Z̃s[end] = mpc.Z̃[end]) JuMP.set_start_value.(Z̃var, Z̃s) @@ -1427,24 +1439,27 @@ The method mutates the `geq`, `X̂0`, `Û0` and `K̇` vectors in argument. The the deterministic state derivative at the ``n_o`` collocation points and the model dynamics are computed by: ```math -\begin{aligned} -\mathbf{s_k}(k+j) & +\mathbf{s_k}(k+j) = \mathbf{M_o} \begin{bmatrix} - \mathbf{k}_1(k+j) - \mathbf{x_0}(k+j) \\ - \mathbf{k}_2(k+j) - \mathbf{x_0}(k+j) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+j) - \mathbf{x_0}(k+j) \\ \end{bmatrix} \\ &\quad + \mathbf{k}_1(k+j) - \mathbf{x_0}(k+j) \\ + \mathbf{k}_2(k+j) - \mathbf{x_0}(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) - \mathbf{x_0}(k+j) \\ \end{bmatrix} - \begin{bmatrix} - \mathbf{f}\Big(\mathbf{k}_1(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ - \mathbf{f}\Big(\mathbf{k}_2(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ - \vdots \\ - \mathbf{f}\Big(\mathbf{k}_{n_o}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \end{bmatrix} -\end{aligned} + \mathbf{k̇}_1(k+j) \\ + \mathbf{k̇}_2(k+j) \\ + \vdots \\ + \mathbf{k̇}_{n_o}(k+j) \end{bmatrix} ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_o(k+j)`` vectors are extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the -deterministic state extracted from `Z̃`. The disturbed input ``\mathbf{û_0}(k+j)`` is defined -in [`f̂_input!`](@ref). The defects for the stochastic states ``\mathbf{s_s}`` are computed +deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_o`` vector for the ``o``th +collocation point is computed from the continuous-time function `model.f!` and: +```math +\mathbf{k̇}_o(k+j) = \mathbf{f}\Big(\mathbf{k_o}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) +``` +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 in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint of the deterministic state trajectories are given by: ```math @@ -1500,7 +1515,7 @@ function con_nonlinprogeq!( k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] - Δko .= ko_Z̃ .- x0_Z̃ + Δko .= @. ko_Z̃ - x0_Z̃ model.f!(k̇o, ko_Z̃, û0, d̂0, p) end # TODO: remove the following allocations From 5e6ceeba39c66fb119c64603b496d8a7994510ec Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 16:37:07 -0500 Subject: [PATCH 38/49] doc: minor correction --- src/controller/transcription.jl | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 4542b9480..3ebcab152 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -128,10 +128,11 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). -Also known as pseudo-spectral method. The `h` argument is the hold order for ``\mathbf{u}``, -and `no` argument, the number of collocation points ``n_o``. Only zero-order hold is -currently implemented, so `h` must be `0`. The decision variable is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points: +Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s +only. The `h` argument is the hold order for ``\mathbf{u}``, and `no` argument, the number +of collocation points ``n_o``. Only zero-order hold is currently implemented, so `h` must +be `0`. The decision variable is similar to [`MultipleShooting`](@ref), but it also includes +the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -1012,7 +1013,7 @@ It warm-starts the solver at: ``` 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 +``\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. """ function set_warmstart!( @@ -1453,10 +1454,10 @@ are computed by: ``` for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_o(k+j)`` vectors are extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the -deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_o`` vector for the ``o``th +deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_o`` derivative for the ``o``th collocation point is computed from the continuous-time function `model.f!` and: ```math -\mathbf{k̇}_o(k+j) = \mathbf{f}\Big(\mathbf{k_o}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) +\mathbf{k̇}_o(k+j) = \mathbf{f}\Big(\mathbf{k}_o(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) ``` The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for the stochastic states ``\mathbf{s_s}`` are computed From 51d3e431e4e183cfd1d1ab77f5a1d0163b02f8f8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 16:57:24 -0500 Subject: [PATCH 39/49] added: reduce allocations --- src/controller/transcription.jl | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 3ebcab152..6f6aa075b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1506,12 +1506,11 @@ function con_nonlinprogeq!( xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] fs!(x̂0next, mpc.estim, model, x̂0_Z̃) ssnext .= @. xsnext - xsnext_Z̃ - # ----------------- collocation point defects ---------------------------------- + # ----------------- collocation constraint defects ----------------------------- u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - # TODO: remove this allocation - Δk = similar(k̇) + Δk = similar(k̇) # TODO: remove this allocation for o=1:no k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] @@ -1519,11 +1518,9 @@ function con_nonlinprogeq!( Δko .= @. ko_Z̃ - x0_Z̃ model.f!(k̇o, ko_Z̃, û0, d̂0, p) end - # TODO: remove the following allocations - k̇_Z̃ = Mo*Δk - sk .= @. k̇_Z̃ - k̇ + sk .= mul!(sk, Mo, Δk) .- k̇ # ----------------- continuity constraint defects ------------------------------ - scnext .= λo*x0_Z̃ + Co*k_Z̃ - x0next_Z̃ + scnext .= mul!(scnext, Co, k_Z̃) .+ (λo.*x0_Z̃) .- x0next_Z̃ end return geq end From fcc824f3d53d1902c0d2978943b613ae5607bc2b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 17:09:36 -0500 Subject: [PATCH 40/49] changed: allocate before the loop --- src/controller/transcription.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6f6aa075b..2b76fef6b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1484,6 +1484,7 @@ function con_nonlinprogeq!( 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)] + ΔK = similar(K̇) # TODO: remove this allocation @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1493,6 +1494,7 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] + Δk = @views ΔK[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*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)] @@ -1510,11 +1512,10 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - Δk = similar(k̇) # TODO: remove this allocation for o=1:no k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] - ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] + ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] Δko .= @. ko_Z̃ - x0_Z̃ model.f!(k̇o, ko_Z̃, û0, d̂0, p) end From a3fd269e0f40e5343992f0415d054d1151e773c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 17:19:11 -0500 Subject: [PATCH 41/49] =?UTF-8?q?changed:=20using=20`i`=20counter=20for=20?= =?UTF-8?q?colloc.=20points=20`o`=20is=20misleading=20with=20the=20`=CE=BB?= =?UTF-8?q?=5Fo`=20variable?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 2b76fef6b..a986f6b37 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -149,7 +149,7 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi \vdots \\ \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} ``` -and ``\mathbf{k}_o(k+j)`` is the deterministic state prediction for the ``o``th collocation +and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or Gauss-Legendre quadrature, respectively. @@ -1452,12 +1452,12 @@ are computed by: \vdots \\ \mathbf{k̇}_{n_o}(k+j) \end{bmatrix} ``` -for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_o(k+j)`` vectors are +for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the -deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_o`` derivative for the ``o``th +deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for the ``i``th collocation point is computed from the continuous-time function `model.f!` and: ```math -\mathbf{k̇}_o(k+j) = \mathbf{f}\Big(\mathbf{k}_o(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) +\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) ``` The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for the stochastic states ``\mathbf{s_s}`` are computed @@ -1512,10 +1512,10 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - for o=1:no - k̇o = @views k̇[(1 + (o-1)*nx):(o*nx)] - Δko = @views Δk[(1 + (o-1)*nx):(o*nx)] - ko_Z̃ = @views k_Z̃[(1 + (o-1)*nx):(o*nx)] + for i=1:no + k̇o = @views k̇[(1 + (i-1)*nx):(i*nx)] + Δko = @views Δk[(1 + (i-1)*nx):(i*nx)] + ko_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] Δko .= @. ko_Z̃ - x0_Z̃ model.f!(k̇o, ko_Z̃, û0, d̂0, p) end From 7537b91520f84e4f67f0f2f150da7e4445bcae75 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Mar 2026 19:49:28 -0500 Subject: [PATCH 42/49] doc: modify continuity constraint equation --- src/controller/transcription.jl | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a986f6b37..7228dfd30 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1464,7 +1464,13 @@ the stochastic states ``\mathbf{s_s}`` are computed as in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint of the deterministic state trajectories are given by: ```math -\mathbf{s_c}(k+j+1) = λ_o \mathbf{x_0}(k+j) + \mathbf{C_o k}(k+j) - \mathbf{x_0}(k+j+1) +\mathbf{s_c}(k+j+1) + = \mathbf{C_o} \begin{bmatrix} + \mathbf{k}_1(k+j) \\ + \mathbf{k}_2(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) \end{bmatrix} + + λ_o \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1) ``` for ``j = 0, 1, ... , H_p-1``. The differentiation matrix ``\mathbf{M_o}``, the continuity matrix ``\mathbf{C_o}`` and the coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref). From a1b7b62eb01e6403d86d4684008c4fd85361a79b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 10:04:10 -0500 Subject: [PATCH 43/49] test: NMPC with `AutoFiniteDiff` The option was ignore since `hessian=true` meants sparse `AutoForwardDiff`, meaning that it was bypassing the other backends for efficiency (a `@warn` was printed to mention this) --- src/controller/transcription.jl | 3 ++- test/3_test_predictive_control.jl | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 7228dfd30..35f31b00b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -152,7 +152,8 @@ where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministi and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or -Gauss-Legendre quadrature, respectively. +Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for +descriptions of `f_threads` and `h_threads` keywords. This transcription computes the predictions by enforcing the collocation and continuity constraints at the collocation points. It is efficient for highly stiff systems, but diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index c5f09dbce..21bd71f27 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1024,8 +1024,8 @@ end nonlinmodel, Nwt=[0], Hp=100, Hc=1, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff(), - hessian=true), - ymax=[100], ymin=[-100] + hessian=AutoFiniteDiff() + ), ymax=[100], ymin=[-100] ) preparestate!(nmpc10, [0], [0]) u = moveinput!(nmpc10, [10], [0]) From d67d407d3c1245d0f68c68b4eecdd46bd9f0a31f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 10:23:49 -0500 Subject: [PATCH 44/49] added: linearly interpolated `d` in `OrthogonalCollocation` --- src/controller/transcription.jl | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 35f31b00b..8590083bd 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1458,12 +1458,16 @@ extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for the ``i``th collocation point is computed from the continuous-time function `model.f!` and: ```math -\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) +\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_0}(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big) ``` -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 in the [`TrapezoidalCollocation`](@ref) method, and the ones for the continuity -constraint of the deterministic state trajectories are given by: +The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). Based on the +normalized time ``τ_i ∈ [0, 1]``, measured disturbances are linearly interpolated: +```math +\mathbf{d̂}_i(k+j) = (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) +``` +The defects for the stochastic states ``\mathbf{s_s}`` are computed as in the +[`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint of the +deterministic state trajectories are given by: ```math \mathbf{s_c}(k+j+1) = \mathbf{C_o} \begin{bmatrix} @@ -1481,16 +1485,18 @@ function con_nonlinprogeq!( mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) - nu, nx̂, nd, nx, h = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.h + nu, nx̂, nd, nx = model.nu, mpc.estim.nx̂, model.nd, model.nx Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads p = model.p - no, Mo, Co, λo = transcription.no, mpc.Mo, mpc.Co, mpc.λo + no, τ = transcription.no, transcription.τ + Mo, Co, λo = mpc.Mo, mpc.Co, mpc.λo nk = get_nk(model, transcription) 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)] + di = mpc.estim.buffer.d ΔK = similar(K̇) # TODO: remove this allocation @threadsif f_threads for j=1:Hp if j < 2 @@ -1503,6 +1509,7 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] Δ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)] @@ -1520,11 +1527,12 @@ function con_nonlinprogeq!( û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) for i=1:no - k̇o = @views k̇[(1 + (i-1)*nx):(i*nx)] - Δko = @views Δk[(1 + (i-1)*nx):(i*nx)] - ko_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] - Δko .= @. ko_Z̃ - x0_Z̃ - model.f!(k̇o, ko_Z̃, û0, d̂0, p) + k̇i = @views k̇[(1 + (i-1)*nx):(i*nx)] + Δki = @views Δk[(1 + (i-1)*nx):(i*nx)] + ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] + Δki .= @. ki_Z̃ - x0_Z̃ + di .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next + model.f!(k̇i, ki_Z̃, û0, d̂0, p) end sk .= mul!(sk, Mo, Δk) .- k̇ # ----------------- continuity constraint defects ------------------------------ From 2493daa8a4c8f3080439a86a08dc8e0ece2196e2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 11:03:08 -0500 Subject: [PATCH 45/49] test: unit case with `OrthogonalCollocation` --- test/3_test_predictive_control.jl | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 21bd71f27..170810fa0 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -993,8 +993,14 @@ end preparestate!(nmpc5_1, [0.0]) u = moveinput!(nmpc5_1, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + + transcription = OrthogonalCollocation(0, 4) + nmpc6 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription) + preparestate!(nmpc6, [0.0]) + u = moveinput!(nmpc6, [1/0.001]) + @test u ≈ [1.0] atol=5e-2 + nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0], nonlinmodel2.p) From 51727eccbec156ca48d37317a86cf9901199f545 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 11:19:43 -0500 Subject: [PATCH 46/49] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 8358954a6..5f8ac20c0 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.16.3" +version = "1.17.0" authors = ["Francis Gagnon"] [deps] From d7971e55b11fdb75370c7fc4a850adcbd8109803 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 11:54:25 -0500 Subject: [PATCH 47/49] test: improve coverage with IMC --- src/estimator/internal_model.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 8eb1eb9d8..24418af80 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -275,7 +275,7 @@ Compute the current stochastic output estimation `ŷs` for [`InternalModel`](@r """ function correct_estimate!(estim::InternalModel, y0m, d0) ŷ0d = estim.buffer.ŷ - h!(ŷ0d, estim.model, estim.x̂d, d0, estim.model.p) + ĥ!(ŷ0d, estim, estim.model, estim.x̂d, d0) ŷs = estim.ŷs for j in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop if j in estim.i_ym @@ -307,8 +307,8 @@ function update_estimate!(estim::InternalModel, _ , d0, u0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext, k = estim.buffer.x̂, estim.buffer.k - f!(x̂dnext, k, model, x̂d, u0, d0, model.p) + x̂dnext, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k + f̂!(x̂dnext, û0, k, estim, estim.model, x̂d, u0, d0) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext From 8877d0f60b6a9095cc6b9f89dc2aca81326c507a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 13:58:36 -0500 Subject: [PATCH 48/49] test: improve coverage --- test/3_test_predictive_control.jl | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 170810fa0..5344c4f7f 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -810,6 +810,7 @@ end @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, JE = (_,_,_)->0.0) @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) + @test_throws ArgumentError NonLinMPC(linmodel1, transcription=TrapezoidalCollocation()) @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, JE=(Ue,_,_,_)->Ue) @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) @@ -875,10 +876,10 @@ end @test isa(nmpc15.optim, JuMP.GenericModel{Float64}) # Ipopt does not support Float32 @test_throws ArgumentError NonLinMPC(nonlinmodel) - @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation()) - @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2)) - @test_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) - @test_throws ArgumentError NonLinMPC(nonlinmodel, Wy=[1 0;0 1]) + @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=2, transcription=TrapezoidalCollocation()) + @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=2, transcription=TrapezoidalCollocation(2)) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=2, oracle=false, hessian=AutoFiniteDiff()) + @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=2, Wy=[1 0;0 1]) end @testitem "NonLinMPC moves and getinfo (LinModel)" setup=[SetupMPCtests] begin From 0ac337cdf392ee26d549e54807f800290158fe27 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 14:04:36 -0500 Subject: [PATCH 49/49] test: improve coverage --- test/3_test_predictive_control.jl | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 5344c4f7f..59d074260 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -880,6 +880,7 @@ end @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=2, transcription=TrapezoidalCollocation(2)) @test_throws ErrorException NonLinMPC(linmodel1, Hp=2, oracle=false, hessian=AutoFiniteDiff()) @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=2, Wy=[1 0;0 1]) + @test_throws ArgumentError OrthogonalCollocation(roots=:gausslobatto) end @testitem "NonLinMPC moves and getinfo (LinModel)" setup=[SetupMPCtests] begin @@ -1000,6 +1001,12 @@ end preparestate!(nmpc6, [0.0]) u = moveinput!(nmpc6, [1/0.001]) @test u ≈ [1.0] atol=5e-2 + + transcription = OrthogonalCollocation(roots=:gausslegendre) + nmpc6_1 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription) + preparestate!(nmpc6_1, [0.0]) + u = moveinput!(nmpc6_1, [1/0.001]) + @test u ≈ [1.0] atol=5e-2 nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10)