reduce allocation EMPC

Author: franckgaga

src/controller/explicitmpc.jl

@@ -49,20 +49,22 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         P̃_chol = cholesky(P̃)
         Ks, Ps = init_stochpred(estim, Hp)
-        d0, D̂0 = zeros(nd), zeros(nd*Hp)
+        # dummy vals (updated just before optimization):
+        d0, D̂0, D̂E = zeros(nd), zeros(nd*Hp), zeros(nd + nd*Hp)
         Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim,
             ΔŨ, ŷ,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, 
+            R̂u, R̂y, noR̂u,
             S̃, T, 
             Ẽ, F, G, J, K, V, P̃, q̃, p,
             P̃_chol,
             Ks, Ps,
-            d0, D̂0,
+            d0, D̂0, D̂E,
             Ŷop, Dop,
         )
         return mpc

src/controller/linmpc.jl

@@ -50,19 +50,21 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
         con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
-        d0, D̂0 = zeros(nd), zeros(nd*Hp)
+        # dummy vals (updated just before optimization):
+        d0, D̂0, D̂E = zeros(nd), zeros(nd*Hp), zeros(nd + nd*Hp)
         Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, 
+            R̂u, R̂y, noR̂u,
             S̃, T,
             Ẽ, F, G, J, K, V, P̃, q̃, p,
             Ks, Ps,
-            d0, D̂0,
+            d0, D̂0, D̂E,
             Ŷop, Dop,
         )
         init_optimization!(mpc)

src/controller/nonlinmpc.jl

@@ -34,6 +34,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
     Ps::Matrix{Float64}
     d0::Vector{Float64}
     D̂0::Vector{Float64}
+    D̂E::Vector{Float64}
     Ŷop::Vector{Float64}
     Dop::Vector{Float64}
     function NonLinMPC{SE, JEFunc}(
@@ -54,19 +55,21 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
         con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
-        d0, D̂0 = zeros(nd), zeros(nd*Hp)
+        # dummy vals (updated just before optimization):
+        d0, D̂0, D̂E = zeros(nd), zeros(nd*Hp), zeros(nd + nd*Hp)
         Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y, noR̂u,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, 
+            R̂u, R̂y, noR̂u,
             S̃, T,  
             Ẽ, F, G, J, K, V, P̃, q̃, p,
             Ks, Ps,
-            d0, D̂0,
+            d0, D̂0, D̂E,
             Ŷop, Dop,
         )
         init_optimization!(mpc)

src/predictive_control.jl

@@ -562,6 +562,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
     mpc.F[:]  = mpc.K  * mpc.estim.x̂  + mpc.V  * mpc.estim.lastu0 + mpc.Ŷop
     if model.nd ≠ 0
         mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
+        mpc.D̂E[:] = [d; D̂]
         mpc.F[:]  = mpc.F  + mpc.G  * mpc.d0 + mpc.J  * mpc.D̂0
     end
     mpc.R̂y[:] = R̂y
@@ -592,6 +593,7 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y,
     predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
     if model.nd ≠ 0
         mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
+        mpc.D̂E[:] = [d; D̂]
     end
     mpc.R̂y[:] = R̂y
     if ~mpc.noR̂u
@@ -963,8 +965,7 @@ function obj_nonlinprog(
         U = mpc.S̃*ΔŨ + mpc.T*(mpc.estim.lastu0 + model.uop)
         UE = [U; U[(end - model.nu + 1):end]]
         ŶE = [mpc.ŷ; Ŷ]
-        D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
-        J += mpc.E*mpc.JE(UE, ŶE, D̂E)
+        J += mpc.E*mpc.JE(UE, ŶE, mpc.D̂E)
     end
     return J
 end
@@ -998,8 +999,7 @@ function obj_nonlinprog(
     if !iszero(mpc.E)
         UE = [U; U[(end - model.nu + 1):end]]
         ŶE = [mpc.ŷ; Ŷ]
-        D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
-        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
+        E_JE = mpc.E*mpc.JE(UE, ŶE, mpc.D̂E)
     else
         E_JE = 0.0
     end