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

src/estimator/construct.jl

@@ -7,6 +7,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     x̂d::Vector{NT}
     x̂s::Vector{NT}
     ŷs::Vector{NT}
+    x̂snext::Vector{NT}
     i_ym::Vector{Int}
     nx̂::Int
     nym::Int
@@ -43,14 +44,14 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         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 = zeros(NT, nxs)
+        x̂s, x̂snext = zeros(NT, nxs), zeros(NT, nxs)
         ŷs = zeros(NT, ny)
         direct = true # InternalModel always uses direct transmission from ym
         corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
-            lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs,
+            lastu0, 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,
@@ -247,9 +248,14 @@ function correct_estimate!(estim::InternalModel, y0m, d0)
     ŷ0d = estim.buffer.ŷ
     h!(ŷ0d, estim.model, estim.x̂d, d0)
     ŷs = estim.ŷs
-    ŷs[estim.i_ym] .= @views y0m .- ŷ0d[estim.i_ym]
-    # ŷs=0 for unmeasured outputs :
-    map(i -> ŷs[i] = (i in estim.i_ym) ? ŷs[i] : 0, eachindex(ŷs)) 
+    for j in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop
+        if j in estim.i_ym
+            i = estim.i_ym[j]
+            ŷs[j] = y0m[i] - ŷ0d[j]
+        else
+            ŷs[j] = 0
+        end
+    end
     return nothing
 end
 
@@ -276,7 +282,7 @@ function update_estimate!(estim::InternalModel, _ , d0, u0)
     f!(x̂dnext, model, x̂d, u0, d0) 
     x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object)
     # --------------- stochastic model -----------------------
-    x̂snext = similar(x̂s) # TODO: remove this allocation with a new buffer?
+    x̂snext = estim.x̂snext
     mul!(x̂snext, estim.Âs, x̂s)
     mul!(x̂snext, estim.B̂s, ŷs, 1, 1)
     estim.x̂s .= x̂snext