Merge 67d924d into 092cf30

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -26,7 +26,7 @@ ModelPredictiveControl.init_matconstraint_mpc
 ## Update Quadratic Optimization
 
 ```@docs
-ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any)
+ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any)
 ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
 ```
 

docs/src/internals/state_estim.md

@@ -52,7 +52,7 @@ ModelPredictiveControl.remove_op!
 ModelPredictiveControl.init_estimate!
 ```
 
-## Update Estimate
+## Correct and Update Estimate
 
 !!! info
     All these methods assume that the `u0`, `y0m` and `d0` arguments are deviation vectors
@@ -63,5 +63,6 @@ ModelPredictiveControl.init_estimate!
     ``\mathbf{x̂_0}``, respectively.
 
 ```@docs
+ModelPredictiveControl.correct_estimate!
 ModelPredictiveControl.update_estimate!
 ```

docs/src/manual/linmpc.md

@@ -62,18 +62,18 @@ plant simulator to test the design. Its sampling time is 2 s thus the control pe
 ## Linear Model Predictive Controller
 
 A linear model predictive controller (MPC) will control both the water level ``y_L`` and
-temperature ``y_T`` in the tank. The tank level should also never fall below 45:
+temperature ``y_T`` in the tank. The tank level should also never fall below 48:
 
 ```math
-y_L ≥ 45
+y_L ≥ 48
 ```
 
 We design our [`LinMPC`](@ref) controllers by including the linear level constraint with
 [`setconstraint!`](@ref) (`±Inf` values should be used when there is no bound):
 
 ```@example 1
 mpc = LinMPC(model, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])
-mpc = setconstraint!(mpc, ymin=[45, -Inf])
+mpc = setconstraint!(mpc, ymin=[48, -Inf])
 ```
 
 in which `Hp` and `Hc` keyword arguments are respectively the predictive and control
@@ -116,10 +116,11 @@ function test_mpc(mpc, model)
         i == 101 && (ry = [54, 30])
         i == 151 && (ul = -20)
         y = model() # simulated measurements
+        preparestate!(mpc, y) # prepare mpc state estimate for current iteration
         u = mpc(ry) # or equivalently : u = moveinput!(mpc, ry)
         u_data[:,i], y_data[:,i], ry_data[:,i] = u, y, ry
-        updatestate!(mpc, u, y) # update mpc state estimate
-        updatestate!(model, u + [0; ul]) # update simulator with the load disturbance
+        updatestate!(mpc, u, y) # update mpc state estimate for next iteration
+        updatestate!(model, u + [0; ul]) # update simulator with load disturbance
     end
     return u_data, y_data, ry_data
 end
@@ -131,10 +132,10 @@ nothing # hide
 The [`LinMPC`](@ref) objects are also callable as an alternative syntax for
 [`moveinput!`](@ref). It is worth mentioning that additional information like the optimal
 output predictions ``\mathbf{Ŷ}`` can be retrieved by calling [`getinfo`](@ref) after
-solving the problem. Also, calling [`updatestate!`](@ref) on the `mpc` object updates its
-internal state for the *NEXT* control period (this is by design, see
-[Functions: State Estimators](@ref) for justifications). That is why the call is done at the
-end of the `for` loop. The same logic applies for `model`.
+solving the problem. Also, calling [`preparestate!`](@ref) on the `mpc` object prepares the
+estimates for the current control period, and [`updatestate!`](@ref) updates them for the
+next one (the same logic applies for `model`). This is why [`preparestate!`](@ref) is called
+before the controller, and [`updatestate!`](@ref), after.
 
 Lastly, we plot the closed-loop test with the `Plots` package:
 
@@ -143,7 +144,7 @@ using Plots
 function plot_data(t_data, u_data, y_data, ry_data)
     p1 = plot(t_data, y_data[1,:], label="meas.", ylabel="level")
     plot!(p1, t_data, ry_data[1,:], label="setpoint", linestyle=:dash, linetype=:steppost)
-    plot!(p1, t_data, fill(45,size(t_data)), label="min", linestyle=:dot, linewidth=1.5)
+    plot!(p1, t_data, fill(48,size(t_data)), label="min", linestyle=:dot, linewidth=1.5)
     p2 = plot(t_data, y_data[2,:], label="meas.", legend=:topleft, ylabel="temp.")
     plot!(p2, t_data, ry_data[2,:],label="setpoint", linestyle=:dash, linetype=:steppost)
     p3 = plot(t_data,u_data[1,:],label="cold", linetype=:steppost, ylabel="flow rate")
@@ -162,11 +163,11 @@ real-life control problems. Constructing a [`LinMPC`](@ref) with input integrato
 
 ```@example 1
 mpc2 = LinMPC(model, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1], nint_u=[1, 1])
