added: MHE update cov with KF if LinModel

Author: franckgaga

src/estimator/kalman.jl

@@ -333,7 +333,7 @@ control period ``k-1``. See [^2] for details.
      Linear Dynamical Systems*, https://web.stanford.edu/class/ee363/lectures/kf.pdf.
 """
 function update_estimate!(estim::KalmanFilter, u, ym, d)
-    return update_estimate_kf!(estim, estim.Â, estim.Ĉm, u, ym, d)
+    return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉm, estim.P̂, estim.x̂)
 end
 
 struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
@@ -736,7 +736,7 @@ function update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=empty(estim.x̂)
     F̂  = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
     Ĥ  = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
     Ĥm = Ĥ[estim.i_ym, :] 
-    return update_estimate_kf!(estim, F̂, Ĥm, u, ym, d)
+    return update_estimate_kf!(estim, u, ym, d, F̂, Ĥm, estim.P̂, estim.x̂)
 end
 
 "Set `estim.P̂` to `estim.P̂0` for the time-varying Kalman Filters."
@@ -766,25 +766,25 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_estimate_kf!(estim, Â, Ĉm, u, ym, d; updatestate=true)
+    update_estimate_kf!(estim, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
 Allows code reuse for [`KalmanFilter`](@ref) and [`ExtendedKalmanFilterKalmanFilter`](@ref).
 They update the state `x̂` and covariance `P̂` with the same equations. The extended filter
 substitutes the augmented model matrices with its Jacobians (`Â = F̂` and `Ĉm = Ĥm`).
 The implementation uses in-place operations and explicit factorization to reduce
-allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `updatestate`
-is `false`, only the covariance `P̂` is updated.
+allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `isnothing(x̂)`,
+only the covariance `P̂` is updated.
 """
