changed: moved lastu0 inside PredictiveController objects

docs/src/internals/predictive_control.md

@@ -40,5 +40,5 @@ ModelPredictiveControl.linconstrainteq!
 ```@docs
 ModelPredictiveControl.optim_objective!(::PredictiveController)
 ModelPredictiveControl.set_warmstart!
-ModelPredictiveControl.getinput
+ModelPredictiveControl.getinput!
 ```

docs/src/public/state_estim.md

@@ -81,6 +81,12 @@ MovingHorizonEstimator
 InternalModel
 ```
 
+## ManualEstimator
+
+```@docs
+ManualEstimator
+```
+
 ## Default Model Augmentation
 
 ```@docs

src/ModelPredictiveControl.jl

@@ -36,6 +36,7 @@ export savetime!, periodsleep
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export MovingHorizonEstimator
+export ManualEstimator
 export default_nint, initstate!
 export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput!
 export TranscriptionMethod, SingleShooting, MultipleShooting

src/controller/construct.jl

@@ -1,3 +1,6 @@
+const MSG_LINMODEL_ERR = "estim.model type must be a LinModel, see ManualEstimator docstring "*
+                         "to use a nonlinear state estimator with a linear controller"
+
 struct PredictiveControllerBuffer{NT<:Real}
     u ::Vector{NT}
     Z̃ ::Vector{NT}

src/controller/execute.jl

@@ -2,9 +2,13 @@
     initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂
 
 Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero.
+
+It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the input increments
+``\mathbf{ΔU}`` to inputs ``\mathbf{U}``.
 """
 function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
     mpc.Z̃ .= 0
+    mpc.lastu0 .= u .- mpc.estim.model.uop
     return initstate!(mpc.estim, u, ym, d)
 end
 
@@ -16,10 +20,11 @@ Compute the optimal manipulated input value `u` for the current control period.
 Solve the optimization problem of `mpc` [`PredictiveController`](@ref) and return the
 results ``\mathbf{u}(k)``. Following the receding horizon principle, the algorithm discards
 the optimal future manipulated inputs ``\mathbf{u}(k+1), \mathbf{u}(k+2), ...`` Note that
-the method mutates `mpc` internal data but it does not modifies `mpc.estim` states. Call
-[`preparestate!(mpc, ym, d)`](@ref) before `moveinput!`, and [`updatestate!(mpc, u, ym, d)`](@ref)
-after, to update `mpc` state estimates. Setpoint and measured disturbance previews can
-be implemented with the `R̂y`, `R̂u` and `D̂` keyword arguments. 
+the method mutates `mpc` internal data (it stores `u - mpc.estim.model.uop` at `mpc.lastu0`
+for instance) but it does not modifies `mpc.estim` states. Call [`preparestate!(mpc, ym, d)`](@ref)
+before `moveinput!`, and [`updatestate!(mpc, u, ym, d)`](@ref) after, to update `mpc` state
+estimates. Setpoint and measured disturbance previews can be implemented with the `R̂y`, `R̂u`
+and `D̂` keyword arguments. 
 
 Calling a [`PredictiveController`](@ref) object calls this method.
 
@@ -69,7 +74,7 @@ function moveinput!(
     linconstraint!(mpc, mpc.estim.model, mpc.transcription)
     linconstrainteq!(mpc, mpc.estim.model, mpc.transcription)
     Z̃ = optim_objective!(mpc)
-    return getinput(mpc, Z̃)
+    return getinput!(mpc, Z̃)
 end
 
 @doc raw"""
@@ -207,7 +212,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     F   = initpred_common!(mpc, model, d, D̂, R̂y, R̂u)
     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
+    mul!(F, mpc.V, mpc.lastu0, 1, 1)            # F = F + V*lastu0
     if model.nd > 0
         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