-mpc2 = setconstraint!(mpc2, ymin=[45, -Inf])
+mpc2 = setconstraint!(mpc2, ymin=[48, -Inf])
 ```
 
-does accelerate the rejection of the load disturbance and eliminates the level constraint
-violation:
+does accelerate the rejection of the load disturbance and almost eliminates the level
+constraint violation:
 
 ```@example 1
 setstate!(model, zeros(model.nx))
@@ -246,7 +247,7 @@ A [`LinMPC`](@ref) controller is constructed on this model:
 
 ```@example 1
 mpc_d = LinMPC(model_d, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])
-mpc_d = setconstraint!(mpc_d, ymin=[45, -Inf])
+mpc_d = setconstraint!(mpc_d, ymin=[48, -Inf])
 ```
 
 A new test function that feeds the measured disturbance ``\mathbf{d}`` to the controller is
@@ -264,6 +265,7 @@ function test_mpc_d(mpc_d, model)
         i == 151 && (ul = -20)
         d = ul .+ dop   # simulated measured disturbance
         y = model()     # simulated measurements
+        preparestate!(mpc_d, y, d) # prepare estimate with the measured disturbance d
         u = mpc_d(ry, d) # also feed the measured disturbance d to the controller
         u_data[:,i], y_data[:,i], ry_data[:,i] = u, y, ry
         updatestate!(mpc_d, u, y, d)    # update estimate with the measured disturbance d

docs/src/manual/nonlinmpc.md

@@ -89,11 +89,11 @@ method.
 An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
 
 ```@example 1
-α=0.01; σQ=[0.1, 0.5]; σR=[0.5]; nint_u=[1]; σQint_u=[0.1]
+α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1]
 estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
 ```
 
-The vectors `σQ` and σR `σR` are the standard deviations of the process and sensor noises,
+The vectors `σQ` and `σR` are the standard deviations of the process and sensor noises,
 respectively. The value for the velocity ``ω`` is higher here (`σQ` second value) since
 ``\dot{ω}(t)`` equation includes an uncertain parameter: the friction coefficient ``K``.
 Also, the argument `nint_u` explicitly adds one integrating state at the model input, the
@@ -237,7 +237,8 @@ savefig("plot6_NonLinMPC.svg"); nothing # hide
 
 ![plot6_NonLinMPC](plot6_NonLinMPC.svg)
 
-the new controller is able to recuperate more energy from the pendulum (i.e. negative work):
+the new controller is able to recuperate a little more energy from the pendulum (i.e.
+negative work):
 
 ```@example 1
 Dict(:W_nmpc => calcW(res_yd), :W_empc => calcW(res2_yd))
@@ -262,12 +263,13 @@ linmodel = linearize(model, x=[π, 0], u=[0])
 A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmodel`:
 
 ```@example 1
+
 skf = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
 mpc = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf)
 mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
 ```
 
-The linear controller has difficulties to reject the 10° step disturbance:
+The linear controller satisfactorily rejects the 10° step disturbance:
 
 ```@example 1
 res_lin = sim!(mpc, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
@@ -278,9 +280,10 @@ savefig("plot7_NonLinMPC.svg"); nothing # hide
 ![plot7_NonLinMPC](plot7_NonLinMPC.svg)
 
 Solving the optimization problem of `mpc` with [`DAQP`](https://darnstrom.github.io/daqp/)
-optimizer instead of the default `OSQP` solver can help here. It is indeed documented that
-`DAQP` can perform better on small/medium dense matrices and unstable poles[^1], which is
-obviously the case here (absolute value of unstable poles are greater than one):
+optimizer instead of the default `OSQP` solver can improve the performance here. It is
+indeed documented that `DAQP` can perform better on small/medium dense matrices and unstable
+poles[^1], which is obviously the case here (absolute value of unstable poles are greater
+than one):
 
 [^1]: Arnström, D., Bemporad, A., and Axehill, D. (2022). A dual active-set solver for
     embedded quadratic programming using recursive LDLᵀ updates. IEEE Trans. Autom. Contr.,
@@ -305,7 +308,7 @@ mpc2 = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp)
 mpc2 = setconstraint!(mpc2; umin, umax)
 ```
 
