added: MovingHorizonEstimator support for direct=true

src/controller/construct.jl

@@ -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}``,
@@ -454,7 +454,8 @@ terminal states at ``k+H_p``:
 \end{aligned}
 ```
 The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at each control period
-``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref).
+``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). The Extended Help provides
+details on the matrices ``\mathbf{E, G, J, K, V, B, e_x̂, g_x̂, j_x̂, k_x̂, v_x̂, b_x̂}``.
 
 # 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̂)

src/controller/execute.jl

@@ -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/estimator/internal_model.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

src/estimator/kalman.jl

@@ -116,7 +116,7 @@ unmeasured ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
 - `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured
     disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
 - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current
-   estimator, in opposition to the delayed/prediction form).
+   estimator, in opposition to the delayed/predictor form).
 
 # Examples
 ```jldoctest
@@ -331,11 +331,10 @@ end
 
 Construct a time-varying Kalman Filter with the [`LinModel`](@ref) `model`.
 
-The process model is identical to [`SteadyKalmanFilter`](@ref). The matrix 
-``\mathbf{P̂}_k(k+1)`` is the estimation error covariance of `model` states augmented with
-the stochastic ones (specified by `nint_u` and `nint_ym`). Three keyword arguments modify
-its initial value with ``\mathbf{P̂}_{-1}(0) = 
-    \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``.
+The process model is identical to [`SteadyKalmanFilter`](@ref). The matrix ``\mathbf{P̂}_k``
+is the estimation error covariance of `model` states augmented with the stochastic ones
+(specified by `nint_u` and `nint_ym`). Three keyword arguments modify its initial value with
+``\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``.
 
 # Arguments
 !!! info