Merge pull request #40 from JuliaControl/debug_mhe

Debug: MHE with `NonLinModel` update covariance with the correct state estimate

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.19.1"
+version = "0.19.2"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -130,4 +130,3 @@ for more detailed examples.
   - [x] state estimates
   - [x] process noise estimates
   - [x] sensor noise estimates
-- [ ] moving horizon estimator with no process noise to reduce the problem size

src/estimator/kalman.jl

@@ -565,7 +565,22 @@ noise, respectively.
      ISBN9780470045343.
 """
 function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:Real
-    x̂, P̂, Q̂, R̂, K̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂
+    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̂
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
     # --- initialize matrices ---
@@ -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
 
 

src/model/linmodel.jl

@@ -47,8 +47,8 @@ with the state ``\mathbf{x}`` and output ``\mathbf{y}`` vectors. The ``\mathbf{z
 comprises the manipulated inputs ``\mathbf{u}`` and measured disturbances ``\mathbf{d}``, 
 in any order. `i_u` provides the indices of ``\mathbf{z}`` that are manipulated, and `i_d`, 
 the measured disturbances. The constructor automatically discretizes continuous systems,
-resamples discrete ones if `Ts ≠ sys.Ts`, compute a new realization if not minimal, and 
-separate the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the
+resamples discrete ones if `Ts ≠ sys.Ts`, computes a new realization if not minimal, and 
+separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the
 documentation assumes discrete dynamics since all systems end up in this form.
 
 See also [`ss`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.ss)