-does improve the rejection of the step disturbance:
+does slightly improve the rejection of the step disturbance:
 
 ```@example 1
 res_lin2 = sim!(mpc2, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
@@ -359,12 +362,13 @@ function sim_adapt!(mpc, nonlinmodel, N, ry, plant, x_0, x̂_0, y_step=[0])
     x̂ = x̂_0
     for i = 1:N
         y = plant() + y_step
+        x̂ = preparestate!(mpc, y)
         u = moveinput!(mpc, ry)
         linmodel = linearize(nonlinmodel; u, x=x̂[1:2])
         setmodel!(mpc, linmodel)
         U_data[:,i], Y_data[:,i], Ry_data[:,i] = u, y, ry
-        x̂ = updatestate!(mpc, u, y) # update mpc state estimate
-        updatestate!(plant, u)      # update plant simulator
+        updatestate!(mpc, u, y) # update mpc state estimate
+        updatestate!(plant, u)  # update plant simulator
     end
     res = SimResult(mpc, U_data, Y_data; Ry_data)
     return res

docs/src/public/generic_func.md

@@ -19,6 +19,12 @@ setconstraint!
 evaloutput
 ```
 
+## Prepare State x
+
+```@docs
+preparestate!
+```
+
 ## Update State x
 
 ```@docs

docs/src/public/state_estim.md

@@ -17,19 +17,15 @@ error with closed-loop control (offset-free tracking).
     integral action is not necessarily desired. The options `nint_u=0` and `nint_ym=0`
     disable it.
 
-The estimators are all implemented in the predictor form (a.k.a. observer form), that is,
-they all estimates at each discrete time ``k`` the states of the next period
-``\mathbf{x̂}_k(k+1)``[^1]. In contrast, the filter form that estimates ``\mathbf{x̂}_k(k)``
-is sometimes slightly more accurate.
-
-[^1]: also denoted ``\mathbf{x̂}_{k+1|k}`` [elsewhere](https://en.wikipedia.org/wiki/Kalman_filter).
-
-The predictor form comes in handy for control applications since the estimations come after
-the controller computations, without introducing any additional delays. Moreover, the
-[`moveinput!`](@ref) method of the predictive controllers does not automatically update the
-estimates with [`updatestate!`](@ref). This allows applying the calculated inputs on the
-real plant before starting the potentially expensive estimator computations (see
-[Manual](@ref man_lin) for examples).
+The estimators are all implemented in the current form (a.k.a. as filter form) by default
+to improve accuracy and robustness, that is, they all estimates at each discrete time ``k``
+the states of the current period ``\mathbf{x̂}_k(k)``[^1] (using the newest measurements, see
+[Manual](@ref man_lin) for examples). The predictor form (a.k.a. delayed form) is also
+available with the option `direct=false`. This allow moving the estimator computations after
+solving the MPC problem with [`moveinput!`](@ref), for when the estimations are expensive
+(for instance, with the [`MovingHorizonEstimator`](@ref)).
+
+[^1]: also denoted ``\mathbf{x̂}_{k|k}`` [elsewhere](https://en.wikipedia.org/wiki/Kalman_filter).
 
 !!! info
     All the estimators support measured ``\mathbf{y^m}`` and unmeasured ``\mathbf{y^u}``

src/ModelPredictiveControl.jl

@@ -25,7 +25,7 @@ import OSQP, Ipopt
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta
 export setop!, setname!
-export setstate!, setmodel!, updatestate!, evaloutput, linearize, linearize!
+export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize!
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export MovingHorizonEstimator

src/controller/execute.jl

@@ -36,8 +36,6 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).
    in the future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``.
 - `R̂u=mpc.Uop` or *`Rhatu`* : predicted manipulated input setpoints, constant in the future 
    by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for ``j=0`` to ``H_p-1``. 
-- `ym=nothing` : current measured outputs ``\mathbf{y^m}(k)``, only required if `mpc.estim` 
-   is an [`InternalModel`](@ref).
 
 # Examples
 ```jldoctest
@@ -55,13 +53,12 @@ function moveinput!(
     Dhat ::Vector = repeat(d,  mpc.Hp),
     Rhaty::Vector = repeat(ry, mpc.Hp),
     Rhatu::Vector = mpc.Uop,
-    ym::Union{Vector, Nothing} = nothing,
     D̂  = Dhat,
     R̂y = Rhaty,
     R̂u = Rhatu
 )
     validate_args(mpc, ry, d, D̂, R̂y, R̂u)
-    initpred!(mpc, mpc.estim.model, d, ym, 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)
@@ -122,7 +119,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     U0 = mpc.S̃*mpc.ΔŨ + mpc.T_lastu0
     J  = obj_nonlinprog!(U0, Ȳ, Ū, mpc, model, Ŷ0, mpc.ΔŨ)
     oldF = copy(mpc.F)
