debug: use correct state estimate for MHE cov update

Author: franckgaga

src/estimator/kalman.jl

@@ -568,6 +568,21 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     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̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
+    return update_estimate_ukf!(estim, u, ym, d, P̂, 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
     # --- initialize matrices ---
     X̂, X̂_next = Matrix{NT}(undef, nx̂, nσ), Matrix{NT}(undef, nx̂, nσ)
     ŷm = Vector{NT}(undef, nym)
@@ -802,7 +817,7 @@ end
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
-Allows code reuse for [`KalmanFilter`](@ref) and [`ExtendedKalmanFilterKalmanFilter`](@ref).
+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 (`Â = F̂` and `Ĉm = Ĥm`).
 The implementation uses in-place operations and explicit factorization to reduce

src/estimator/mhe/execute.jl

@@ -80,7 +80,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
     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.P̂arr_old, estim, model, uarr, ymarr, darr)
+        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
     return nothing
@@ -324,16 +324,16 @@ function trunc_bounds(estim::MovingHorizonEstimator{NT}, Bmin, Bmax, n) where NT
 end
 
 "Update the covariance `P̂` with the `KalmanFilter` if `model` is a `LinModel`."
-function update_cov!(P̂, estim::MovingHorizonEstimator, ::LinModel, u, ym, d) 
+function update_cov!(estim::MovingHorizonEstimator, ::LinModel, _ , P̂, u, ym, d) 
     return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉ[estim.i_ym, :], P̂)
 end
 "Update it with the `ExtendedKalmanFilter` if model is not a `LinModel`."
-function update_cov!(P̂, estim::MovingHorizonEstimator, model::SimModel, u, ym, d) 
+function update_cov!(estim::MovingHorizonEstimator, model::SimModel, x̂, P̂, u, ym, d) 
     # TODO: also support UnscentedKalmanFilter
     x̂next, ŷ = similar(estim.x̂), similar(model.yop)
-    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, estim.model, x̂, u, d), x̂next, estim.x̂)
-    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, estim.model, x̂, d), ŷ, estim.x̂)
-    return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :],  P̂)
+    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, estim.model, x̂, u, d), x̂next, x̂)
+    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, estim.model, x̂, d), ŷ, x̂)
+    return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :], P̂)
 end