-function update_estimate_kf!(estim, Â, Ĉm, u, ym, d; updatestate=true)
-    x̂, P̂, Q̂, R̂, K̂, M̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂
+function update_estimate_kf!(estim, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
+    Q̂, R̂, M̂ = estim.Q̂, estim.R̂, estim.M̂
     mul!(M̂, P̂, Ĉm')
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' + R̂)))
-    if updatestate
-        mul!(K̂, Â, M̂)
+    if !isnothing(x̂)
+        mul!(estim.K̂, Â, M̂)
         ŷm = @views ĥ(estim, estim.model, x̂, d)[estim.i_ym]
-        x̂[:] = f̂(estim, estim.model, x̂, u, d) +  K̂ * (ym - ŷm)
+        x̂[:] = f̂(estim, estim.model, x̂, u, d) +  estim.K̂ * (ym - ŷm)
     end
     P̂.data[:] = Â * (P̂ - M̂ * Ĉm * P̂) * Â' + Q̂ # .data is necessary for Hermitians
     return nothing

src/estimator/mhe.jl

@@ -12,7 +12,6 @@ struct MovingHorizonEstimator{
     W̃::Vector{NT}
     lastu0::Vector{NT}
     x̂::Vector{NT}
-    P̂::Hermitian{NT, Matrix{NT}}
     He::Int
     i_ym::Vector{Int}
     nx̂ ::Int
@@ -24,18 +23,17 @@ struct MovingHorizonEstimator{
     Cs_y::Matrix{NT}
     nint_u ::Vector{Int}
     nint_ym::Vector{Int}
-    Â ::Matrix{NT}
-    B̂u::Matrix{NT}
-    Ĉ ::Matrix{NT}
-    B̂d::Matrix{NT}
-    D̂d::Matrix{NT}
+    Â   ::Matrix{NT}
+    B̂u  ::Matrix{NT}
+    Ĉ   ::Matrix{NT}
+    B̂d  ::Matrix{NT}
+    D̂d  ::Matrix{NT}
     P̂0::Hermitian{NT, Matrix{NT}}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
     invP̄::Hermitian{NT, Matrix{NT}}
     invQ̂_He::Hermitian{NT, Matrix{NT}}
     invR̂_He::Hermitian{NT, Matrix{NT}}
-    K̂::Matrix{NT}
     M̂::Matrix{NT}
     X̂min::Vector{NT}
     X̂max::Vector{NT}
@@ -46,6 +44,7 @@ struct MovingHorizonEstimator{
     D ::Union{Vector{NT}, Missing}
     Ŵ ::Union{Vector{NT}, Missing}
     x̂0_past::Vector{NT}
+    P̂0_past::Hermitian{NT, Matrix{NT}}
     Nk::Vector{Int}
     function MovingHorizonEstimator{NT, SM, JM}(
         model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM
@@ -65,8 +64,7 @@ struct MovingHorizonEstimator{
         invP̄ = Hermitian(inv(P̂0), :L)
         invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L)
         invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L)
-        P̂ = copy(P̂0)
-        K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym)
+        M̂ = zeros(NT, nx̂, nym)
         nvar, nX̂ = nx̂*(He + 1), nx̂*(He + 1)
         X̂min  , X̂max    =   fill(-Inf, nX̂), fill(+Inf, nX̂)
         i_X̂min, i_X̂max  = .!isinf.(X̂min)  , .!isinf.(X̂max)
@@ -75,18 +73,19 @@ struct MovingHorizonEstimator{
         X̂, Ym   = zeros(NT, nx̂*He), zeros(NT, nym*He)
         U, D, Ŵ = zeros(NT, nu*He), zeros(NT, nd*He), zeros(NT, nx̂*He)
         x̂0_past = zeros(NT, nx̂)
+        P̂0_past = copy(P̂0)
         Nk = [0]
         estim = new{NT, SM, JM}(
             model, optim, W̃,
-            lastu0, x̂, P̂, He,
+            lastu0, x̂, He,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, Ĉ, B̂d, D̂d,
             P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He,
-            K̂, M̂,
+            M̂,
             X̂min, X̂max, i_g,
             X̂, Ym, U, D, Ŵ, 
-            x̂0_past, Nk
+            x̂0_past, P̂0_past, Nk
         )
         init_optimization!(estim, optim)
         return estim
@@ -123,18 +122,13 @@ and the covariances are repeated ``N_k`` times:
     \mathbf{R̂}_{N_k} &= \text{diag}\mathbf{(R̂,R̂,...,R̂)} 
 \end{aligned}
 ```
-The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` incorporate the estimated process noise
+The estimation horizon ``H_e`` limits the window length ``N_k = \min(k+1, H_e)``. The 
+vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` encompass the estimated process noise
 ``\mathbf{ŵ}(k-j)`` and sensor noise ``\mathbf{v̂}(k-j)`` from ``j=0`` to ``N_k-1``. The 
-estimation horizon ``H_e`` limits the window length:
-```math
-N_k =                     \begin{cases} 
-    k + 1   &  k < H_e    \\
-    H_e     &  k ≥ H_e    \end{cases}
-```
-See [`SteadyKalmanFilter`](@ref) for details on ``\mathbf{R̂}, \mathbf{Q̂}`` covariances and
-model augmentation. The process model is identical to the one in [`UnscentedKalmanFilter`](@ref)
-documentation. Note that the estimation error covariance ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is
-approximated with an [`ExtendedKalmanFilter`](@ref).
+latter is generated by the augmented model ``\mathbf{f̂, ĥ}``. See [`SteadyKalmanFilter`](@ref)
+for details on ``\mathbf{R̂}, \mathbf{Q̂}`` covariances and model augmentation. The process
+model is identical to the one in [`UnscentedKalmanFilter`](@ref) documentation. The matrix
+``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFilter`](@ref).
 
 !!! warning
     See the Extended Help of [`NonLinMPC`](@ref) function if you get an error like:    
@@ -394,16 +388,16 @@ end
 
 "Reset `estim.P̂`, `estim.invP̄` and the time windows for the moving horizon estimator."
 function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) 
-    estim.invP̄.data[:] = Hermitian(inv(estim.P̂0), :L)
-    estim.P̂.data[:]    = estim.P̂0
-    estim.x̂0_past     .= 0
-    estim.W̃           .= 0
-    estim.X̂           .= 0
-    estim.Ym          .= 0
-    estim.U           .= 0
-    estim.D           .= 0
-    estim.Ŵ           .= 0
-    estim.Nk          .= 0
+    estim.invP̄.data[:]      = Hermitian(inv(estim.P̂0), :L)
+    estim.P̂0_past.data[:]   = estim.P̂0
+    estim.x̂0_past           .= 0
+    estim.W̃                 .= 0
+    estim.X̂                 .= 0
+    estim.Ym                .= 0
+    estim.U                 .= 0
+    estim.D                 .= 0
+    estim.Ŵ                 .= 0
+    estim.Nk                .= 0
     return nothing
 end
 
@@ -412,7 +406,51 @@ end
     
 Update [`MovingHorizonEstimator`](@ref) state `estim.x̂`.
 
-TBW
+Denoting the estimated process noises augmented with the state at arrival ``\mathbf{W̃} = 
+[\begin{smallmatrix} \mathbf{x̂}_k(k-N_k+1) \\ \mathbf{Ŵ} \end{smallmatrix}]``, the following
+optimization problem is solved at each discrete time ``k``:
+```math
+\min_{\mathbf{W̃}}   \mathbf{x̄}' \mathbf{P̄}^{-1}       \mathbf{x̄} 
+                  + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ}  
+                  + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂}
+```
+in which:
+```math
+\begin{aligned}
+    N_k                 &= \min(k+1, H_e)                                       \\
+    \mathbf{x̄}          &= \mathbf{x̂}_k(k-N_k+1) - \mathbf{x̂}_{k-N_k}(k-N_k+1)  \\
+    \mathbf{P̄}          &= \mathbf{P̂}_{k-N_k}(k-N_k+1)                          \\
+    \mathbf{Q̂}_{N_k}    &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)}                      \\
+    \mathbf{R̂}_{N_k}    &= \text{diag}\mathbf{(R̂,R̂,...,R̂)}                      \\
+\end{aligned}
+```
+and also:
+```math
+\mathbf{Ŵ} = 
+\begin{bmatrix}
+    \mathbf{ŵ}(k-N_k+1)     \\[6px]
+    \mathbf{ŵ}(k-N_k+2)     \\[6px]
+    \vdots                  \\[6px]
+    \mathbf{ŵ}(k)
+\end{bmatrix} , \quad
+\mathbf{V̂} =
+\begin{bmatrix}
+    \mathbf{y^m} - \mathbf{ĥ^m}\Big(\mathbf{x̂}_k(k-N_k+1), \mathbf{d}(k-N_k+1)\Big)  \\
+    \mathbf{y^m} - \mathbf{ĥ^m}\Big(\mathbf{x̂}_k(k-N_k+2), \mathbf{d}(k-N_k+2)\Big)  \\
+    \vdots                                                                           \\
+    \mathbf{y^m} - \mathbf{ĥ^m}\Big(\mathbf{x̂}_k(k), \mathbf{d}(k)\Big)
+\end{bmatrix}
+```
+The augmented model and the additive process noise recursively generates the states:
+```math
+\mathbf{x̂}_k(k-j+1) = \mathbf{f̂}\Big(\mathbf{x̂}_k(k-j), \mathbf{u}(k-j), \mathbf{d}(k-j)\Big) 
+                       + \mathbf{ŵ}(k-j)
+```
+Once the problem solved, the next estimate ``\mathbf{x̂}_k(k+1)`` is computed by inserting
+the optimal values of ``\mathbf{x̂}_k(k-N_k+1)`` and ``\mathbf{Ŵ}`` in the last equation from
+``j=N_k-1`` to ``0``, inclusively. Afterward, if ``k ≥ H_e``, the arrival covariance for the
+next time step ``\mathbf{P̂}_{k-N_k+1}(k-N_k+2)`` is estimated with the equations of
+[`update_estimate!(::ExtendedKalmanFilter)`](@ref) (or `KalmanFilter`, for [`LinModel`](@ref)).
 """
 function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real
     model, optim, x̂ = estim.model, estim.optim, estim.x̂
@@ -477,18 +515,27 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
     Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, estim.W̃)
     x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))]
     if Nk == He
-        # update the arrival covariance with an Extended Kalman Filter:
-        # TODO: also support UnscentedKalmanFilter, and KalmanFilter for LinModel
-        F̂  = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
-        Ĥ  = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
-        Ĥm = Ĥ[estim.i_ym, :] 
-        update_estimate_kf!(estim, F̂, Ĥm, u, ym, d, updatestate=false)
-        P̄ = estim.P̂
-        estim.invP̄.data[:] = Hermitian(inv(P̄), :L) # .data is necessary for Hermitian
+        P̂0_past = update_cov!(estim, model, u, ym, d)
+        estim.invP̄.data[:] = Hermitian(inv(P̂0_past), :L)
     end
     return nothing
 end
 
+"Update the covariance `estim.P̂0_past` with the `KalmanFilter` if `model` is a `LinModel`."
+function update_cov!(estim::MovingHorizonEstimator, ::LinModel, u, ym, d) 
+    update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉ[estim.i_ym, :], estim.P̂0_past)
+    return estim.P̂0_past
+end
+"Update it with the `ExtendedKalmanFilter` if model is not a `LinModel`."
+function update_cov!(estim::MovingHorizonEstimator, ::SimModel, u, ym, d) 
+    # TODO: also support UnscentedKalmanFilter
+    F̂  = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
+    Ĥ  = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
+    Ĥm = Ĥ[estim.i_ym, :] 
+    update_estimate_kf!(estim, u, ym, d, F̂, Ĥm, estim.P̂0_past)
+    return estim.P̂0_past
+end
+
 """
     obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, W̃)
 

src/state_estim.jl

@@ -442,8 +442,8 @@ end
 
 include("estimator/kalman.jl")
 include("estimator/luenberger.jl")
-include("estimator/internal_model.jl")
 include("estimator/mhe.jl")
+include("estimator/internal_model.jl")
 
 """
     evalŷ(estim::StateEstimator, _ , d) -> ŷ