-    predictstoch!(mpc, mpc.estim, mpc.d0 + model.dop, mpc.ŷ[mpc.estim.i_ym]) 
+    predictstoch!(mpc, mpc.estim) 
     Ŷs .= mpc.F # predictstoch! init mpc.F with Ŷs value if estim is an InternalModel
     mpc.F .= oldF  # restore old F value
     info[:ΔU]   = mpc.ΔŨ[1:mpc.Hc*model.nu]
@@ -164,7 +161,7 @@ end
 
 
 @doc raw"""
-    initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u) -> nothing
+    initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing
 
 Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`.
 
@@ -183,11 +180,11 @@ They are computed with these equations using in-place operations:
 \end{aligned}
 ```
 """
-function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u)
+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, ym, d)
-    predictstoch!(mpc, mpc.estim, d, ym) # init mpc.F with Ŷs for InternalModel
+    ŷ .= evalŷ(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) 
     mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)
@@ -216,14 +213,14 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
 end
 
 @doc raw"""
-    initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y, R̂u)
+    initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
 
 Init `ŷ, F, d0, D̂0, D̂E, R̂y0, R̂u0` vectors when model is not a [`LinModel`](@ref).
 """
-function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y, R̂u)
+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, ym, d)
-    predictstoch!(mpc, mpc.estim, d, ym) # init mpc.F with Ŷs for InternalModel
+    mpc.ŷ .= evalŷ(mpc.estim, d)
+    predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
@@ -238,28 +235,23 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y,
 end
 
 @doc raw"""
-    predictstoch!(mpc::PredictiveController, estim::InternalModel, x̂s, d, ym)
+    predictstoch!(mpc::PredictiveController, estim::InternalModel)
 
 Init `mpc.F` vector with ``\mathbf{F = Ŷ_s}`` when `estim` is an [`InternalModel`](@ref).
 """
-function predictstoch!(
-    mpc::PredictiveController{NT}, estim::InternalModel, d, ym
-) where {NT<:Real}
-    isnothing(ym) && error("Predictive controllers with InternalModel need the measured "*
-                           "outputs ym in keyword argument to compute control actions u")
+function predictstoch!(mpc::PredictiveController{NT}, estim::InternalModel) where {NT<:Real}
     F, ny = mpc.F, estim.model.ny
-    ŷd = similar(estim.model.yop)
-    h!(ŷd, estim.model, estim.x̂d, d - estim.model.dop)
-    ŷd .+= estim.model.yop 
-    ŷs = zeros(NT, estim.model.ny)
-    ŷs[estim.i_ym] .= @views ym .- ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
+    ŷ0d = similar(estim.model.yop)
+    h!(ŷ0d, estim.model, estim.x̂d, estim.d0)
+    ŷs = zeros(NT, ny)
+    ŷs[estim.i_ym] .= @views estim.y0m .- ŷ0d[estim.i_ym]  # ŷs=0 for unmeasured outputs
     Ŷs = F
     mul!(Ŷs, mpc.Ks, estim.x̂s)
     mul!(Ŷs, mpc.Ps, ŷs, 1, 1)
     return nothing
 end
 "Separate stochastic predictions are not needed if `estim` is not [`InternalModel`](@ref)."
-predictstoch!(mpc::PredictiveController, ::StateEstimator, _ , _ ) = (mpc.F .= 0; nothing)
+predictstoch!(mpc::PredictiveController, ::StateEstimator) = (mpc.F .= 0; nothing)
 
 @doc raw"""
     linconstraint!(mpc::PredictiveController, model::LinModel)
@@ -505,7 +497,16 @@ end
 set_objective_linear_coef!(::PredictiveController, _ ) = nothing
 
 """
-    updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂
+    preparestate!(mpc::PredictiveController, ym, d=[]) -> x̂
+
+Call [`preparestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
+"""
+function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty)
+    return preparestate!(mpc.estim, ym, d)
+end
+
+"""
+    updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂next
 
 Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
@@ -523,7 +524,7 @@ setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return m
 
 
 @doc raw"""
-    setmodel!(mpc::PredictiveController, model=mpc.estim.model, <keyword arguments>) -> mpc
+    setmodel!(mpc::PredictiveController, model=mpc.estim.model; <keyword arguments>) -> mpc
 
 Set `model` and objective function weights of `mpc` [`PredictiveController`](@ref).