Merge branch 'main' into move_block

Author: franckgaga

Project.toml

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

README.md

@@ -129,6 +129,7 @@ for more detailed examples.
   - extended Kalman filter
   - unscented Kalman filter
   - moving horizon estimator
+- disable built-in observer to manually provide your own state estimate
 - easily estimate unmeasured disturbances by adding one or more integrators at the:
   - manipulated inputs
   - measured outputs

docs/make.jl

@@ -12,6 +12,7 @@ links = InterLinks(
     "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv",
     "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv",
     "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv",
+    "LowLevelParticleFilters" => "https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/objects.inv",
 )
 
 DocMeta.setdocmeta!(

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/internals/state_estim.md

@@ -39,6 +39,7 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 
 ```@docs
 ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
+ModelPredictiveControl.set_warmstart_mhe!
 ```
 
 ## Remove Operating Points

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

@@ -14,6 +14,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}
@@ -47,12 +48,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):
@@ -72,6 +74,7 @@ struct ExplicitMPC{
             Hp, Hc, nϵ, nb,
             weights,
             R̂u, R̂y,
+            lastu0,
             P̃Δu, P̃u, Tu, Tu_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
@@ -158,7 +161,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 "*
@@ -217,7 +220,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 ---
@@ -234,6 +237,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