Merge pull request #41 from JuliaControl/mhe_covestim

franckgaga · web-flow · commit d19acc4ee0ef · 2024-03-28T13:34:46.000-04:00
Added: MHE supports custom estimator for the arrival covariance
diff --git a/Project.toml b/Project.toml
@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.19.2"
+version = "0.20.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md
@@ -79,7 +79,7 @@ savefig(ans, "plot1_NonLinMPC.svg"); nothing # hide
 An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
 
 ```@example 1
-α=0.1; σQ=[0.1, 0.5]; σR=[0.5]; nint_u=[1]; σQint_u=[0.1]
+α=0.01; σQ=[0.1, 0.5]; σR=[0.5]; nint_u=[1]; σQint_u=[0.1]
 estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
 ```
 
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -163,7 +163,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{Q̂, R̂}``.
 """
 function SteadyKalmanFilter(model::SM, i_ym, nint_u, nint_ym, Q̂, R̂) where {NT<:Real, SM<:LinModel{NT}}
     Q̂, R̂ = to_mat(Q̂), to_mat(R̂)
-    return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂ , R̂)
+    return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂)
 end
 
 
@@ -315,7 +315,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat
 """
 function KalmanFilter(model::SM, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂) where {NT<:Real, SM<:LinModel{NT}}
     P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
-    return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂)
+    return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
 end
 
 @doc raw"""
@@ -428,7 +428,7 @@ represents the measured outputs of ``\mathbf{ĥ}`` function (and unmeasured one
 
 # Arguments
 - `model::SimModel` : (deterministic) model for the estimations.
-- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 ≤ α ≤ 1)``.
+- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``.
 - `β=2` : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``.
 - `κ=0` : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``.
 - `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
@@ -478,14 +478,14 @@ function UnscentedKalmanFilter(
 end
 
 @doc raw"""
-    UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α, β, κ)
+    UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α=1e-3, β=2, κ=0)
 
 Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and `R̂`.
 
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
 """
 function UnscentedKalmanFilter(
-    model::SM, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α, β, κ
+    model::SM, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α=1e-3, β=2, κ=0
 ) where {NT<:Real, SM<:SimModel{NT}}
     P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
     return UnscentedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, α, β, κ)
@@ -565,22 +565,7 @@ noise, respectively.
      ISBN9780470045343.
 """
 function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:Real
-    return update_estimate_ukf!(estim, u, ym, d, estim.P̂, estim.x̂)
-end
-
-"""
-    update_estimate_ukf!(estim::StateEstimator, u, ym, d, P̂, x̂=nothing)
-
-Update Unscented Kalman Filter estimates and covariance matrices.
-
-Allows code reuse for [`UnscentedKalmanFilter`](@ref) and [`MovingHorizonEstimator`](@ref).
-See  [`update_estimate!(::UnscentedKalmanFilter, ::Any, ::Any, ::Any)`(@ref) docstring
-for the equations. If `isnothing(x̂)`, only the covariance `P̂` is updated.
-"""
-function update_estimate_ukf!(
-    estim::StateEstimator{NT}, u, ym, d, P̂, x̂=nothing
-) where NT<:Real
-    Q̂, R̂, K̂ = estim.Q̂, estim.R̂, estim.K̂
+    x̂, P̂, Q̂, R̂, K̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
     # --- initialize matrices ---
