debug: update current estimated output ŷ for MPC

Author: franckgaga

src/controller/explicitmpc.jl

@@ -33,7 +33,7 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
     function ExplicitMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt) where {SE<:StateEstimator}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ = zeros(ny)
+        ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         Cwt = Inf # no slack variable ϵ for ExplicitMPC
         Ewt = 0   # economic costs not supported for ExplicitMPC
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt)

src/controller/linmpc.jl

@@ -36,7 +36,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
     function LinMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim) where {SE<:StateEstimator}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ = zeros(ny)
+        ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         Ewt = 0   # economic costs not supported for LinMPC
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt)
         M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))

src/controller/nonlinmpc.jl

@@ -41,7 +41,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
     ) where {SE<:StateEstimator, JEFunc<:Function}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ = zeros(ny)
+        ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt)
         M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
         N_Hc = Diagonal(convert(Vector{Float64}, repeat(Nwt, Hc)))

src/predictive_control.jl

@@ -557,6 +557,7 @@ Init linear model prediction matrices `F`, `q̃` and `p`.
 See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u)
+    mpc.ŷ[:] = evalŷ(mpc.estim, ym, d)
     predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
     mpc.F[:]  = mpc.K  * mpc.estim.x̂  + mpc.V  * mpc.estim.lastu0 + mpc.Ŷop
     if model.nd ≠ 0
@@ -587,6 +588,7 @@ Init `Ŷop`, `d0` and `D̂0` matrices when model is not a [`LinModel`](@ref).
 [`InternalModel`](@ref).
 """
 function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y, R̂u)
+    mpc.ŷ[:] = evalŷ(mpc.estim, ym, d)
     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