@@ -254,7 +259,7 @@ Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs`
 is an [`InternalModel`](@ref). The function returns `mpc.F`.
 """
 function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
-    mul!(mpc.Tu_lastu0, mpc.Tu, mpc.estim.lastu0)
+    mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0)
     mpc.ŷ .= evaloutput(mpc.estim, d)
     if model.nd > 0
         mpc.d0 .= d .- model.dop
@@ -446,20 +451,23 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty)
 end
 
 @doc raw"""
-    getinput(mpc::PredictiveController, Z̃) -> u
+    getinput!(mpc::PredictiveController, Z̃) -> u
 
-Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `Z̃`.
+Get current manipulated input `u` from the solution `Z̃`, store it and return it.
 
 The first manipulated input ``\mathbf{u}(k)`` is extracted from the decision vector
-``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle).
+``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle). It also
+stores `u - mpc.estim.model.uop` at `mpc.lastu0`.
 """
-function getinput(mpc, Z̃)
+function getinput!(mpc, Z̃)
+    model = mpc.estim.model
     Δu  = mpc.buffer.u
-    for i in 1:mpc.estim.model.nu
+    for i in 1:model.nu
         Δu[i] = Z̃[i]
     end
     u   = Δu
-    u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop
+    u .+= mpc.lastu0 .+ model.uop
+    mpc.lastu0 .=  u .- model.uop
     return u
 end
 
@@ -548,6 +556,7 @@ function setmodel!(
         Ñ_Hc      = Ntilde_Hc,
         kwargs...
     )
+    uop_old = copy(mpc.estim.model.uop)
     x̂op_old = copy(mpc.estim.x̂op)
     nu, ny, Hp, Hc, nϵ = model.nu, model.ny, mpc.Hp, mpc.Hc, mpc.nϵ
     setmodel!(mpc.estim, model; kwargs...)
@@ -593,12 +602,12 @@ function setmodel!(
         mpc.weights.L_Hp .= L_Hp
         mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp)
     end
-    setmodel_controller!(mpc, x̂op_old)
+    setmodel_controller!(mpc, uop_old, x̂op_old)
     return mpc
 end
 
 "Update the prediction matrices, linear constraints and JuMP optimization."
-function setmodel_controller!(mpc::PredictiveController, x̂op_old)
+function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
@@ -628,6 +637,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old)
     con.x̂0min .+= x̂op_old # convert x̂0 to x̂ with the old operating point
     con.x̂0max .+= x̂op_old # convert x̂0 to x̂ with the old operating point
     # --- operating points ---
+    mpc.lastu0 .+= uop_old .- model.uop
     for i in 0:Hp-1
         mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop
         mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop

src/controller/explicitmpc.jl

@@ -13,6 +13,7 @@ struct ExplicitMPC{
     weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
+    lastu0::Vector{NT}
     P̃Δu::Matrix{NT}
     P̃u ::Matrix{NT} 
     Tu ::Matrix{NT}
@@ -46,12 +47,13 @@ struct ExplicitMPC{
         nϵ = 0    # no slack variable ϵ for ExplicitMPC
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        lastu0 = zeros(NT, nu)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
-        F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
+        F = zeros(NT, ny*Hp)
         P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
@@ -71,6 +73,7 @@ struct ExplicitMPC{
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,
+            lastu0,
             P̃Δu, P̃u, Tu, Tu_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
@@ -157,7 +160,7 @@ function ExplicitMPC(
     N_Hc = Diagonal(repeat(Nwt, Hc)),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
 ) where {NT<:Real, SE<:StateEstimator{NT}}
-    isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
+    isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) 
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
@@ -215,7 +218,7 @@ addinfo!(info, mpc::ExplicitMPC) = info
 
 
 "Update the prediction matrices and Cholesky factorization."
-function setmodel_controller!(mpc::ExplicitMPC, _ )
+function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ )
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     # --- predictions matrices ---
@@ -232,6 +235,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ )
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc)
     # --- operating points ---
+    mpc.lastu0 .+= uop_old .- model.uop
     for i in 0:Hp-1
         mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop
         mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop

src/controller/linmpc.jl

@@ -22,6 +22,7 @@ struct LinMPC{
     weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
+    lastu0::Vector{NT}
     P̃Δu::Matrix{NT}
     P̃u ::Matrix{NT} 
     Tu ::Matrix{NT}
@@ -60,6 +61,7 @@ struct LinMPC{
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
@@ -91,6 +93,7 @@ struct LinMPC{
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,
+            lastu0,
             P̃Δu, P̃u, Tu, Tu_lastu0,
             Ẽ, F, G, J, K, V, B, 
             H̃, q̃, r,
@@ -256,7 +259,7 @@ function LinMPC(
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
 ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel}
-    isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
+    isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) 
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*

src/controller/nonlinmpc.jl

@@ -38,6 +38,7 @@ struct NonLinMPC{
     p::PT
     R̂u::Vector{NT}
     R̂y::Vector{NT}
+    lastu0::Vector{NT}
     P̃Δu::Matrix{NT}
     P̃u ::Matrix{NT}
     Tu ::Matrix{NT}
@@ -83,6 +84,7 @@ struct NonLinMPC{
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
@@ -118,6 +120,7 @@ struct NonLinMPC{
             weights,
             JE, p,
             R̂u, R̂y,
+            lastu0,
             P̃Δu, P̃u, Tu, Tu_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,

src/controller/transcription.jl

@@ -852,7 +852,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript
     nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂
     fx̂ .= mpc.con.bx̂
     mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
-    mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
+    mul!(fx̂, mpc.con.vx̂, mpc.lastu0, 1, 1)
     if model.nd > 0
         mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
         mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
@@ -938,7 +938,7 @@ function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::Multiple
     Fŝ  = mpc.con.Fŝ
     Fŝ .= mpc.con.Bŝ
     mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1)
-    mul!(Fŝ, mpc.con.Vŝ, mpc.estim.lastu0, 1, 1)
+    mul!(Fŝ, mpc.con.Vŝ, mpc.lastu0, 1, 1)
     if model.nd > 0
         mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1)
         mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1)

src/estimator/execute.jl

@@ -2,17 +2,13 @@
     remove_op!(estim::StateEstimator, ym, d, u=nothing) -> y0m, d0, u0
 
 Remove operating pts on measured outputs `ym`, disturbances `d` and inputs `u` (if provided).
-
-If `u` is provided, also store current inputs without operating points `u0` in 
-`estim.lastu0`. This field is used for [`PredictiveController`](@ref) computations.
 """
 function remove_op!(estim::StateEstimator, ym, d, u=nothing)
     y0m, u0, d0 = estim.buffer.ym, estim.buffer.u, estim.buffer.d
     y0m .= @views ym .- estim.model.yop[estim.i_ym]
     d0  .= d  .- estim.model.dop
     if !isnothing(u)
         u0 .= u .- estim.model.uop
-        estim.lastu0 .= u0
     end
     return y0m, d0, u0
 end
@@ -108,8 +104,7 @@ 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
-stores `u - estim.model.uop` at `estim.lastu0` and removes the operating points with 
-[`remove_op!`](@ref), and call [`init_estimate!`](@ref):
+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 
@@ -408,7 +403,6 @@ function setmodel!(
     yop_old = copy(estim.model.yop)
     dop_old = copy(estim.model.dop)
     setmodel_linmodel!(estim.model, model)
-    estim.lastu0 .+= uop_old .- model.uop
     setmodel_estimator!(estim, model, uop_old, yop_old, dop_old, Q̂, R̂)
     return estim
 end

src/estimator/internal_model.jl

@@ -1,6 +1,5 @@
 struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     model::SM
-    lastu0::Vector{NT}
     x̂op::Vector{NT}
     f̂op::Vector{NT}
     x̂0 ::Vector{NT}
@@ -41,7 +40,6 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)
         Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:]
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
-        lastu0 = zeros(NT, nu)
         # x̂0 and x̂d are same object (updating x̂d will update x̂0):
         x̂d = x̂0 = zeros(NT, model.nx) 
         x̂s, x̂snext = zeros(NT, nxs), zeros(NT, nxs)
@@ -51,7 +49,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
         return new{NT, SM}(
             model, 
-            lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext,
+            x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Bs, Cs, Ds, 
             Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,