added Uop vector for EMPC

Author: franckgaga

src/controller/linmpc.jl

@@ -26,11 +26,14 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     q̃ ::Vector{Float64}
     Ks::Matrix{Float64}
     Ps::Matrix{Float64}
+    d0::Vector{Float64}
+    D̂0::Vector{Float64}
+    Uop::Vector{Float64}
     Yop::Vector{Float64}
     Dop::Vector{Float64}
     function LinMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim) where {S<:StateEstimator}
         model = estim.model
-        nu, nxd, nxs, ny = model.nu, model.nx, estim.nxs, model.ny
+        nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
         x̂d, x̂s, ŷ = zeros(nxd), zeros(nxs), zeros(ny)
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
         M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
@@ -69,8 +72,8 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
         nvar = size(Ẽ, 2)
         P̃, q̃ = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
-        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
-        set_silent(optim)
+        d0, D̂0 = zeros(nd), zeros(nd*Hp)
+        Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         @variable(optim, ΔŨ[1:nvar])
         A = con.A[con.i_b, :]
         b = con.b[con.i_b]
@@ -89,9 +92,11 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,
-            Yop, Dop,
+            d0, D̂0,
+            Uop, Yop, Dop,
         )
         @objective(optim, Min, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
+        set_silent(optim)
         return mpc
     end
 end

src/controller/nonlinmpc.jl

@@ -28,13 +28,16 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
     q̃ ::Vector{Float64}
     Ks::Matrix{Float64}
     Ps::Matrix{Float64}
+    d0::Vector{Float64}
+    D̂0::Vector{Float64}
+    Uop::Vector{Float64}
     Yop::Vector{Float64}
     Dop::Vector{Float64}
     function NonLinMPC{S, JEFunc}(
         estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE::JEFunc, ru, optim
     ) where {S<:StateEstimator, JEFunc<:Function}
         model = estim.model
-        nu, nxd, nxs, ny = model.nu, model.nx, estim.nxs, model.ny
+        nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
         x̂d, x̂s, ŷ = zeros(nxd), zeros(nxs), zeros(ny)
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, Ewt)
         M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
@@ -73,8 +76,8 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
         nvar = size(Ẽ, 2)
         P̃, q̃ = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
-        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
-        set_silent(optim)
+        d0, D̂0 = zeros(nd), zeros(nd*Hp)
+        Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         @variable(optim, ΔŨ[1:nvar])
         A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ŷmin; A_Ŷmax]
         A = con.A[con.i_b, :]
@@ -95,11 +98,13 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,
-            Yop, Dop,
+            d0, D̂0,
+            Uop, Yop, Dop,
         )
         J = (ΔŨ...) -> obj_nonlinprog(mpc, model, ΔŨ)
         register(mpc.optim, :J, nvar, J, autodiff=true)
         @NLobjective(mpc.optim, Min, J(ΔŨ...))
+        set_silent(optim)
         return mpc
     end
 end

src/predictive_control.jl

@@ -353,18 +353,18 @@ See [`init_deterpred`](@ref) and [`init_quadprog`](@ref) for the definition of t
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂y)
     mpc.F[:] = mpc.Kd*mpc.x̂d + mpc.Q*mpc.estim.lastu0 + Ŷs + mpc.Yop
     if model.nd ≠ 0
-        mpc.F .+= mpc.G*(d - model.dop) + mpc.J*(D̂ - mpc.Dop)
+        mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
+        mpc.F[:] = mpc.F + mpc.G*mpc.d0 + mpc.J*mpc.D̂0
     end
     Ẑ = mpc.F - R̂y
     mpc.q̃[:] = 2(mpc.M_Hp*mpc.Ẽ)'*Ẑ
     p = Ẑ'*mpc.M_Hp*Ẑ
     if ~isempty(mpc.R̂u)
-        V̂ = mpc.T_Hp*(mpc.estim.lastu0 + model.uop) - mpc.R̂u
+        lastu = mpc.estim.lastu0 + model.uop
+        V̂ = mpc.T_Hp*lastu - mpc.R̂u
         mpc.q̃[:] = mpc.q̃ + 2(mpc.L_Hp*mpc.T_Hp)'*V̂
         p += V̂'*mpc.L_Hp*V̂
     end
-    #d0 = zeros(model.nd, 0)         # only used for NonLinModel objects
-    #D̂0 = zeros(model.nd*mpc.Hp, 0)  # only used for NonLinModel objects
     return p
 end
 
@@ -380,9 +380,10 @@ without the operating points ``\mathbf{d_{op}}``.
 """
 function initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs , _ )
     mpc.F[:] = Ŷs + mpc.Yop
+    if model.nd ≠ 0
+        mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
+    end
     p = 0.0 # only used for LinModel objects
-    #d0 = d - model.dop
-    #D̂0 = D̂ - mpc.Dop
     return p
 end