Merge pull request #102 from JuliaControl/cleanup_&_mpc_lessalloc

Warning `preparestate!`, debug `InternalModel`, reduce alloc `PredictiveController`

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.24.0"
+version = "0.24.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

docs/src/internals/predictive_control.md

@@ -34,4 +34,5 @@ ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
 
 ```@docs
 ModelPredictiveControl.optim_objective!(::PredictiveController)
+ModelPredictiveControl.getinput
 ```

docs/src/internals/state_estim.md

@@ -40,12 +40,6 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
 ```
 
-## Evaluate Estimated Output
-
-```@docs
-ModelPredictiveControl.evalŷ
-```
-
 ## Remove Operating Points
 
 ```@docs

src/controller/construct.jl

@@ -603,15 +603,15 @@ Init the quadratic programming Hessian `H̃` for MPC.
 
 The matrix appear in the quadratic general form:
 ```math
-    J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + p 
+    J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + r 
 ```
 The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): 
 ```math
     \mathbf{H̃} = 2 (  \mathbf{Ẽ}'\mathbf{M}_{H_p}\mathbf{Ẽ} + \mathbf{Ñ}_{H_c} 
                     + \mathbf{S̃}'\mathbf{L}_{H_p}\mathbf{S̃} )
 ```
-The vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period ``k``, see
-[`initpred!`](@ref). ``p`` does not impact the minima position. It is thus useless at
+The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see
+[`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
 optimization but required to evaluate the minimal ``J`` value.
 """
 function init_quadprog(::LinModel, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)

src/controller/execute.jl

@@ -42,33 +42,34 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).
 ```jldoctest
 julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1);
 
-julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3)
+julia> preparestate!(mpc, [0]); ry = [5];
+
+julia> u = moveinput!(mpc, ry); round.(u, digits=3)
 1-element Vector{Float64}:
  1.0
 ```
 """
 function moveinput!(
     mpc::PredictiveController, 
     ry::Vector = mpc.estim.model.yop, 
-    d ::Vector = mpc.estim.buffer.empty;
-    Dhat ::Vector = repeat(d,  mpc.Hp),
-    Rhaty::Vector = repeat(ry, mpc.Hp),
+    d ::Vector = mpc.buffer.empty;
+    Dhat ::Vector = repeat!(mpc.buffer.D̂,  d,  mpc.Hp),
+    Rhaty::Vector = repeat!(mpc.buffer.R̂y, ry, mpc.Hp),
     Rhatu::Vector = mpc.Uop,
     D̂  = Dhat,
     R̂y = Rhaty,
     R̂u = Rhatu
 )
+    if mpc.estim.direct && !mpc.estim.corrected[]
+        @warn "preparestate! should be called before moveinput! with current estimators"
+    end
     validate_args(mpc, ry, d, D̂, R̂y, R̂u)
     initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u)
     linconstraint!(mpc, mpc.estim.model)
     ΔŨ = optim_objective!(mpc)
-    Δu = ΔŨ[1:mpc.estim.model.nu] # receding horizon principle: only Δu(k) is used (1st one)
-    u = mpc.estim.lastu0 + mpc.estim.model.uop + Δu
-    return u
+    return getinput(mpc, ΔŨ)
 end
 
-
-
 @doc raw"""
     getinfo(mpc::PredictiveController) -> info
 
@@ -102,7 +103,7 @@ available for [`NonLinMPC`](@ref).
 ```jldoctest
 julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1);
 
-julia> u = moveinput!(mpc, [10]);
+julia> preparestate!(mpc, [0]); u = moveinput!(mpc, [10]);
 
 julia> round.(getinfo(mpc)[:Ŷ], digits=3)
 1-element Vector{Float64}:
@@ -164,7 +165,7 @@ end
 @doc raw"""
     initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing
 
-Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`.
+Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`.
 
 See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 They are computed with these equations using in-place operations:
@@ -176,15 +177,15 @@ They are computed with these equations using in-place operations:
     \mathbf{C_u}     &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}})          \\
     \mathbf{q̃}       &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} 
                             + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}]              \\
-    p                &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
+    r                &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
                             + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u}
 \end{aligned}
 ```
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
-    ŷ, F, q̃, p = mpc.ŷ, mpc.F, mpc.q̃, mpc.p
-    ŷ .= evalŷ(mpc.estim, d)
+    ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
+    ŷ .= 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) 
@@ -201,13 +202,13 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     Cy = F .- mpc.R̂y0
     M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
     mul!(q̃, M_Hp_Ẽ', Cy)
-    p .= dot(Cy, mpc.M_Hp, Cy)
+    r .= dot(Cy, mpc.M_Hp, Cy)
     if ~mpc.noR̂u
         mpc.R̂u0 .= R̂u .- mpc.Uop
         Cu = mpc.T_lastu0 .- mpc.R̂u0
         L_Hp_S̃ = mpc.L_Hp*mpc.S̃
         mul!(q̃, L_Hp_S̃', Cu, 1, 1)
-        p .+= dot(Cu, mpc.L_Hp, Cu)
+        r .+= dot(Cu, mpc.L_Hp, Cu)
     end
     lmul!(2, q̃)
     return nothing
@@ -220,7 +221,7 @@ Init `ŷ, F, d0, D̂0, D̂E, R̂y0, R̂u0` vectors when model is not a [`LinMod
 """
 function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
-    mpc.ŷ .= evalŷ(mpc.estim, d)
+    mpc.ŷ .= evaloutput(mpc.estim, d)
     predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
