added: nb argument to init_predmat

franckgaga · franckgaga · commit 04b169256b08 · 2025-12-31T17:24:36.000-05:00
diff --git 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, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ)
     A_x̂min, A_x̂max, ẽ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
@@ -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, Ẽ = 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̃ = E  # no slack variable ϵ for ExplicitMPC
     mpc.Ẽ .= Ẽ
     mpc.G .= G
diff --git 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
         )
         Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = 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
@@ -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
         )
         Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = 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
@@ -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}
     Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, 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(Â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}
     Ĉ, D̂d = estim.Ĉ, 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)

Original file line number	Diff line number	Diff line change
`@@ -67,7 +67,7 @@ struct LinMPC{`
`67`	`67`	`PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)`
`68`	`68`	`Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)`
`69`	`69`	`E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(`
`70`		`- model, estim, transcription, Hp, Hc`
	`70`	`+ model, estim, transcription, Hp, Hc, nb`
`71`	`71`	`)`
`72`	`72`	`Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)`
`73`	`73`	`# dummy vals (updated just before optimization):`
Original file line number	Diff line number	Diff line change
`@@ -95,7 +95,7 @@ struct NonLinMPC{`
`95`	`95`	`PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)`
`96`	`96`	`Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)`
`97`	`97`	`E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(`
`98`		`- model, estim, transcription, Hp, Hc`
	`98`	`+ model, estim, transcription, Hp, Hc, nb`
`99`	`99`	`)`
`100`	`100`	`Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)`
`101`	`101`	`# dummy vals (updated just before optimization):`