@@ -742,7 +727,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat
 """
 function ExtendedKalmanFilter(model::SM, i_ym, nint_u, nint_ym,P̂0, Q̂, R̂) where {NT<:Real, SM<:SimModel{NT}}
     P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
-    return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂)
+    return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
 end
 
 
@@ -759,7 +744,8 @@ substitutions ``\mathbf{Â = F̂}(k)`` and ``\mathbf{Ĉ^m = Ĥ^m}(k)``:
                            [\mathbf{Ĥ^m}(k)\mathbf{P̂}_{k-1}(k)\mathbf{Ĥ^m}'(k) + \mathbf{R̂}]^{-1}    \\
     \mathbf{K̂}(k)       &= \mathbf{F̂}(k) \mathbf{M̂}(k)                                    \\
     \mathbf{ŷ^m}(k)     &= \mathbf{ĥ^m}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{d}(k) \Big)     \\
-    \mathbf{x̂}_{k}(k+1) &= \mathbf{f̂}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{u}(k), \mathbf{d}(k) \Big)
+    \mathbf{x̂}_{k}(k+1) &= \math, covestim::CE,
+    )    bf{f̂}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{u}(k), \mathbf{d}(k) \Big)
                            + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)]             \\
     \mathbf{P̂}_{k}(k+1) &= \mathbf{F̂}(k)[\mathbf{P̂}_{k-1}(k)
                            - \mathbf{M̂}(k)\mathbf{Ĥ^m}(k)\mathbf{P̂}_{k-1}(k)]\mathbf{F̂}'(k) 
@@ -770,7 +756,8 @@ substitutions ``\mathbf{Â = F̂}(k)`` and ``\mathbf{Ĉ^m = Ĥ^m}(k)``:
 automatically computes the Jacobians:
 ```math
 \begin{aligned}
-    \mathbf{F̂}(k) &= \left. \frac{∂\mathbf{f̂}(\mathbf{x̂}, \mathbf{u}, \mathbf{d})}{∂\mathbf{x̂}} \right|_{\mathbf{x̂ = x̂}_{k-1}(k),\, \mathbf{u = u}(k),\, \mathbf{d = d}(k)}  \\
+    \mathbf{F̂}(k) &= \left. \fra, covestim::CE,
+    )    c{∂\mathbf{f̂}(\mathbf{x̂}, \mathbf{u}, \mathbf{d})}{∂\mathbf{x̂}} \right|_{\mathbf{x̂ = x̂}_{k-1}(k),\, \mathbf{u = u}(k),\, \mathbf{d = d}(k)}  \\
     \mathbf{Ĥ}(k) &= \left. \frac{∂\mathbf{ĥ}(\mathbf{x̂}, \mathbf{d})}{∂\mathbf{x̂}}             \right|_{\mathbf{x̂ = x̂}_{k-1}(k),\, \mathbf{d = d}(k)}
 \end{aligned}
 ```
@@ -813,34 +800,29 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
+    update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂)
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
 Allows code reuse for [`KalmanFilter`](@ref), [`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 (`Â = F̂` and `Ĉm = Ĥm`).
 The implementation uses in-place operations and explicit factorization to reduce
-allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `isnothing(x̂)`,
-only the covariance `P̂` is updated.
+allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations.
 """
-function update_estimate_kf!(
-    estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂=nothing
-) where NT<:Real
-    Q̂, R̂, M̂ = estim.Q̂, estim.R̂, estim.M̂
+function update_estimate_kf!(estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂) where NT<:Real
+    Q̂, R̂, M̂, K̂ = estim.Q̂, estim.R̂, estim.M̂, estim.K̂
     mul!(M̂, P̂, Ĉm')
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂)))
-    if !isnothing(x̂)
-        mul!(estim.K̂, Â, M̂)
-        x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, estim.model.ny)
-        ĥ!(ŷ, estim, estim.model, x̂, d)
-        ŷm = @views ŷ[estim.i_ym]
-        v̂  = ŷm
-        v̂ .= ym .- ŷm
-        f̂!(x̂next, estim, estim.model, x̂, u, d)
-        mul!(x̂next, estim.K̂, v̂, 1, 1)
-        estim.x̂ .= x̂next
-    end
+    mul!(K̂, Â, M̂)
+    x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, estim.model.ny)
+    ĥ!(ŷ, estim, estim.model, x̂, d)
+    ŷm = @views ŷ[estim.i_ym]
+    v̂  = ŷm
+    v̂ .= ym .- ŷm
+    f̂!(x̂next, estim, estim.model, x̂, u, d)
+    mul!(x̂next, K̂, v̂, 1, 1)
+    estim.x̂ .= x̂next
     P̂.data .= Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂ # .data is necessary for Hermitians
     return nothing
 end
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -43,14 +43,16 @@ end
 
 struct MovingHorizonEstimator{
     NT<:Real, 
-    SM<:SimModel, 
-    JM<:JuMP.GenericModel
+    SM<:SimModel,
+    JM<:JuMP.GenericModel,
+    CE<:StateEstimator,
 } <: StateEstimator{NT}
     model::SM
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
     # different since solvers that support non-Float64 are scarce.
     optim::JM
     con::EstimatorConstraint{NT}