@@ -367,9 +368,9 @@ at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates t
 function obj_nonlinprog!(
     U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
-    J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[]
+    J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
     if !iszero(mpc.E)
-        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
+        ŷ, D̂E = mpc.ŷ, mpc.D̂E
         U = U0
         U  .+= mpc.Uop
         uend = @views U[(end-model.nu+1):end]
@@ -501,6 +502,24 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty)
     return preparestate!(mpc.estim, ym, d)
 end
 
+@doc raw"""
+    getinput(mpc::PredictiveController, ΔŨ) -> u
+
+Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `ΔŨ`.
+
+The first manipulated input ``\mathbf{u}(k)`` is extracted from the input increments vector
+``\mathbf{ΔŨ}`` and applied on the plant (from the receding horizon principle).
+"""
+function getinput(mpc, ΔŨ)
+    Δu  = mpc.buffer.u
+    for i in 1:mpc.estim.model.nu
+        Δu[i] = ΔŨ[i]
+    end
+    u   = Δu
+    u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop
+    return u
+end
+
 """
     updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂next
 

src/controller/explicitmpc.jl

@@ -24,7 +24,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     H̃_chol::Cholesky{NT, Matrix{NT}}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
@@ -34,6 +34,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
+    buffer::PredictiveControllerBuffer{NT}
     function ExplicitMPC{NT, SE}(
         estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp
     ) where {NT<:Real, SE<:StateEstimator}
@@ -57,14 +58,15 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         S̃, Ñ_Hc, Ẽ  = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         H̃_chol = cholesky(H̃)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         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)
         mpc = new{NT, SE}(
             estim,
             ΔŨ, ŷ,
@@ -73,11 +75,12 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
-            H̃, q̃, p,
+            H̃, q̃, r,
             H̃_chol,
             Ks, Ps,
             d0, D̂0, D̂E,
             Uop, Yop, Dop,
+            buffer
         )
         return mpc
     end

src/controller/linmpc.jl

@@ -34,7 +34,7 @@ struct LinMPC{
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
@@ -43,6 +43,7 @@ struct LinMPC{
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
+    buffer::PredictiveControllerBuffer{NT}
     function LinMPC{NT, SE, JM}(
         estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM
     ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel}
@@ -67,13 +68,14 @@ struct LinMPC{
         )
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         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)
         mpc = new{NT, SE, JM}(
             estim, optim, con,
             ΔŨ, ŷ,
@@ -82,10 +84,11 @@ struct LinMPC{
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B, 
-            H̃, q̃, p,
+            H̃, q̃, r,
             Ks, Ps,
             d0, D̂0, D̂E,
             Uop, Yop, Dop,
+            buffer
         )
         init_optimization!(mpc, model, optim)
         return mpc

src/controller/nonlinmpc.jl

@@ -36,7 +36,7 @@ struct NonLinMPC{
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
@@ -45,6 +45,7 @@ struct NonLinMPC{
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
+    buffer::PredictiveControllerBuffer{NT}
     function NonLinMPC{NT, SE, JM, JEFunc}(
         estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, optim::JM
     ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function}
@@ -68,13 +69,14 @@ struct NonLinMPC{
         )
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         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)
         mpc = new{NT, SE, JM, JEFunc}(
             estim, optim, con,
             ΔŨ, ŷ,
@@ -83,10 +85,11 @@ struct NonLinMPC{
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
-            H̃, q̃, p,
+            H̃, q̃, r,
             Ks, Ps,
             d0, D̂0, D̂E,
             Uop, Yop, Dop,
+            buffer
         )
         init_optimization!(mpc, model, optim)
         return mpc

src/estimator/execute.jl

@@ -87,7 +87,8 @@ end
 Init `estim.x̂0` states from current inputs `u`, measured outputs `ym` and disturbances `d`.
 
 The method tries to find a good steady-state for the initial estimate ``\mathbf{x̂}``. It
-removes the operating points with [`remove_op!`](@ref) and call [`init_estimate!`](@ref):
+stores `u - estim.model.uop` at `estim.lastu0` and removes the operating points with 
+[`remove_op!`](@ref), and call [`init_estimate!`](@ref):
 
 - If `estim.model` is a [`LinModel`](@ref), it finds the steady-state of the augmented model
   using `u` and `d` arguments, and uses the `ym` argument to enforce that 
@@ -99,16 +100,14 @@ If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂_0`.
 
 # Examples
 ```jldoctest
-julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2]);
+julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2], direct=false);
 
 julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3)
 3-element Vector{Float64}:
   5.0
   0.0
  -0.1
 
-julia> preparestate!(estim, y); 
-
 julia> x̂ ≈ updatestate!(estim, u, y)
 true
 
@@ -171,19 +170,25 @@ init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing
 
 Evaluate `StateEstimator` outputs `ŷ` from `estim.x̂0` states and disturbances `d`.
 
-It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. Calling a
-[`StateEstimator`](@ref) object calls this `evaloutput` method.
+It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. If `estim.direct` is
+`true`, the method [`preparestate!`](@ref) should be called beforehand to correct the state
+estimate. 
+
+Calling a [`StateEstimator`](@ref) object calls this `evaloutput` method.
 
 # Examples
 ```jldoctest
-julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20]));
+julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20]), direct=false);
 
 julia> ŷ = evaloutput(kf)
 1-element Vector{Float64}:
  20.0
 ```
 """
 function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real
+    if estim.direct && !estim.corrected[]
+        @warn "preparestate! should be called before evaloutput with current estimators"
+    end
     validate_args(estim.model, d)
     ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d
     d0 .= d .- estim.model.dop