- added: reduce allocations in PredictiveController with new buffers (bis)

the matrices `buffer.Ẽ` and `buffer.S̃` are added to store intermediary results

Author: franckgaga

src/controller/execute.jl

@@ -192,33 +192,34 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
     ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
     Cy, Cu = mpc.buffer.Cy, mpc.buffer.Cu
+    M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ẽ, mpc.buffer.S̃
     ŷ .= evaloutput(mpc.estim, d)
-    predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
-    F .+= mpc.B
-    mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) 
-    mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)
+    predictstoch!(mpc, mpc.estim)           # init F with Ŷs for InternalModel
+    F .+= mpc.B                             # F = F + B
+    mul!(F, mpc.K, mpc.estim.x̂0, 1, 1)      # F = F + K*x̂0
+    mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)  # F = F + V*lastu0
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
         mpc.D̂e[1:model.nd]     .= d
         mpc.D̂e[model.nd+1:end] .= D̂
-        mul!(F, mpc.G, mpc.d0, 1, 1)
-        mul!(F, mpc.J, mpc.D̂0, 1, 1)
+        mul!(F, mpc.G, mpc.d0, 1, 1)        # F = F + G*d0
+        mul!(F, mpc.J, mpc.D̂0, 1, 1)        # F = F + J*D̂0
     end
     # --- output setpoint tracking term ---
     mpc.R̂y .= R̂y
     Cy .= F .- (R̂y .- mpc.Yop)
-    M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ
-    mul!(q̃, M_Hp_Ẽ', Cy)                # q̃ = M_Hp*Ẽ'*Cy
+    mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ)
+    mul!(q̃, M_Hp_Ẽ', Cy)                    # q̃ = M_Hp*Ẽ'*Cy
     r .= dot(Cy, mpc.weights.M_Hp, Cy)
     # --- input setpoint tracking term ---
     mpc.R̂u .= R̂u
     Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) 
-    L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃
-    mul!(q̃, L_Hp_S̃', Cu, 1, 1)         # q̃ = q̃ + L_Hp*S̃'*Cu
+    mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃)
+    mul!(q̃, L_Hp_S̃', Cu, 1, 1)              # q̃ = q̃ + L_Hp*S̃'*Cu
     r .+= dot(Cu, mpc.weights.L_Hp, Cu)
     # --- finalize ---
-    lmul!(2, q̃)                        # q̃ = 2*q̃
+    lmul!(2, q̃)                             # q̃ = 2*q̃
     return nothing
 end
 
@@ -230,7 +231,7 @@ Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel
 function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
     mpc.ŷ .= evaloutput(mpc.estim, d)
-    predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
+    predictstoch!(mpc, mpc.estim)           # init F with Ŷs for InternalModel
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop

src/controller/explicitmpc.jl

@@ -60,7 +60,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE}(
             estim,
             ΔŨ, ŷ,

src/controller/linmpc.jl

@@ -65,7 +65,7 @@ struct LinMPC{
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE, JM}(
             estim, optim, con,
             ΔŨ, ŷ,

src/controller/nonlinmpc.jl

@@ -79,7 +79,7 @@ struct NonLinMPC{
         test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE, JM, JEfunc, GCfunc, P}(
             estim, optim, con,
             ΔŨ, ŷ,

src/predictive_control.jl

@@ -25,6 +25,8 @@ struct PredictiveControllerBuffer{NT<:Real}
     D̂ ::Vector{NT}
     Cy::Vector{NT}
     Cu::Vector{NT}
+    Ẽ ::Matrix{NT}
+    S̃ ::Matrix{NT}
     empty::Vector{NT}
 end
 
@@ -35,14 +37,18 @@ Create a buffer for `PredictiveController` objects.
 
 The buffer is used to store intermediate results during computation without allocating.
 """
-function PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int, Hp::Int) where NT <: Real
+function PredictiveControllerBuffer{NT}(
+    nu::Int, ny::Int, nd::Int, Hp::Int, Hc::Int, nϵ::Int
+) where NT <: Real
     u  = Vector{NT}(undef, nu)
     R̂y = Vector{NT}(undef, ny*Hp)
     D̂  = Vector{NT}(undef, nd*Hp)
     Cy = Vector{NT}(undef, ny*Hp)
     Cu = Vector{NT}(undef, nu*Hp)
+    Ẽ  = Matrix{NT}(undef, ny*Hp, nu*Hc + nϵ)
+    S̃  = Matrix{NT}(undef, nu*Hp, nu*Hc + nϵ)
     empty = Vector{NT}(undef, 0)
-    return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, empty)
+    return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, Ẽ, S̃, empty)
 end
 
 include("controller/construct.jl")