+    covestim::CE
     Z̃::Vector{NT}
     lastu0::Vector{NT}
     x̂::Vector{NT}
@@ -95,17 +97,16 @@ struct MovingHorizonEstimator{
     x̂arr_old::Vector{NT}
     P̂arr_old::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̂, Cwt, optim::JM
-    ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
+    function MovingHorizonEstimator{NT, SM, JM, CE}(
+        model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim::JM, covestim::CE
+    ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
         nu, nd = model.nu, model.nd
         He < 1  && throw(ArgumentError("Estimation horizon He should be ≥ 1"))
         Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0"))
         nym, nyu = validate_ym(model, i_ym)
         As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
-        nŵ = nx̂
         Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y)
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         lastu0 = zeros(NT, model.nu)
@@ -129,8 +130,8 @@ struct MovingHorizonEstimator{
         x̂arr_old = zeros(NT, nx̂)
         P̂arr_old = copy(P̂0)
         Nk = [0]
-        estim = new{NT, SM, JM}(
-            model, optim, con, 
+        estim = new{NT, SM, JM, CE}(
+            model, optim, con, covestim,  
             Z̃, lastu0, x̂, 
             He,
             i_ym, nx̂, nym, nyu, nxs, 
@@ -186,9 +187,9 @@ N_k =                     \begin{cases}
 ```
 The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` encompass the estimated process noise
 ``\mathbf{ŵ}(k-j)`` and sensor noise ``\mathbf{v̂}(k-j)`` from ``j=N_k-1`` to ``0``. The 
-Extended Help defines the two vectors and the scalar ``ϵ``. See [`UnscentedKalmanFilter`](@ref)
-for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. The
-matrix ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFilter`](@ref).
+Extended Help defines the two vectors, the slack variable ``ϵ``, and the estimation of the
+covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+1)``. See [`UnscentedKalmanFilter`](@ref)
+for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances.
 
 !!! warning
     See the Extended Help if you get an error like:    
@@ -247,22 +248,27 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer
     The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). 
     It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for
     problems with two or more types of bounds, to ensure feasibility (e.g. on the estimated
-    state and sensor noise).
-
-    For [`LinModel`](@ref), the optimization is treated as a quadratic program with a
-    time-varying Hessian, which is generally cheaper than nonlinear programming.
+    state and sensor noise). 
     
-    For [`NonLinModel`](@ref), the optimization relies on automatic differentiation (AD).
-    Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` 
-    functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
-    for common mistakes when writing these functions. 
+    The optimization and the estimation of the covariance at arrival 
+    ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` depend on `model`:
+
+    - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program
+      with a time-varying Hessian, which is generally cheaper than nonlinear programming. By
+      default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable).
+    - Else, a nonlinear program with automatic differentiation (AD) solves the optimization.
+      Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` 
+      functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
+      for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref)
+      estimates the arrival covariance by default.
         
     Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to 
-    `10/Cwt` (if not already set), to scale the small values of ``ϵ``.
+    `10/Cwt` (if not already set), to scale the small values of ``ϵ``. Use the second
+    constructor to specify the covariance estimation method.
 """
 function MovingHorizonEstimator(
     model::SM;
-    He::Union{Int, Nothing}=nothing,
+    He::Union{Int, Nothing} = nothing,
     i_ym::IntRangeOrVector = 1:model.ny,
     σP0::Vector = fill(1/model.nx, model.nx),
     σQ ::Vector = fill(1/model.nx, model.nx),
@@ -280,33 +286,40 @@ function MovingHorizonEstimator(
     P̂0 = Hermitian(diagm(NT[σP0; σP0int_u; σP0int_ym].^2), :L)
     Q̂  = Hermitian(diagm(NT[σQ;  σQint_u;  σQint_ym ].^2), :L)
     R̂  = Hermitian(diagm(NT[σR;].^2), :L)
-    isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified"))        
-    return MovingHorizonEstimator{NT, SM, JM}(
-        model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim
-    )
+    isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) 
+    return MovingHorizonEstimator(model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim)
 end
 
-"Return a `JuMP.Model` with OSQP optimizer if `model` is a [`LinModel`](@ref)."
 default_optim_mhe(::LinModel) = JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false)
-"Else, return it with Ipopt optimizer."
 default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false)
 
 @doc raw"""
-    MovingHorizonEstimator(model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim)
+    MovingHorizonEstimator(model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim[, covestim])
 
 Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and `R̂`.
 
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
+The final argument `covestim` also allows specifying a custom [`StateEstimator`](@ref) 
+object for the estimation of covariance at the arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+1)``. The
+supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and 
+[`ExtendedKalmanFilter`](@ref).
 """
 function MovingHorizonEstimator(
-    model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim::JM
-) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
+    model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, Cwt, optim::JM, 
+    covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
+) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
     P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
-    return MovingHorizonEstimator{NT, SM, JM}(
-        model, He, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, Cwt, optim
+    return MovingHorizonEstimator{NT, SM, JM, CE}(
+        model, He, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, Cwt, optim, covestim
     )
 end
 
+function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
+    return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
+end
+function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
+    return UnscentedKalmanFilter(model,  i_ym, nint_u, nint_ym, P̂0, Q̂, R̂)
+end
 
 @doc raw"""
     setconstraint!(estim::MovingHorizonEstimator; <keyword arguments>) -> estim
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -78,11 +78,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
     V̂, X̂ = predict!(V̂, X̂, ŷ, estim, model, estim.Z̃)
     x̂ .= X̂[end-nx̂+1:end]
-    if Nk == estim.He
-        uarr, ymarr, darr = estim.U[1:model.nu], estim.Ym[1:nym], estim.D[1:model.nd]
-        update_cov!(estim, model, estim.x̂arr_old, estim.P̂arr_old, uarr, ymarr, darr)
-        estim.invP̄.data .= Hermitian(inv(estim.P̂arr_old), :L)
-    end
+    Nk == estim.He && update_cov!(estim::MovingHorizonEstimator)
     return nothing
 end
 
@@ -323,20 +319,18 @@ function trunc_bounds(estim::MovingHorizonEstimator{NT}, Bmin, Bmax, n) where NT
     return Bmin_t, Bmax_t
 end
 
-"Update the covariance `P̂` with the `KalmanFilter` if `model` is a `LinModel`."
-function update_cov!(estim::MovingHorizonEstimator, ::LinModel, _ , P̂, u, ym, d) 
-    return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉ[estim.i_ym, :], P̂)
-end
-"Update it with the `ExtendedKalmanFilter` if model is not a `LinModel`."
-function update_cov!(estim::MovingHorizonEstimator, model::SimModel, x̂, P̂, u, ym, d) 
-    # TODO: also support UnscentedKalmanFilter
-    x̂next, ŷ = similar(estim.x̂), similar(model.yop)
-    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, estim.model, x̂, u, d), x̂next, x̂)
-    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, estim.model, x̂, d), ŷ, x̂)
-    return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :], P̂)
+"Update the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)."
+function update_cov!(estim::MovingHorizonEstimator)
+    nu, nd, nym = estim.model.nu, estim.model.nd, estim.nym
+    uarr, ymarr, darr = @views estim.U[1:nu], estim.Ym[1:nym], estim.D[1:nd]
+    estim.covestim.x̂      .= estim.x̂arr_old
+    estim.covestim.P̂.data .= estim.P̂arr_old # .data is necessary for Hermitian
+    update_estimate!(estim.covestim, uarr, ymarr, darr)
+    estim.P̂arr_old.data   .= estim.covestim.P̂
+    estim.invP̄.data       .= Hermitian(inv(estim.P̂arr_old), :L)
+    return nothing
 end
 
-
 """
     obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) 
 
