From 587e32dca4781bb9d4e49d8d97c541e351256bac Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 11:04:11 -0400 Subject: [PATCH 01/13] doc: consistent notation for slack var. --- src/controller/transcription.jl | 8 ++++---- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 58aa70ac5..4f0dc3b77 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1154,11 +1154,11 @@ If supported by `mpc.optim`, it warm-starts the solver at: \vdots \\ \mathbf{Δu}(k+H_c-2|k-1) \\ \mathbf{0} \\ - ϵ(k-1) + ϵ_{k-1} \end{bmatrix} ``` where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the -last control period ``k-1``, and ``ϵ(k-1)``, the slack variable of the last control period. +last control period ``k-1``, and ``ϵ_{k-1}``, the slack variable of the last control period. """ function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) nu, Hc, Z̃s = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ @@ -1195,7 +1195,7 @@ It warm-starts the solver at: \vdots \\ \mathbf{k}(k+H_p-2|k-1) \\ \mathbf{k}(k+H_p-2|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 @@ -1243,7 +1243,7 @@ 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) \\ - ϵ(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 diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 73fbeb1d3..7574d41b4 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -639,7 +639,7 @@ If supported by `estim.optim`, it warm-starts the solver at: \mathbf{0} \\ \end{bmatrix} ``` -where ``ε(k-1)``, ``\mathbf{x̂}_{k-1}(k-N_k+p)`` and ``\mathbf{ŵ}_{k-1}(k-j)`` are +where ``ε_{k-1}``, ``\mathbf{x̂}_{k-1}(k-N_k+p)`` and ``\mathbf{ŵ}_{k-1}(k-j)`` are respectively the slack variable, the arrival state estimate and the process noise estimates computed at the last time step ``k-1``. If the objective function is not finite at this point, all the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The From 9385a3910227d6f1e056788140e90cc43125b90f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 11:57:19 -0400 Subject: [PATCH 02/13] 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 4f0dc3b77..7db16cb6f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -607,7 +607,7 @@ Init the matrices for computing the defects over the predicted states. Knowing that the decision vector ``\mathbf{Z}`` contains both ``\mathbf{ΔU}`` and ``\mathbf{X̂_0}`` vectors (with a [`MultipleShooting`](@ref) transcription), an equation similar to the prediction matrices (see [`init_predmat`](@ref)) computes the defects of -the estimated states of ``H_p``: +the estimated states over ``H_p``: ```math \begin{aligned} \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} From 1b860f9f4d545fb7df4f46829450ec29cd5b3bde Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 14:09:01 -0400 Subject: [PATCH 03/13] added: reduce allocations in `init_orthocolloc` --- 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 7db16cb6f..7bc67b932 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -249,14 +249,14 @@ function init_orthocolloc( 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) + Po[iRows, iCols] = (τ[i]^j)*I + Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I 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, transcription)*I(nx) + Co[:, iCols] = lagrange_end(j, transcription)*I end Co = sparse(Co) λo = lagrange_end(0, transcription) From 47ddbacd2642d9cd3ca910988af4a963ecdb873b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 14:16:51 -0400 Subject: [PATCH 04/13] changed: `I` does not support broadcast right now --- src/controller/transcription.jl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 7bc67b932..42ef66bd7 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -246,17 +246,18 @@ function init_orthocolloc( τ = 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 + I_nx = I(nx) 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 - Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I + 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, transcription)*I + Co[:, iCols] = lagrange_end(j, transcription)*I_nx end Co = sparse(Co) λo = lagrange_end(0, transcription) From df62964a6feb31ed52833d36be039f13263eb661 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 14:40:46 -0400 Subject: [PATCH 05/13] wip: linear equality constraints for continuity constraints --- src/controller/transcription.jl | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 42ef66bd7..ca9702e34 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -780,6 +780,22 @@ function init_defectmat( return ES, GS, JS, KS, VS, BS end +function init_defectmat( + model::NonLinModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, + Hp, Hc, _ +) where {NT<:Real} + nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs + nZ = get_nZ(estim, transcription, Hp, Hc) + nK = nZ - nu*Hc - nx̂*Hp + As = estim.As + # --- current measured disturbances d0 and predictions D̂0 --- + GS = zeros(NT, nx̂*Hp, nd) + JS = zeros(NT, nx̂*Hp, nd*Hp) + # --- state x̂op and state update f̂op operating points --- + BS = zeros(NT, nx̂*Hp) + return ES, GS, JS, KS, VS, BS +end + """ init_defectmat( model::NonLinModel, estim::InternalModel, ::TranscriptionMethod, Hp, Hc, _ @@ -1666,8 +1682,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads p = model.p - no, τ = transcription.no, transcription.τ - Mo, Co, λo = mpc.Mo, mpc.Co, mpc.λo + Mo, no, τ = mpc.Mo, transcription.no, transcription.τ nk = get_nk(model, transcription) nx_nk = nx + nk D̂0 = mpc.D̂0 @@ -1685,9 +1700,7 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - scnext = @views geq[(1 + nx_nk*(j-1) ):(nx_nk*(j-1) + nx)] - sk = @views geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j )] + sk = @views geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j)] # ----------------- collocation constraint defects ----------------------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ @@ -1713,8 +1726,6 @@ function con_nonlinprogeq!( end end sk .-= k̇ - # ----------------- continuity constraint defects ------------------------------ - scnext .= mul!(scnext, Co, k_Z̃) .+ (λo.*x̂d_Z̃) .- x̂dnext_Z̃ end return geq end From fe9a66735cc6698f3e4cfa7c666802d506979695 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 8 Jul 2026 17:39:34 -0400 Subject: [PATCH 06/13] wip: idem --- src/controller/nonlinmpc.jl | 6 ++-- src/controller/transcription.jl | 54 ++++++++++++++++++++++++--------- 2 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 33c81c96d..5b25ba213 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -103,7 +103,10 @@ struct NonLinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) + Mo, Co, λo = init_orthocolloc(model, transcription) + ES, GS, JS, KS, VS, BS = init_defectmat( + model, estim, transcription, Hp, Hc, nb, Co, λo + ) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, @@ -122,7 +125,6 @@ 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_function_mpc(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) - 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ϵ) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index ca9702e34..52ace22e4 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -600,7 +600,8 @@ end @doc raw""" init_defectmat( - model::LinModel, estim::StateEstimator, transcription::MultipleShooting, Hp, Hc, nb + model::LinModel, estim::StateEstimator, transcription::MultipleShooting, + Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Init the matrices for computing the defects over the predicted states. @@ -667,7 +668,8 @@ matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended H (see [`move_blocking`](@ref)). """ function init_defectmat( - model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc, nb + model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, + Hp, Hc, nb, ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d @@ -710,7 +712,8 @@ end @doc raw""" init_defectmat( - model::SimModel, estim::StateEstimator, ::TranscriptionMethod, Hp, Hc, _ + model::SimModel, estim::StateEstimator, ::TranscriptionMethod, + Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Init the matrices for computing the defects of the stochastic states only. @@ -753,7 +756,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H """ function init_defectmat( model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, - Hp, Hc, _ + Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs nZ = get_nZ(estim, transcription, Hp, Hc) @@ -782,12 +785,31 @@ end function init_defectmat( model::NonLinModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, - Hp, Hc, _ + Hp, Hc, _ , Co, λo ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs - nZ = get_nZ(estim, transcription, Hp, Hc) - nK = nZ - nu*Hc - nx̂*Hp + nk = get_nk(model, transcription) As = estim.As + λo_I = λo*I(nx) + # --- current state estimates x̂0 --- + KS = zeros(NT, nx̂*Hp, nx̂) + KS[1:nx, 1:nx] = λo_I + KS[nx+1:nx̂, nx+1:nx̂] = As + # --- previous manipulated inputs lastu0 --- + VS = zeros(nxs*Hp, nu) + # --- decision variables Z --- + ESΔu = zeros(NT, nx̂*Hp, nu*Hc) + ESx̂ = diagm(fill(NT(-1.0), nx̂*Hp)) + for j=1:Hp-1 + iRow_λo = (1:nx) .+ nx̂*j + iCol_λo = (1:nx) .+ nx̂*(j-1) + iRow_As = (nx+1:nx̂) .+ nx̂*j + iCol_As = (nx+1:nx̂) .+ nx̂*(j-1) + ESx̂[iRow_λo, iCol_λo] = λo_I + ESx̂[iRow_As, iCol_As] = As + end + ESk = repeatdiag([Co; zeros(NT, nxs, nk)], Hp) + ES = [ESΔu ESx̂ ESk] # --- current measured disturbances d0 and predictions D̂0 --- GS = zeros(NT, nx̂*Hp, nd) JS = zeros(NT, nx̂*Hp, nd*Hp) @@ -798,32 +820,37 @@ end """ init_defectmat( - model::NonLinModel, estim::InternalModel, ::TranscriptionMethod, Hp, Hc, _ + model::NonLinModel, estim::InternalModel, ::TranscriptionMethod, + Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Return empty matrices for [`InternalModel`](@ref) (the state vector is not augmented). """ function init_defectmat( - ::NonLinModel, estim::InternalModel, transcription::TranscriptionMethod, Hp, Hc, _ + ::NonLinModel, estim::InternalModel, transcription::TranscriptionMethod, + Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) end """ init_defectmat( - model::SimModel, estim::StateEstimator, transcription::SingleShooting, Hp, Hc, _ + model::SimModel, estim::StateEstimator, transcription::SingleShooting, + Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Return empty matrices for [`SingleShooting`](@ref) transcription (N/A). """ function init_defectmat( - ::SimModel, estim::StateEstimator, transcription::SingleShooting, Hp, Hc, _ + ::SimModel, estim::StateEstimator, transcription::SingleShooting, + Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) end function init_defectmat( - ::NonLinModel, estim::InternalModel, transcription::SingleShooting, Hp, Hc, _ + ::NonLinModel, estim::InternalModel, transcription::SingleShooting, + Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) end @@ -1684,7 +1711,6 @@ function con_nonlinprogeq!( p = model.p Mo, no, τ = mpc.Mo, transcription.no, transcription.τ 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)] D̂temp = mpc.buffer.D̂ @@ -1700,7 +1726,7 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - sk = @views geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j)] + sk = @views geq[(1 + nk*(j-1)):(nk*j)] # ----------------- collocation constraint defects ----------------------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ From 156becca25140cc1e4725f0bdb1752feec116d19 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 08:44:31 -0400 Subject: [PATCH 07/13] debug: resolve method ambiguity --- src/controller/transcription.jl | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 52ace22e4..5ab948063 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -238,6 +238,8 @@ and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagran ```math L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` +The ``\mathbf{M_o}`` matrix is used in the collocation constraints (nonlinear), and the +``\mathbf{C_o}`` and ``λ_o`` constants, in the continuity constraints (linear). """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation @@ -784,19 +786,34 @@ function init_defectmat( end function init_defectmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, + model::NonLinModel, estim::StateEstimator, transcription::OrthogonalCollocation, + Hp, Hc, _ , Co, λo +) + As, nxs = estim.As, estim.nxs + return init_defectmat_orthocolloc(model, estim, transcription, Hp, Hc, Co, λo, As, nxs) +end + +function init_defectmat( + model::NonLinModel, estim::InternalModel{NT}, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo ) where {NT<:Real} - nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs + As, nxs = zeros(NT, 0, 0), 0 # the state vector is not augmented, no stochastic defects + return init_defectmat_orthocolloc(model, estim, transcription, Hp, Hc, Co, λo, As, nxs) +end + +function init_defectmat_orthocolloc( + model::NonLinModel, estim::StateEstimator{NT}, transcription::OrthogonalCollocation, + Hp, Hc, Co, λo, As, nxs +) where {NT<:Real} + nu, nx, nd, nx̂ = model.nu, model.nx, model.nd, estim.nx̂ nk = get_nk(model, transcription) - As = estim.As λo_I = λo*I(nx) # --- current state estimates x̂0 --- KS = zeros(NT, nx̂*Hp, nx̂) KS[1:nx, 1:nx] = λo_I KS[nx+1:nx̂, nx+1:nx̂] = As # --- previous manipulated inputs lastu0 --- - VS = zeros(nxs*Hp, nu) + VS = zeros(nx̂*Hp, nu) # --- decision variables Z --- ESΔu = zeros(NT, nx̂*Hp, nu*Hc) ESx̂ = diagm(fill(NT(-1.0), nx̂*Hp)) @@ -855,7 +872,6 @@ function init_defectmat( return init_defectmat_empty(estim, transcription, Hp, Hc) end - function init_defectmat_empty( estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc ) where {NT<:Real} From acf41f93ce8d53129a9029de20190ec5d8c35328 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 10:58:23 -0400 Subject: [PATCH 08/13] doc: defining defect matrices for `OrthogonalCollocation` They are detailed in the extended help, like for all the other transcription methods. --- src/controller/transcription.jl | 108 +++++++++++++++++++++++--------- 1 file changed, 79 insertions(+), 29 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5ab948063..bb073fd28 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -646,10 +646,10 @@ matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended H \vdots & \vdots & \ddots & \vdots \\ \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} - -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ + \mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{Â} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & \mathbf{-I} \end{bmatrix} \\ \mathbf{G_S} &= \begin{bmatrix} \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ \mathbf{J_S} &= \begin{bmatrix} @@ -722,11 +722,11 @@ Init the matrices for computing the defects of the stochastic states only. The documentation of [`init_estimstoch`](@ref) shows that the stochastic model of the unmeasured disturbances is linear and discrete-time. The defect of the stochastic states -over ``H_p`` is therefore: +over ``H_p`` is therefore computed by: ```math \begin{aligned} - \mathbf{Ŝ_s} &= \mathbf{E_S Z} + \mathbf{K_S x̂_0}(k) \\ - &= \mathbf{E_S Z} + \mathbf{F_S} + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{K_S x̂_0}(k) \\ + &= \mathbf{E_S Z} + \mathbf{F_S} \end{aligned} ``` The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended Help section. @@ -737,32 +737,26 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H the defect matrices are computed with: ```math \begin{aligned} - \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ + \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} - \mathbf{0} &-\mathbf{I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \mathbf{0} & \mathbf{A_s} & \mathbf{0} &-\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} &-\mathbf{I} \end{bmatrix} \\ - \mathbf{E_{S}^{k}} &= \mathbf{0} \\ + \mathbf{0} & \mathbf{-I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{bmatrix} \\ \mathbf{K_S} &= \begin{bmatrix} - \mathbf{0} & \mathbf{A_s} \\ - \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} \end{bmatrix} + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} \end{aligned} ``` - and: - - if `transcription` is an [`OrthogonalCollocation`](@ref), ``\mathbf{E_S} = [\begin{smallmatrix} - \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k}} \end{smallmatrix}]`` - - else ``\mathbf{E_S} = [\begin{smallmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{smallmatrix}]`` """ function init_defectmat( - model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, + model::SimModel, estim::StateEstimator{NT}, ::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs - nZ = get_nZ(estim, transcription, Hp, Hc) - nK = nZ - nu*Hc - nx̂*Hp As = estim.As # --- current state estimates x̂0 --- KS = zeros(NT, nxs*Hp, nx̂) @@ -770,13 +764,14 @@ function init_defectmat( # --- previous manipulated inputs lastu0 --- VS = zeros(nxs*Hp, nu) # --- decision variables Z --- - zeros_nI = [zeros(NT, nxs, nx) -I] - ES = [zeros(NT, nxs*Hp, nu*Hc) repeatdiag(zeros_nI, Hp) zeros(NT, nxs*Hp, nK)] + ESΔu = zeros(NT, nxs*Hp, nu*Hc) + ESx̂ = repeatdiag([zeros(NT, nxs, nx) -I], Hp) for j=1:Hp-1 - iRow = (1:nxs) .+ nxs*j - iCol = (nx+1:nx̂) .+ nx̂*(j-1) .+ nu*Hc - ES[iRow, iCol] = As + iRow = (1:nxs) .+ nxs*j + iCol = (nx+1:nx̂) .+ nx̂*(j-1) + ESx̂[iRow, iCol] = As end + ES = [ESΔu ESx̂] # --- current measured disturbances d0 and predictions D̂0 --- GS = zeros(NT, nxs*Hp, nd) JS = zeros(NT, nxs*Hp, nd*Hp) @@ -785,6 +780,61 @@ function init_defectmat( return ES, GS, JS, KS, VS, BS end +@doc raw""" + init_defectmat( + model::SimModel, estim::StateEstimator, ::TranscriptionMethod, + Hp, Hc, nb, Co=nothing, λo=nothing + ) -> ES, GS, JS, KS, VS, BS + +Init the matrices for computing the continuity constraints and stochastic state defects. + +The documentation of [`init_orthocolloc`](@ref) shows that continuity constraints of the +[`OrthogonalCollocation`](@ref) are in fact linear. Combined with the stochastic state +defects, the linear equality constraints for this transcription is given by: +```math +\begin{aligned} + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{K_S x̂_0}(k) \\ + &= \mathbf{E_S Z} + \mathbf{F_S} +\end{aligned} +``` +The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended Help section. + + +# Extended Help +!!! details "Extended Help" + Using the stochastic matrix ``\mathbf{A_s}`` in `estim` and the continuity constraints + constants of [`init_orthocolloc`](@ref), the defect matrices are: + ```math + \begin{aligned} + \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ + \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} + \mathbf{-I} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{-I} & \mathbf{0} &\mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + λ_o\mathbf{I} & \mathbf{0} &\mathbf{-I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} &\mathbf{-I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & λ_o\mathbf{I} & \mathbf{0} & \mathbf{-I} & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{A_s} & \mathbf{0} & \mathbf{-I} \end{bmatrix} \\ + \mathbf{E_{S}^{k}} &= \begin{bmatrix} + \mathbf{C_o} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{C_o} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{C_o} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix} \\ + \mathbf{E_S} &= \begin{bmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k}} \end{bmatrix} \\ + \mathbf{K_S} &= \begin{bmatrix} + λ_o\mathbf{I} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} + \end{aligned} + ``` + Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented so + the rows of ``\mathbf{E_S}`` and ``\mathbf{K_S}`` with an even index are removed. +""" function init_defectmat( model::NonLinModel, estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo From ef51011b5c25eccbd680f221571e73c15630dced Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 12:06:09 -0400 Subject: [PATCH 09/13] doc: added equation for continuity constraint This is a bit more explicit. The matrix equation in `init_defectmat` is harder to follow. --- src/controller/transcription.jl | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index bb073fd28..c8a6e58bb 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -238,8 +238,20 @@ and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagran ```math L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} ``` -The ``\mathbf{M_o}`` matrix is used in the collocation constraints (nonlinear), and the -``\mathbf{C_o}`` and ``λ_o`` constants, in the continuity constraints (linear). + +The collocation constraints are nonlinear, but the defects of deterministic states +``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: +```math +\mathbf{s_c}(k+j+1) = \mathbf{0} = + = \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̂_d}(k+j) - \mathbf{x̂_d}(k+j+1) +``` +for ``j = 0, 1, ... , H_p-1``. The ``\mathbf{k}_i(k+j)`` and ``\mathbf{x̂_d}(k+j)`` vectors +are all directly extracted from the decision variable `Z̃`. """ function init_orthocolloc( model::SimModel{NT}, transcription::OrthogonalCollocation @@ -714,7 +726,7 @@ end @doc raw""" init_defectmat( - model::SimModel, estim::StateEstimator, ::TranscriptionMethod, + model::SimModel, estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS @@ -782,7 +794,7 @@ end @doc raw""" init_defectmat( - model::SimModel, estim::StateEstimator, ::TranscriptionMethod, + model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS From a44f34f354cbe505b020933100169f5db9a3eafe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 12:26:20 -0400 Subject: [PATCH 10/13] doc: remove duplicate `=` sign --- 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 c8a6e58bb..bd146e5c8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -243,7 +243,7 @@ The collocation constraints are nonlinear, but the defects of deterministic stat ``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: ```math \mathbf{s_c}(k+j+1) = \mathbf{0} = - = \mathbf{C_o} \begin{bmatrix} + \mathbf{C_o} \begin{bmatrix} \mathbf{k}_1(k+j) \\ \mathbf{k}_2(k+j) \\ \vdots \\ From 8b3da89dc55cb74c0194618e1e09d8b6af58b7f4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 13:01:19 -0400 Subject: [PATCH 11/13] doc: minor change --- docs/src/internals/predictive_control.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 34f5734b3..8bf16e5db 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -15,6 +15,7 @@ The prediction methodology of this module is mainly based on Maciejowski textboo ModelPredictiveControl.move_blocking ModelPredictiveControl.init_ZtoΔU ModelPredictiveControl.init_ZtoU +ModelPredictiveControl.init_orthocolloc ModelPredictiveControl.init_predmat ModelPredictiveControl.init_defectmat ModelPredictiveControl.relaxU @@ -25,7 +26,6 @@ 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) From 936d7ed6aae1ed7dc9f9c825e116ee954e33cb04 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 13:14:16 -0400 Subject: [PATCH 12/13] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 1c4701456..ad123c03b 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.5.1" +version = "2.6.0" authors = ["Francis Gagnon"] [deps] From e468690435ebe60a265e3dcff00894af41e029d8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 9 Jul 2026 13:52:32 -0400 Subject: [PATCH 13/13] test: improve coverage of `OrthogonalCollocation` --- test/3_test_predictive_control.jl | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index d9c590d99..4cbbe2d3b 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -859,30 +859,37 @@ end @test nmpc11.estim.nint_ym == [0, 0] gc! = (LHS,_,_,_,_,_)-> (LHS .= 0.0) # useless, only for coverage nmpc16 = NonLinMPC(nonlinmodel, Hp=10, transcription=MultipleShooting(), nc=10, gc=gc!) - @test nmpc16.transcription == MultipleShooting() + @test isa(nmpc16.transcription, MultipleShooting) @test length(nmpc16.Z̃) == nonlinmodel.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ @test nmpc16.con.neq == nmpc16.estim.model.nx*nmpc16.Hp @test nmpc16.con.nc == 10 nonlinmodel_c = NonLinModel((ẋ,x,u,_,_)->ẋ .= -0.1x .+ u, (y,x,_,_)->y.=x, 1, 1, 1, 1) nmpc17 = NonLinMPC(nonlinmodel_c, Hp=10, transcription=TrapezoidalCollocation(), nc=10, gc=gc!) - @test nmpc17.transcription == TrapezoidalCollocation() + @test isa(nmpc17.transcription, TrapezoidalCollocation) @test length(nmpc17.Z̃) == nonlinmodel_c.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ @test nmpc17.con.neq == nmpc17.estim.model.nx*nmpc17.Hp @test nmpc17.con.nc == 10 - nmpc18 = NonLinMPC(nonlinmodel, Hp=10, + nmpc18 = NonLinMPC( + InternalModel(nonlinmodel_c), Hp=10, transcription=OrthogonalCollocation(), Cwt=Inf + ) + @test isa(nmpc18.transcription, OrthogonalCollocation) + Hp, Hc, no, nx̂ = nmpc18.Hp, nmpc18.Hc, nmpc18.transcription.no, nmpc18.estim.nx̂ + @test length(nmpc18.Z̃) == nonlinmodel_c.nu*Hc + nx̂*Hp + nonlinmodel_c.nx*Hp*no + @test nmpc18.con.neq == nonlinmodel_c.nx*Hp*no + nmpc19 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff(), hessian=AutoFiniteDiff() ) - @test nmpc18.gradient == AutoFiniteDiff() - @test nmpc18.jacobian == AutoFiniteDiff() - @test nmpc18.hessian == AutoFiniteDiff() + @test nmpc19.gradient == AutoFiniteDiff() + @test nmpc19.jacobian == AutoFiniteDiff() + @test nmpc19.hessian == AutoFiniteDiff() nonlinmodel_simple = NonLinModel((x,u,_,_)->x+u,(x,_,_)->x, 1, 1, 1, 1, solver=nothing) - nmpc19 = NonLinMPC(nonlinmodel_simple, Hc=1, Hp=10, Cwt=Inf, + nmpc20 = NonLinMPC(nonlinmodel_simple, Hc=1, Hp=10, Cwt=Inf, hessian=SecondOrder(AutoForwardDiff(), AutoForwardDiff()) ) - @test nmpc19.hessian == SecondOrder(AutoForwardDiff(), AutoForwardDiff()) - @test_nowarn repr(nmpc19) # printing SecondOrder backends, for coverage + @test nmpc20.hessian == SecondOrder(AutoForwardDiff(), AutoForwardDiff()) + @test_nowarn repr(nmpc20) # printing SecondOrder backends, for coverage nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc15 = NonLinMPC(nonlinmodel2, Hp=15)