Merge pull request #96 from JuliaControl/mhe_direct_initial_posteriori

 added: MovingHorizonEstimator support for `direct=true`, initialized with `P̂(-1|-1)`

Author: franckgaga

Project.toml

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

docs/src/assets/plot_controller.svg

@@ -33,5 +33,5 @@ ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
 ## Solve Optimization Problem
 
 ```@docs
-ModelPredictiveControl.optim_objective!
+ModelPredictiveControl.optim_objective!(::PredictiveController)
 ```

docs/src/assets/plot_estimator.svg

@@ -34,6 +34,12 @@ ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel)
 ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 ```
 
+## Solve Optimization Problem
+
+```@docs
+ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
+```
+
 ## Evaluate Estimated Output
 
 ```@docs

docs/src/internals/predictive_control.md

@@ -203,7 +203,7 @@ mpc_mhe = LinMPC(estim, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])
 mpc_mhe = setconstraint!(mpc_mhe, ymin=[45, -Inf])
 ```
 
-The rejection is not improved here:
+The rejection is indeed improved:
 
 ```@example 1
 setstate!(model, zeros(model.nx))
@@ -216,10 +216,6 @@ savefig("plot3_LinMPC.svg"); nothing # hide
 
 ![plot3_LinMPC](plot3_LinMPC.svg)
 
-This is because the more performant `direct=true` version of the [`MovingHorizonEstimator`](@ref)
-is not not implemented yet. The rejection will be improved with the `direct=true` version
-(coming soon).
-
 ## Adding Feedforward Compensation
 
 Suppose that the load disturbance ``u_l`` of the last section is in fact caused by a

docs/src/internals/state_estim.md

@@ -44,7 +44,7 @@ The predictive controllers support both soft and hard constraints, defined by:
     \mathbf{u_{min}  - c_{u_{min}}}  ϵ ≤&&\       \mathbf{u}(k+j) &≤ \mathbf{u_{max}  + c_{u_{max}}}  ϵ &&\qquad  j = 0, 1 ,..., H_p - 1 \\
     \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\      \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad  j = 0, 1 ,..., H_c - 1 \\
     \mathbf{y_{min}  - c_{y_{min}}}  ϵ ≤&&\       \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max}  + c_{y_{max}}}  ϵ &&\qquad  j = 1, 2 ,..., H_p     \\
-    \mathbf{x̂_{min}  - c_{x̂_{min}}}  ϵ ≤&&\ \mathbf{x̂}_{i}(k+j) &≤ \mathbf{x̂_{max}  + c_{x̂_{max}}}  ϵ &&\qquad  j = H_p
+    \mathbf{x̂_{min}  - c_{x̂_{min}}}  ϵ ≤&&\     \mathbf{x̂}_i(k+j) &≤ \mathbf{x̂_{max}  + c_{x̂_{max}}}  ϵ &&\qquad  j = H_p
 \end{alignat*}
 ```
 and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the
@@ -435,7 +435,7 @@ The model predictions are evaluated from the deviation vectors (see [`setop!`](@
                  &= \mathbf{E ΔU} + \mathbf{F}
 \end{aligned}
 ```
-in which ``\mathbf{x̂_0}(k) = \mathbf{x̂}_{i}(k) - \mathbf{x̂_{op}}``, with ``i = k`` if 
+in which ``\mathbf{x̂_0}(k) = \mathbf{x̂}_i(k) - \mathbf{x̂_{op}}``, with ``i = k`` if 
 `estim.direct==true`, otherwise ``i = k - 1``. The predicted outputs ``\mathbf{Ŷ_0}`` and
 measured disturbances ``\mathbf{D̂_0}`` respectively include ``\mathbf{ŷ_0}(k+j)`` and 
 ``\mathbf{d̂_0}(k+j)`` values with ``j=1`` to ``H_p``, and input increments ``\mathbf{ΔU}``,
@@ -453,8 +453,9 @@ terminal states at ``k+H_p``:
                         &= \mathbf{e_x̂ ΔU} + \mathbf{f_x̂}
 \end{aligned}
 ```
-The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at each control period
-``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref).
+The matrices ``\mathbf{E, G, J, K, V, B, e_x̂, g_x̂, j_x̂, k_x̂, v_x̂, b_x̂}`` are defined in the
+Extended Help section. The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are  recalculated at
+each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref).
 
 # Extended Help
 !!! details "Extended Help"
@@ -529,7 +530,7 @@ function init_predmat(estim::StateEstimator{NT}, model::LinModel, Hp, Hc) where
     # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ...
     Âpow_csum  = cumsum(Âpow, dims=3)
     # helper function to improve code clarity and be similar to eqs. in docstring:
-    getpower(array3D, power) = array3D[:,:, power+1]
+    getpower(array3D, power) = @views array3D[:,:, power+1]
     # --- state estimates x̂ ---
     kx̂ = getpower(Âpow, Hp)
     K  = Matrix{NT}(undef, Hp*ny, nx̂)

docs/src/manual/linmpc.md

@@ -17,7 +17,8 @@ Solve the optimization problem of `mpc` [`PredictiveController`](@ref) and retur
 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
-[`updatestate!(mpc, u, ym, d)`](@ref) to update `mpc` state estimates.
+[`preparestate!(mpc, ym, d)`](@ref) before `moveinput!`, and [`updatestate!(mpc, u, ym, d)`](@ref)
+after, to update `mpc` state estimates.
 
 Calling a [`PredictiveController`](@ref) object calls this method.
 

src/controller/construct.jl

@@ -197,8 +197,9 @@ end
 
 One integrator on each measured output by default if `model` is not a  [`LinModel`](@ref).
 
-If the integrator quantity per manipulated input `nint_u ≠ 0`, the method returns zero
-integrator on each measured output.
+Theres is no verification the augmented model remains observable. If the integrator quantity
+per manipulated input `nint_u ≠ 0`, the method returns zero integrator on each measured
+output.
 """
 function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
     validate_ym(model, i_ym)

src/controller/execute.jl

@@ -86,7 +86,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̂}(0)``. It
+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):
 
 - If `estim.model` is a [`LinModel`](@ref), it finds the steady-state of the augmented model
@@ -203,7 +203,7 @@ Prepare `estim.x̂0` estimate with meas. outputs `ym` and dist. `d` for the curr
 
 This function should be called at the beginning of each discrete time step. Its behavior
 depends if `estim` is a [`StateEstimator`](@ref) in the current/filter (1.) or 
-delayed/predictor (2.) form:
+delayed/predictor (2.) formulation:
 
 1. If `estim.direct` is `true`, it removes the operating points with [`remove_op!`](@ref),
    calls [`correct_estimate!`](@ref), and returns the corrected state estimate 
@@ -246,7 +246,9 @@ This function should be called at the end of each discrete time step. It removes
 operating points with [`remove_op!`](@ref), calls [`update_estimate!`](@ref) and returns the
 state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`preparestate!`](@ref)
 should be called prior to this one to correct the estimate when applicable (if
-`estim.direct == true`). 
+`estim.direct == true`). Note that the [`MovingHorizonEstimator`](@ref) with the default
+`direct=true` option is not able to estimate ``\mathbf{x̂}_k(k+1)``, the returned value
+is therefore the current corrected state ``\mathbf{x̂}_k(k)``.
 
 # Examples
 ```jldoctest