From 142f0ad3ecfa3fdc7b83c9efd8d1a43cdae57a32 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 14:47:48 -0500 Subject: [PATCH 01/22] doc: adding compat admonition for MTK example --- docs/src/manual/mtk.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/src/manual/mtk.md b/docs/src/manual/mtk.md index aecfea645..2a514e0f2 100644 --- a/docs/src/manual/mtk.md +++ b/docs/src/manual/mtk.md @@ -22,6 +22,10 @@ the last section. as a basic starting template to combine both packages. There is no guarantee that it will work for all corner cases. +!!! compat + The example works on `ModelingToolkit.jl` v10 (corresponding to the following `[compat]` + entry: `ModelingToolkit = "10"`). + We first construct and instantiate the pendulum model: ```@example 1 From cf6931effb0462ebf9e312f8abf447d8256f1ff5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 14:52:01 -0500 Subject: [PATCH 02/22] doc: correct prediction matrices for `LinModel` with move blocking --- src/controller/transcription.jl | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8593c61a0..a3f6fec1b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -295,15 +295,26 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). # Extended Help !!! details "Extended Help" Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see - [`augment_model`](@ref)), and the function ``\mathbf{W}(j) = ∑_{i=0}^j \mathbf{Â}^i``, + [`augment_model`](@ref)), the following two functions with integer arguments: + ```math + \begin{aligned} + \mathbf{Q}(i, j) &= \begin{bmatrix} + \mathbf{Ĉ W}(i+0)\mathbf{B̂_u} \\ + \mathbf{Ĉ W}(i+1)\mathbf{B̂_u} \\ + \vdots \\ + \mathbf{Ĉ W}(i+j-1)\mathbf{B̂_u} + \end{bmatrix} \\ + \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ + \end{aligned} + ``` the prediction matrices are computed by : ```math \begin{aligned} \mathbf{E} &= \begin{bmatrix} - \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ W}(1)\mathbf{B̂_u} & \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} & \mathbf{Ĉ W}(H_p-2)\mathbf{B̂_u} & \cdots & \mathbf{Ĉ W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{Q}(0, n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Q}(n_1, n_2) & \mathbf{Q}(0, n_2) & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Q}(n_{H_c-1}, n_{H_c}) & \mathbf{Q}(n_{H_c-2}, n_{H_c-1}) & \cdots & \mathbf{Q}(0, n_{H_c}) \end{bmatrix} \\ \mathbf{G} &= \begin{bmatrix} \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} \\ \mathbf{Ĉ}\mathbf{Â}^{1} \mathbf{B̂_d} \\ @@ -319,11 +330,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{Ĉ}\mathbf{Â}^{2} \\ \vdots \\ \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ - \mathbf{V} &= \begin{bmatrix} - \mathbf{Ĉ W}(0)\mathbf{B̂_u} \\ - \mathbf{Ĉ W}(1)\mathbf{B̂_u} \\ - \vdots \\ - \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{V} &= Q(0, H_p) \\ \mathbf{B} &= \begin{bmatrix} \mathbf{Ĉ W}(0) \\ \mathbf{Ĉ W}(1) \\ From 799ba3711beadde3ad7da6bed19635f04ee444b6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 15:17:41 -0500 Subject: [PATCH 03/22] doc: correct terminal state prediction matrices for `LinModel` --- src/controller/transcription.jl | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a3f6fec1b..6c4f06e01 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -295,7 +295,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). # Extended Help !!! details "Extended Help" Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see - [`augment_model`](@ref)), the following two functions with integer arguments: + [`augment_model`](@ref)), and the following two functions with integer arguments: ```math \begin{aligned} \mathbf{Q}(i, j) &= \begin{bmatrix} @@ -307,7 +307,9 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ \end{aligned} ``` - the prediction matrices are computed by : + the prediction matrices are computed from the [`move_blocking`](@ref) vector + ``\mathbf{n_b} = [\begin{smallmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{smallmatrix}]'`` + and the following equations: ```math \begin{aligned} \mathbf{E} &= \begin{bmatrix} @@ -343,9 +345,10 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \begin{aligned} \mathbf{e_x̂} &= \begin{bmatrix} \mathbf{W}(H_p-1)\mathbf{B̂_u} & - \mathbf{W}(H_p-2)\mathbf{B̂_u} & + \mathbf{W}(H_p-n_1-1)\mathbf{B̂_u} & + \mathbf{W}(H_p-n_2-1)\mathbf{B̂_u} & \cdots & - \mathbf{W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{W}(H_p-n_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ \mathbf{j_x̂} &= \begin{bmatrix} \mathbf{Â}^{H_p-2} \mathbf{B̂_d} & From 4cc02a36a613786b7381ac3a336d6c7e419a9c77 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 15:48:14 -0500 Subject: [PATCH 04/22] changed: introducing `W(m)` function for clarity --- src/controller/transcription.jl | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6c4f06e01..ab8a2b907 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -344,11 +344,11 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{e_x̂} &= \begin{bmatrix} - \mathbf{W}(H_p-1)\mathbf{B̂_u} & - \mathbf{W}(H_p-n_1-1)\mathbf{B̂_u} & - \mathbf{W}(H_p-n_2-1)\mathbf{B̂_u} & + \mathbf{W}(H_p-1) \mathbf{B̂_u} & + \mathbf{W}(H_p-n_1-1) \mathbf{B̂_u} & + \mathbf{W}(H_p-n_2-1) \mathbf{B̂_u} & \cdots & - \mathbf{W}(H_p-n_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{W}(H_p-n_{H_c-1}-1) \mathbf{B̂_u} \end{bmatrix} \\ \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ \mathbf{j_x̂} &= \begin{bmatrix} \mathbf{Â}^{H_p-2} \mathbf{B̂_d} & @@ -376,8 +376,9 @@ function init_predmat( end # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... Âpow_csum = cumsum(Âpow, dims=3) - # helper function to improve code clarity and be similar to eqs. in docstring: + # two helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] + W(m) = @views Âpow_csum[:,:, m+1] # --- current state estimates x̂0 --- kx̂ = getpower(Âpow, Hp) K = Matrix{NT}(undef, Hp*ny, nx̂) @@ -386,11 +387,11 @@ function init_predmat( K[iRow,:] = Ĉ*getpower(Âpow, j) end # --- previous manipulated inputs lastu0 --- - vx̂ = getpower(Âpow_csum, Hp-1)*B̂u + vx̂ = W(H_p-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) for j=1:Hp iRow = (1:ny) .+ ny*(j-1) - V[iRow,:] = Ĉ*getpower(Âpow_csum, j-1)*B̂u + V[iRow,:] = Ĉ*W(j-1)*B̂u end # --- decision variables Z --- nZ = get_nZ(estim, transcription, Hp, Hc) @@ -400,7 +401,7 @@ function init_predmat( iRow = (ny*(j-1)+1):(ny*Hp) iCol = (1:nu) .+ nu*(j-1) E[iRow, iCol] = V[iRow .- ny*(j-1),:] - ex̂[: , iCol] = getpower(Âpow_csum, Hp-j)*B̂u + ex̂[: , iCol] = W(Hp-j)*B̂u end # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d @@ -420,11 +421,11 @@ function init_predmat( end end # --- state x̂op and state update f̂op operating points --- - coef_bx̂ = getpower(Âpow_csum, Hp-1) + coef_bx̂ = W(Hp-1) coef_B = Matrix{NT}(undef, ny*Hp, nx̂) for j=1:Hp iRow = (1:ny) .+ ny*(j-1) - coef_B[iRow,:] = Ĉ*getpower(Âpow_csum, j-1) + coef_B[iRow,:] = Ĉ*W(j-1) end f̂op_n_x̂op = estim.f̂op - estim.x̂op bx̂ = coef_bx̂ * f̂op_n_x̂op From 891156cbc916d82b594c9ff7f90344c4b24a16f1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 15:53:44 -0500 Subject: [PATCH 05/22] changed: similar function in `init_predmat_mhe` --- src/estimator/mhe/construct.jl | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index f0d777f4b..eb93d4baa 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1249,6 +1249,8 @@ function init_predmat_mhe( # --- state x̂op and state update f̂op operating points --- # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... Âpow_csum = cumsum(Âpow3D, dims=3) + # helper function to improve code clarity and be similar to eqs. in docstring: + S(j) = @views Âpow_csum[:,:, j+1] f̂_op_n_x̂op = (f̂op - x̂op) coef_B = zeros(NT, nym*He, nx̂) row_begin = iszero(p) ? 0 : 1 @@ -1256,14 +1258,14 @@ function init_predmat_mhe( j=0 for i=row_begin:row_end iRow = (1:nym) .+ nym*i - coef_B[iRow,:] = -Ĉm*getpower(Âpow_csum, j) + coef_B[iRow,:] = -Ĉm*S(j) j+=1 end B = coef_B*f̂_op_n_x̂op coef_Bx̂ = Matrix{NT}(undef, nx̂*He, nx̂) for j=0:He-1 iRow = (1:nx̂) .+ nx̂*j - coef_Bx̂[iRow,:] = getpower(Âpow_csum, j) + coef_Bx̂[iRow,:] = S(j) end Bx̂ = coef_Bx̂*f̂_op_n_x̂op return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ From 189b6edfd4064b1bfafb257fdbcb3ad0b05e56e6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 16:14:19 -0500 Subject: [PATCH 06/22] added: `Q!(Q, i, j)` function for move blocking --- src/controller/transcription.jl | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index ab8a2b907..aa7646825 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -332,7 +332,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{Ĉ}\mathbf{Â}^{2} \\ \vdots \\ \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ - \mathbf{V} &= Q(0, H_p) \\ + \mathbf{V} &= \mathbf{Q}(0, H_p) \\ \mathbf{B} &= \begin{bmatrix} \mathbf{Ĉ W}(0) \\ \mathbf{Ĉ W}(1) \\ @@ -376,9 +376,16 @@ function init_predmat( end # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... Âpow_csum = cumsum(Âpow, dims=3) - # two helper functions to improve code clarity and be similar to eqs. in docstring: + # three helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] W(m) = @views Âpow_csum[:,:, m+1] + function Q!(Q, i, j) + for ℓ=1:j + iRows = (1:ny) .+ ny*(ℓ-1) + Q[iRows, :] = Ĉ*W(i+ℓ-1)*B̂u + end + return Q + end # --- current state estimates x̂0 --- kx̂ = getpower(Âpow, Hp) K = Matrix{NT}(undef, Hp*ny, nx̂) @@ -387,7 +394,7 @@ function init_predmat( K[iRow,:] = Ĉ*getpower(Âpow, j) end # --- previous manipulated inputs lastu0 --- - vx̂ = W(H_p-1)*B̂u + vx̂ = W(Hp-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) for j=1:Hp iRow = (1:ny) .+ ny*(j-1) From 8118bac927d81cc9784c4c866ba2b5cd64c34fcd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 16:30:06 -0500 Subject: [PATCH 07/22] doc: minor modification --- src/controller/transcription.jl | 21 +++++---------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index aa7646825..4929695e4 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -344,21 +344,13 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{e_x̂} &= \begin{bmatrix} - \mathbf{W}(H_p-1) \mathbf{B̂_u} & - \mathbf{W}(H_p-n_1-1) \mathbf{B̂_u} & - \mathbf{W}(H_p-n_2-1) \mathbf{B̂_u} & - \cdots & - \mathbf{W}(H_p-n_{H_c-1}-1) \mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{W}(H_p-1)\mathbf{B̂_u} & \mathbf{W}(H_p-n_1-1)\mathbf{B̂_u} & \cdots & \mathbf{W}(H_p-n_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ \mathbf{j_x̂} &= \begin{bmatrix} - \mathbf{Â}^{H_p-2} \mathbf{B̂_d} & - \mathbf{Â}^{H_p-3} \mathbf{B̂_d} & - \cdots & - \mathbf{0} - \end{bmatrix} \\ + \mathbf{Â}^{H_p-2}\mathbf{B̂_d} & \mathbf{Â}^{H_p-3}\mathbf{B̂_d} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{k_x̂} &= \mathbf{Â}^{H_p} \\ \mathbf{v_x̂} &= \mathbf{W}(H_p-1)\mathbf{B̂_u} \\ - \mathbf{b_x̂} &= \mathbf{W}(H_p-1) \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \mathbf{b_x̂} &= \mathbf{W}(H_p-1)\mathbf{\big(f̂_{op} - x̂_{op}\big)} \end{aligned} ``` """ @@ -392,14 +384,11 @@ function init_predmat( for j=1:Hp iRow = (1:ny) .+ ny*(j-1) K[iRow,:] = Ĉ*getpower(Âpow, j) - end + end # --- previous manipulated inputs lastu0 --- vx̂ = W(Hp-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) - for j=1:Hp - iRow = (1:ny) .+ ny*(j-1) - V[iRow,:] = Ĉ*W(j-1)*B̂u - end + Q!(V, 0, Hp) # --- decision variables Z --- nZ = get_nZ(estim, transcription, Hp, Hc) ex̂ = Matrix{NT}(undef, nx̂, nZ) From 0cc258925ed675551e79b91faee81840e0470532 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 17:24:02 -0500 Subject: [PATCH 08/22] doc: correct mistake on the last row of `E` matrix with move blocking --- 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 4929695e4..126d4ca9f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -313,10 +313,10 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{E} &= \begin{bmatrix} - \mathbf{Q}(0, n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Q}(n_1, n_2) & \mathbf{Q}(0, n_2) & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Q}(n_{H_c-1}, n_{H_c}) & \mathbf{Q}(n_{H_c-2}, n_{H_c-1}) & \cdots & \mathbf{Q}(0, n_{H_c}) \end{bmatrix} \\ + \mathbf{Q}(0, n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Q}(n_1, n_2) & \mathbf{Q}(0, n_2) & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Q}(n_{H_c-1}, n_{H_c}) & \mathbf{Q}(n_{H_c-2}, n_{H_c}) & \cdots & \mathbf{Q}(0, n_{H_c}) \end{bmatrix} \\ \mathbf{G} &= \begin{bmatrix} \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} \\ \mathbf{Ĉ}\mathbf{Â}^{1} \mathbf{B̂_d} \\ From 04b169256b0875bcdb2d9d1645109a353230a745 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 31 Dec 2025 17:24:36 -0500 Subject: [PATCH 09/22] added: `nb` argument to `init_predmat` --- src/controller/execute.jl | 4 ++-- src/controller/explicitmpc.jl | 6 +++--- src/controller/linmpc.jl | 2 +- src/controller/nonlinmpc.jl | 2 +- src/controller/transcription.jl | 27 ++++++++++++++++----------- 5 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index ff7de660a..4d3968668 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -624,11 +624,11 @@ end function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription weights = mpc.weights - nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc + nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb optim, con = mpc.optim, mpc.con # --- prediction matrices --- E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( - model, estim, transcription, Hp, Hc + model, estim, transcription, Hp, Hc, nb ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, con.c_x̂min, con.c_x̂max, mpc.nϵ) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 370a8d319..4dfd07283 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -53,7 +53,7 @@ struct ExplicitMPC{ validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) - E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc) + E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb) # dummy val (updated just before optimization): F = zeros(NT, ny*Hp) P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC @@ -226,9 +226,9 @@ addinfo!(info, mpc::ExplicitMPC) = info function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ ) model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription weights = mpc.weights - nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc + nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb # --- predictions matrices --- - E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc) + E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb) Ẽ = E # no slack variable ϵ for ExplicitMPC mpc.Ẽ .= Ẽ mpc.G .= G diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index b176e1ee0..99e8428f7 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -67,7 +67,7 @@ struct LinMPC{ PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( - model, estim, transcription, Hp, Hc + model, estim, transcription, Hp, Hc, nb ) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) # dummy vals (updated just before optimization): diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 2a8203859..17bd7ba44 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -95,7 +95,7 @@ struct NonLinMPC{ PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( - model, estim, transcription, Hp, Hc + model, estim, transcription, Hp, Hc, nb ) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) # dummy vals (updated just before optimization): diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 126d4ca9f..83a3b2c88 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -255,7 +255,7 @@ end @doc raw""" init_predmat( - model::LinModel, estim, transcription::SingleShooting, Hp, Hc + model::LinModel, estim, transcription::SingleShooting, Hp, Hc, nb ) -> E, G, J, K, V, ex̂, gx̂, jx̂, kx̂, vx̂ Construct the prediction matrices for [`LinModel`](@ref) and [`SingleShooting`](@ref). @@ -355,7 +355,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ``` """ function init_predmat( - model::LinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc + model::LinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, nb ) where {NT<:Real} Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd @@ -394,10 +394,15 @@ function init_predmat( ex̂ = Matrix{NT}(undef, nx̂, nZ) E = zeros(NT, Hp*ny, nZ) for j=1:Hc # truncated with control horizon - iRow = (ny*(j-1)+1):(ny*Hp) iCol = (1:nu) .+ nu*(j-1) - E[iRow, iCol] = V[iRow .- ny*(j-1),:] - ex̂[: , iCol] = W(Hp-j)*B̂u + for i=j:Hc + i_Q = i == j ? 0 : nb[i-1] + + end + + + n = j > 1 ? nb[j-1] : 0 + ex̂[: , iCol] = W(Hp-n-1)*B̂u end # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d @@ -431,7 +436,7 @@ end @doc raw""" init_predmat( - model::LinModel, estim, transcription::MultipleShooting, Hp, Hc + model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ Construct the prediction matrices for [`LinModel`](@ref) and [`MultipleShooting`](@ref). @@ -451,7 +456,7 @@ They are defined in the Extended Help section. ``` """ function init_predmat( - model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc + model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc, nb ) where {NT<:Real} Ĉ, D̂d = estim.Ĉ, estim.D̂d nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd @@ -476,12 +481,12 @@ function init_predmat( end """ - init_predmat(model::NonLinModel, estim, transcription::SingleShooting, Hp, Hc) + init_predmat(model::NonLinModel, estim, transcription::SingleShooting, Hp, Hc, nb) Return empty matrices for [`SingleShooting`](@ref) of [`NonLinModel`](@ref) """ function init_predmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc + model::NonLinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) @@ -496,7 +501,7 @@ function init_predmat( end @doc raw""" - init_predmat(model::NonLinModel, estim, transcription::TranscriptionMethod, Hp, Hc) + init_predmat(model::NonLinModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb) Return the terminal state matrices for [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). @@ -509,7 +514,7 @@ given in the Extended Help section. for ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]`` """ function init_predmat( - model::NonLinModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc + model::NonLinModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) From bd3600c702c0eae17d5eb455729123bebb063f0d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 11:41:24 -0500 Subject: [PATCH 10/22] added: correct `E` matrix implemented for `LinModel` --- src/controller/transcription.jl | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 83a3b2c88..35596d085 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -353,6 +353,9 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{b_x̂} &= \mathbf{W}(H_p-1)\mathbf{\big(f̂_{op} - x̂_{op}\big)} \end{aligned} ``` + The complex structure of the ``\mathbf{E}`` and ``\mathbf{e_x̂}`` matrices is due to the + move blocking implementation: the decision variable ``\mathbf{Z}`` only contains the + input increment ``\mathbf{Δu}`` of the free moves. """ function init_predmat( model::LinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, nb @@ -393,14 +396,15 @@ function init_predmat( nZ = get_nZ(estim, transcription, Hp, Hc) ex̂ = Matrix{NT}(undef, nx̂, nZ) E = zeros(NT, Hp*ny, nZ) - for j=1:Hc # truncated with control horizon + for j=1:Hc iCol = (1:nu) .+ nu*(j-1) - for i=j:Hc - i_Q = i == j ? 0 : nb[i-1] - + for i=1:j + i_Q = i > 1 ? nb[i-1] : 0 + j_Q = nb[j - i + 1] + iRow = (1:ny*j_Q) .+ nu*sum(nb[1:i-1]) + Q = @views E[iRow, iCol] + Q!(Q, i_Q, j_Q) end - - n = j > 1 ? nb[j-1] : 0 ex̂[: , iCol] = W(Hp-n-1)*B̂u end From 326ada4af305e859ae2c1a4c6292ceb647f5a002 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 15:10:08 -0500 Subject: [PATCH 11/22] debug: wip debug move blocking --- src/controller/transcription.jl | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 35596d085..e47667a3a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -355,7 +355,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ``` The complex structure of the ``\mathbf{E}`` and ``\mathbf{e_x̂}`` matrices is due to the move blocking implementation: the decision variable ``\mathbf{Z}`` only contains the - input increment ``\mathbf{Δu}`` of the free moves. + input increment ``\mathbf{Δu}`` of the free moves (see [`move_blocking`](@ref)). """ function init_predmat( model::LinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, nb @@ -398,16 +398,20 @@ function init_predmat( E = zeros(NT, Hp*ny, nZ) for j=1:Hc iCol = (1:nu) .+ nu*(j-1) - for i=1:j + for i=1:Hc-j+1 i_Q = i > 1 ? nb[i-1] : 0 - j_Q = nb[j - i + 1] - iRow = (1:ny*j_Q) .+ nu*sum(nb[1:i-1]) + j_Q = nb[i + j - 1] + iRow = (1:ny*j_Q) .+ ny*sum(nb[1:i+j-2]) Q = @views E[iRow, iCol] Q!(Q, i_Q, j_Q) end n = j > 1 ? nb[j-1] : 0 ex̂[: , iCol] = W(Hp-n-1)*B̂u end + + #display(E) + + # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d G = Matrix{NT}(undef, Hp*ny, nd) From 0bd1699317c909d4d814cd3e2c9e663b00093cef Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 16:43:48 -0500 Subject: [PATCH 12/22] =?UTF-8?q?doc:=20new=20`j=5F=E2=84=93`=20notation?= =?UTF-8?q?=20in=20`move=5Fblocking`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 008443f53..e561d0f97 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -499,15 +499,16 @@ strictly positive integers): ```math \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}' ``` -The vector that includes all the manipulated input increments ``\mathbf{Δu}`` is then +Introducing ``j_ℓ = ∑_{i=1}^{ℓ} n_i`` to convert from block lengths to discrete time steps, +the vector that includes all the manipulated input free moves ``\mathbf{Δu}`` is then defined as: ```math \mathbf{ΔU} = \begin{bmatrix} - \mathbf{Δu}(k + 0) \\[0.1em] - \mathbf{Δu}(k + ∑_{i=1}^1 n_i) \\[0.1em] - \mathbf{Δu}(k + ∑_{i=1}^2 n_i) \\[0.1em] - \vdots \\[0.1em] - \mathbf{Δu}(k + ∑_{i=1}^{H_c-1} n_i) + \mathbf{Δu}(k + 0) \\ + \mathbf{Δu}(k + j_1) \\ + \mathbf{Δu}(k + j_2) \\ + \vdots \\ + \mathbf{Δu}(k + j_{H_c-1}) \end{bmatrix} ``` The provided `nb` vector is modified to ensure `sum(nb) == Hp`: From 01d0410b1847edda7ed69512604559913884593d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 18:09:23 -0500 Subject: [PATCH 13/22] doc: three arguments `Q` function in `init_predmat` --- src/controller/construct.jl | 6 +++--- src/controller/transcription.jl | 29 +++++++++++++++-------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index e561d0f97..ae0b4a368 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -499,9 +499,9 @@ strictly positive integers): ```math \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}' ``` -Introducing ``j_ℓ = ∑_{i=1}^{ℓ} n_i`` to convert from block lengths to discrete time steps, -the vector that includes all the manipulated input free moves ``\mathbf{Δu}`` is then -defined as: +Introducing the notation ``j_ℓ = ∑_{i=1}^{ℓ} n_i`` to convert from block lengths to discrete +time steps, the vector that includes all the manipulated input free moves ``\mathbf{Δu}`` is +then defined as: ```math \mathbf{ΔU} = \begin{bmatrix} \mathbf{Δu}(k + 0) \\ diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e47667a3a..a1af0c46b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -298,25 +298,24 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). [`augment_model`](@ref)), and the following two functions with integer arguments: ```math \begin{aligned} - \mathbf{Q}(i, j) &= \begin{bmatrix} - \mathbf{Ĉ W}(i+0)\mathbf{B̂_u} \\ - \mathbf{Ĉ W}(i+1)\mathbf{B̂_u} \\ + \mathbf{Q}(i, k, b) &= \begin{bmatrix} + \mathbf{Ĉ W}(i-b+0)\mathbf{B̂_u} \\ + \mathbf{Ĉ W}(i-b+1)\mathbf{B̂_u} \\ \vdots \\ - \mathbf{Ĉ W}(i+j-1)\mathbf{B̂_u} + \mathbf{Ĉ W}(k-b-1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ + \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ \end{aligned} ``` - the prediction matrices are computed from the [`move_blocking`](@ref) vector - ``\mathbf{n_b} = [\begin{smallmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{smallmatrix}]'`` - and the following equations: + the prediction matrices are computed from the ``j_ℓ`` integers introduced in the + [`move_blocking`](@ref) documentation and the following equations: ```math \begin{aligned} \mathbf{E} &= \begin{bmatrix} - \mathbf{Q}(0, n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Q}(n_1, n_2) & \mathbf{Q}(0, n_2) & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Q}(n_{H_c-1}, n_{H_c}) & \mathbf{Q}(n_{H_c-2}, n_{H_c}) & \cdots & \mathbf{Q}(0, n_{H_c}) \end{bmatrix} \\ + \mathbf{Q}(0, j_1, 0) & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Q}(j_1, j_2, 0) & \mathbf{Q}(j_1, j_2, j_1) & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Q}(j_{H_c-1}, j_{H_c}, 0) & \mathbf{Q}(j_{H_c-1}, j_{H_c}, j_1) & \cdots & \mathbf{Q}(j_{H_c-1}, j_{H_c}, j_{H_c-1}) \end{bmatrix} \\ \mathbf{G} &= \begin{bmatrix} \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} \\ \mathbf{Ĉ}\mathbf{Â}^{1} \mathbf{B̂_d} \\ @@ -332,7 +331,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{Ĉ}\mathbf{Â}^{2} \\ \vdots \\ \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ - \mathbf{V} &= \mathbf{Q}(0, H_p) \\ + \mathbf{V} &= \mathbf{Q}(0, H_p, 0) \\ \mathbf{B} &= \begin{bmatrix} \mathbf{Ĉ W}(0) \\ \mathbf{Ĉ W}(1) \\ @@ -344,7 +343,7 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{e_x̂} &= \begin{bmatrix} - \mathbf{W}(H_p-1)\mathbf{B̂_u} & \mathbf{W}(H_p-n_1-1)\mathbf{B̂_u} & \cdots & \mathbf{W}(H_p-n_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{W}(H_p-1)\mathbf{B̂_u} & \mathbf{W}(H_p-j_1-1)\mathbf{B̂_u} & \cdots & \mathbf{W}(H_p-j_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ \mathbf{j_x̂} &= \begin{bmatrix} \mathbf{Â}^{H_p-2}\mathbf{B̂_d} & \mathbf{Â}^{H_p-3}\mathbf{B̂_d} & \cdots & \mathbf{0} \end{bmatrix} \\ @@ -400,10 +399,12 @@ function init_predmat( iCol = (1:nu) .+ nu*(j-1) for i=1:Hc-j+1 i_Q = i > 1 ? nb[i-1] : 0 + @show i j_Q = nb[i + j - 1] iRow = (1:ny*j_Q) .+ ny*sum(nb[1:i+j-2]) Q = @views E[iRow, iCol] Q!(Q, i_Q, j_Q) + @show i_Q, j_Q end n = j > 1 ? nb[j-1] : 0 ex̂[: , iCol] = W(Hp-n-1)*B̂u From 441ad610b770a5c0fc7606193f4166d0d112fb18 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 21:18:44 -0500 Subject: [PATCH 14/22] added: implementing new 3-args `Q!` function --- src/controller/transcription.jl | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a1af0c46b..0fcbd110b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -373,13 +373,14 @@ function init_predmat( # three helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] W(m) = @views Âpow_csum[:,:, m+1] - function Q!(Q, i, j) - for ℓ=1:j - iRows = (1:ny) .+ ny*(ℓ-1) - Q[iRows, :] = Ĉ*W(i+ℓ-1)*B̂u + function Q!(Q, i, k, b) + for ℓ=0:k-i-1 + iRows = (1:ny) .+ ny*ℓ + Q[iRows, :] = Ĉ * W(i-b+ℓ) * B̂u end return Q end + jℓ = cumsum(nb) # --- current state estimates x̂0 --- kx̂ = getpower(Âpow, Hp) K = Matrix{NT}(undef, Hp*ny, nx̂) @@ -390,27 +391,28 @@ function init_predmat( # --- previous manipulated inputs lastu0 --- vx̂ = W(Hp-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) - Q!(V, 0, Hp) + Q!(V, 0, Hp, 0) # --- decision variables Z --- nZ = get_nZ(estim, transcription, Hp, Hc) ex̂ = Matrix{NT}(undef, nx̂, nZ) E = zeros(NT, Hp*ny, nZ) for j=1:Hc iCol = (1:nu) .+ nu*(j-1) - for i=1:Hc-j+1 - i_Q = i > 1 ? nb[i-1] : 0 - @show i - j_Q = nb[i + j - 1] - iRow = (1:ny*j_Q) .+ ny*sum(nb[1:i+j-2]) + for i=j:Hc + @show i, j + i_Q = (i == 1 && j == 1) ? 0 : jℓ[i-1] + k_Q = jℓ[i] + b_Q = (j == 1) ? 0 : jℓ[j-1] + iRow = (1:ny*nb[i]) .+ ny*i_Q + @show iRow Q = @views E[iRow, iCol] - Q!(Q, i_Q, j_Q) - @show i_Q, j_Q + Q!(Q, i_Q, k_Q, b_Q) end - n = j > 1 ? nb[j-1] : 0 - ex̂[: , iCol] = W(Hp-n-1)*B̂u + j_ex̂ = (j == 1) ? 0 : jℓ[j-1] + ex̂[: , iCol] = W(Hp - j_ex̂ - 1)*B̂u end - #display(E) + display(E) # --- current measured disturbances d0 and predictions D̂0 --- From 4f9277a7da47aa426ef02c0c885242cbc1674ed2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 21:28:58 -0500 Subject: [PATCH 15/22] doc: minor correction --- src/controller/construct.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index ae0b4a368..8c3a8a4b2 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -500,8 +500,8 @@ strictly positive integers): \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}' ``` Introducing the notation ``j_ℓ = ∑_{i=1}^{ℓ} n_i`` to convert from block lengths to discrete -time steps, the vector that includes all the manipulated input free moves ``\mathbf{Δu}`` is -then defined as: +time steps, the vector that includes all the free moves of the manipulated input is then +defined as: ```math \mathbf{ΔU} = \begin{bmatrix} \mathbf{Δu}(k + 0) \\ From e8ee15330ee73e96e73e9080e50d11a5dfd736f0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Jan 2026 22:14:42 -0500 Subject: [PATCH 16/22] changed: minor correction --- src/controller/transcription.jl | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 0fcbd110b..83d7bc13f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -370,6 +370,7 @@ function init_predmat( end # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... Âpow_csum = cumsum(Âpow, dims=3) + jℓ = cumsum(nb) # introduced in move_blocking docstring # three helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] W(m) = @views Âpow_csum[:,:, m+1] @@ -380,7 +381,6 @@ function init_predmat( end return Q end - jℓ = cumsum(nb) # --- current state estimates x̂0 --- kx̂ = getpower(Âpow, Hp) K = Matrix{NT}(undef, Hp*ny, nx̂) @@ -399,22 +399,16 @@ function init_predmat( for j=1:Hc iCol = (1:nu) .+ nu*(j-1) for i=j:Hc - @show i, j i_Q = (i == 1 && j == 1) ? 0 : jℓ[i-1] k_Q = jℓ[i] b_Q = (j == 1) ? 0 : jℓ[j-1] iRow = (1:ny*nb[i]) .+ ny*i_Q - @show iRow Q = @views E[iRow, iCol] Q!(Q, i_Q, k_Q, b_Q) end j_ex̂ = (j == 1) ? 0 : jℓ[j-1] - ex̂[: , iCol] = W(Hp - j_ex̂ - 1)*B̂u - end - - display(E) - - + ex̂[:, iCol] = W(Hp - j_ex̂ - 1)*B̂u + end # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d G = Matrix{NT}(undef, Hp*ny, nd) From 1a41588a3d5220ea507282130e0872d51bd0db83 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 01:38:44 -0500 Subject: [PATCH 17/22] changed: renamed `k` to `m` in `Q!` function --- src/controller/transcription.jl | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 83d7bc13f..e8a6c5c68 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -298,11 +298,11 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). [`augment_model`](@ref)), and the following two functions with integer arguments: ```math \begin{aligned} - \mathbf{Q}(i, k, b) &= \begin{bmatrix} + \mathbf{Q}(i, m, b) &= \begin{bmatrix} \mathbf{Ĉ W}(i-b+0)\mathbf{B̂_u} \\ \mathbf{Ĉ W}(i-b+1)\mathbf{B̂_u} \\ \vdots \\ - \mathbf{Ĉ W}(k-b-1)\mathbf{B̂_u} + \mathbf{Ĉ W}(m-b-1)\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ \end{aligned} @@ -374,8 +374,8 @@ function init_predmat( # three helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] W(m) = @views Âpow_csum[:,:, m+1] - function Q!(Q, i, k, b) - for ℓ=0:k-i-1 + function Q!(Q, i, m, b) + for ℓ=0:m-i-1 iRows = (1:ny) .+ ny*ℓ Q[iRows, :] = Ĉ * W(i-b+ℓ) * B̂u end @@ -400,11 +400,11 @@ function init_predmat( iCol = (1:nu) .+ nu*(j-1) for i=j:Hc i_Q = (i == 1 && j == 1) ? 0 : jℓ[i-1] - k_Q = jℓ[i] + m_Q = jℓ[i] b_Q = (j == 1) ? 0 : jℓ[j-1] iRow = (1:ny*nb[i]) .+ ny*i_Q Q = @views E[iRow, iCol] - Q!(Q, i_Q, k_Q, b_Q) + Q!(Q, i_Q, m_Q, b_Q) end j_ex̂ = (j == 1) ? 0 : jℓ[j-1] ex̂[:, iCol] = W(Hp - j_ex̂ - 1)*B̂u From 2bb5b86a8c5250dcbef7c7c687902c23406a9186 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 12:05:38 -0500 Subject: [PATCH 18/22] doc: debugged equations in `init_defectmat` --- src/controller/transcription.jl | 43 +++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e8a6c5c68..e013174e8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -558,27 +558,44 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th # Extended Help !!! details "Extended Help" - The matrices are computed by: + Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see + [`augment_model`](@ref)), the [`move_blocking`](@ref) vector ``\mathbf{n_b}``, and the + following ``\mathbf{Q}(n_i)`` matrix of size `(nx̂*ni, nu)`: + ```math + \mathbf{Q}(n_i) = \begin{bmatrix} + \mathbf{B̂_u} \\ + \mathbf{B̂_u} \\ + \vdots \\ + \mathbf{B̂_u} \end{bmatrix} + ``` + The defect matrices are computed with: ```math \begin{aligned} \mathbf{E_ŝ} &= \begin{bmatrix} - \mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} & -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{B̂_u} & \mathbf{B̂_u} & \cdots & \mathbf{0} & \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{B̂_u} & \mathbf{B̂_u} & \cdots & \mathbf{B̂_u} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ + \mathbf{E_{ŝ}^{Δu}} & \mathbf{E_{ŝ}^{x̂}} \end{bmatrix} \\ + \mathbf{E_{ŝ}^{Δu}} &= \begin{bmatrix} + \mathbf{Q}(n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Q}(n_2) & \mathbf{Q}(n_2) & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ + \mathbf{E_{ŝ}^{x̂}} &= \begin{bmatrix} + -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & -\mathbf{I} \end{bmatrix} \\ \mathbf{G_ŝ} &= \begin{bmatrix} - \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ + \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ \mathbf{J_ŝ} &= \begin{bmatrix} - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ \mathbf{K_ŝ} &= \begin{bmatrix} - \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ + \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ \mathbf{V_ŝ} &= \begin{bmatrix} - \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \vdots \\ \mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \vdots \\ \mathbf{B̂_u} \end{bmatrix} \\ \mathbf{B_ŝ} &= \begin{bmatrix} - \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} + \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} \end{aligned} ``` """ From 625669453f7c78b66c77c6cdc6d3518d2b615d4a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 12:20:37 -0500 Subject: [PATCH 19/22] debug: move blocking with `LinModel` and `MultipleShooting` works --- src/controller/execute.jl | 2 +- src/controller/linmpc.jl | 2 +- src/controller/nonlinmpc.jl | 2 +- src/controller/transcription.jl | 29 ++++++++++++++++++++++------- 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 4d3968668..7ce81ceae 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -639,7 +639,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) mpc.V .= V mpc.B .= B # --- defect matrices --- - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) con.Ẽŝ .= Ẽŝ con.Gŝ .= Gŝ diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 99e8428f7..6ed09dc8c 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -69,7 +69,7 @@ struct LinMPC{ E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) # dummy vals (updated just before optimization): F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 17bd7ba44..053a1095d 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -97,7 +97,7 @@ struct NonLinMPC{ E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) # dummy vals (updated just before optimization): F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e013174e8..925e223a2 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -539,7 +539,7 @@ function init_predmat( end @doc raw""" - init_defectmat(model::LinModel, estim, transcription::MultipleShooting, Hp, Hc) + init_defectmat(model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb) Init the matrices for computing the defects over the predicted states. @@ -598,12 +598,23 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} \end{aligned} ``` + The ``\mathbf{E_ŝ^{Δu}}`` matrix structure is due to the move blocking implementation: + the ``\mathbf{ΔU}`` vector only contains the input increment of the free moves + (see [`move_blocking`](@ref)). """ function init_defectmat( - model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc + model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc, nb ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d + # helper function to be similar to eqs. in docstring: + function Q!(Q, ni) + for ℓ=0:ni-1 + iRows = (1:nx̂) .+ nx̂*ℓ + Q[iRows, :] = B̂u + end + return Q + end # --- current state estimates x̂0 --- Kŝ = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] # --- previous manipulated inputs lastu0 --- @@ -611,10 +622,14 @@ function init_defectmat( # --- decision variables Z --- nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) Eŝ = [zeros(nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] - for j=1:Hc, i=j:Hp - iRow = (1:nx̂) .+ nx̂*(i-1) + for j=1:Hc iCol = (1:nu) .+ nu*(j-1) - Eŝ[iRow, iCol] = B̂u + for i=j:Hc + ni = nb[i] + iRow = (1:nx̂*ni) .+ nx̂*sum(nb[1:i-1]) + Q = @views Eŝ[iRow, iCol] + Q!(Q, ni) + end end for j=1:Hp-1 iRow = (1:nx̂) .+ nx̂*j @@ -630,12 +645,12 @@ function init_defectmat( end """ - init_defectmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc) + init_defectmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb) Return empty matrices for all other cases (N/A). """ function init_defectmat( - model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc + model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, _ ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) From b38eec4eeb1f9da1cc404117940f66205b7cb746 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 12:58:38 -0500 Subject: [PATCH 20/22] test: comparing move blocking results in all the cases --- Project.toml | 2 +- test/3_test_predictive_control.jl | 39 +++++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index b4bb5276f..8b582f39d 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.14.3" +version = "1.14.4" authors = ["Francis Gagnon"] [deps] diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index f94890174..6b640ea80 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1472,3 +1472,42 @@ end @test U_linmpc ≈ U_nonlinmpc2 rtol=1e-3 atol=1e-3 @test U_nonlinmpc1 ≈ U_nonlinmpc2 rtol=1e-3 atol=1e-3 end + +@testitem "LinMPC v.s. NonLinMPC with move blocking" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) + G = tf( 10, [400, 1]) + Ts = 100.0 + linmodel = LinModel(G, Ts) + f = (x, u, _, p) -> p.A * x + p.Bu * u + h = (x, _, p) -> p.C * x + nonlinmodel = NonLinModel(f, h, Ts, 1, 1, 1, p=linmodel, solver=nothing) + Mwt, Nwt, Hp, Hc = [1], [0], 30, [1, 2, 3, 24] + N = 25 + mpc1 = LinMPC(linmodel; Mwt, Nwt, Hp, Hc, transcription=SingleShooting()) + mpc2 = LinMPC(linmodel; Mwt, Nwt, Hp, Hc, transcription=MultipleShooting()) + mpc3 = NonLinMPC(linmodel; Mwt, Nwt, Hp, Hc, transcription=SingleShooting()) + mpc4 = NonLinMPC(linmodel; Mwt, Nwt, Hp, Hc, transcription=MultipleShooting()) + mpc5 = NonLinMPC(nonlinmodel; Mwt, Nwt, Hp, Hc, transcription=SingleShooting()) + mpc6 = NonLinMPC(nonlinmodel; Mwt, Nwt, Hp, Hc, transcription=MultipleShooting()) + U1, U2, U3 = zeros(1, N), zeros(1, N), zeros(1, N) + U4, U5, U6 = zeros(1, N), zeros(1, N), zeros(1, N) + for i=1:N + r = [5.0] + y = linmodel() + preparestate!(mpc1, y); preparestate!(mpc2, y); preparestate!(mpc3, y); + preparestate!(mpc4, y); preparestate!(mpc5, y); preparestate!(mpc6, y); + u1 = moveinput!(mpc1, r); u2 = moveinput!(mpc2, r); u3 = moveinput!(mpc3, r); + u4 = moveinput!(mpc4, r); u5 = moveinput!(mpc5, r); u6 = moveinput!(mpc6, r); + U1[:, i] = u1; U2[:, i] = u2; U3[:, i] = u3; + U4[:, i] = u4; U5[:, i] = u5; U6[:, i] = u6; + updatestate!(mpc1, u1, y); updatestate!(mpc2, u2, y); updatestate!(mpc3, u3, y); + updatestate!(mpc4, u4, y); updatestate!(mpc5, u5, y); updatestate!(mpc6, u6, y); + updatestate!(linmodel, u1); + end + @test U1 ≈ U2 rtol=1e-3 atol=1e-3 + @test U1 ≈ U3 rtol=1e-3 atol=1e-3 + @test U1 ≈ U4 rtol=1e-3 atol=1e-3 + @test U1 ≈ U5 rtol=1e-3 atol=1e-3 + @test U1 ≈ U6 rtol=1e-3 atol=1e-3 +end \ No newline at end of file From 65957c208dedf56fd42542711936c7bd6d462f90 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 13:59:50 -0500 Subject: [PATCH 21/22] test: debug test --- test/3_test_predictive_control.jl | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 6b640ea80..6f9970c5d 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1475,13 +1475,11 @@ end @testitem "LinMPC v.s. NonLinMPC with move blocking" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) G = tf( 10, [400, 1]) - Ts = 100.0 - linmodel = LinModel(G, Ts) + linmodel = LinModel(G, 100.0) f = (x, u, _, p) -> p.A * x + p.Bu * u h = (x, _, p) -> p.C * x - nonlinmodel = NonLinModel(f, h, Ts, 1, 1, 1, p=linmodel, solver=nothing) + nonlinmodel = NonLinModel(f, h, 100.0, 1, 1, 1, p=linmodel, solver=nothing) Mwt, Nwt, Hp, Hc = [1], [0], 30, [1, 2, 3, 24] N = 25 mpc1 = LinMPC(linmodel; Mwt, Nwt, Hp, Hc, transcription=SingleShooting()) From 7bb8e7e6b0117d2eb846a9d9e2332d6b419aa4b8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 2 Jan 2026 14:36:04 -0500 Subject: [PATCH 22/22] doc: detail --- 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 925e223a2..0a6208740 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -579,10 +579,10 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th \vdots & \vdots & \ddots & \vdots \\ \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ \mathbf{E_{ŝ}^{x̂}} &= \begin{bmatrix} - -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & -\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_ŝ} &= \begin{bmatrix} \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ \mathbf{J_ŝ} &= \begin{bmatrix}