Merge pull request #375 from JuliaControl/mhe_skf_dispatch

added: dispatch on `covestim` type when inverting `P̄` in the `MovingHorizonEstimator`

Author: franckgaga

src/estimator/mhe/execute.jl

@@ -30,8 +30,8 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0)
     nd > 0 && (estim.D0[1:nd] .= d0) # add d0(-1) to the data window
     estim.lastu0 .= u0
     # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0)
-    invert_cov!(estim, estim.cov.P̂_0)
     estim.P̂arr_old  .= estim.cov.P̂_0
+    invert_cov!(estim, estim.covestim)
     estim.x̂0arr_old .= 0
     return nothing
 end
@@ -709,7 +709,7 @@ function correct_cov!(estim::MovingHorizonEstimator)
         correct_estimate!(estim.covestim, y0marr, d0arr)
         all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite")
         estim.P̂arr_old .= estim.covestim.cov.P̂
-        invert_cov!(estim, estim.P̂arr_old)
+        invert_cov!(estim, estim.covestim)
     catch err
         if err isa PosDefException
             @error("Arrival covariance P̄ is not positive definite: keeping the old one")
@@ -736,7 +736,7 @@ function update_cov!(estim::MovingHorizonEstimator)
         update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
         all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite")
         estim.P̂arr_old .= estim.covestim.cov.P̂
-        invert_cov!(estim, estim.P̂arr_old)
+        invert_cov!(estim, estim.covestim)
     catch err
         if err isa PosDefException
             @error("Arrival covariance P̄ is not positive definite: keeping the old one")
@@ -749,8 +749,9 @@ function update_cov!(estim::MovingHorizonEstimator)
     return nothing
 end
 
-"Invert the covariance estimate at arrival `P̄`."
-function invert_cov!(estim::MovingHorizonEstimator, P̄)
+"Invert the covariance estimate at arrival `P̄` and store it in `estim.cov.invP̄`."
+function invert_cov!(estim::MovingHorizonEstimator, covestim::StateEstimator)
+    P̄ = estim.P̂arr_old
     estim.cov.invP̄ .= P̄
     try
         inv!(estim.cov.invP̄)
@@ -763,6 +764,9 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄)
     end
     return nothing
 end
+"Do nothing if `covestim` is a [`SteadyKalmanFilter`]."
+invert_cov!(::MovingHorizonEstimator, ::SteadyKalmanFilter) = nothing
+
 
 """
     obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) 

test/2_test_state_estim.jl

@@ -1011,6 +1011,17 @@ end
     info = getinfo(mhe3)
     @test info[:x̂] ≈ x̂ atol=1e-9
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
+    covestim = SteadyKalmanFilter(linmodel)
+    mhe3b = MovingHorizonEstimator(linmodel; He=1, σP_0=nothing, covestim)
+    preparestate!(mhe3b, [50, 30], [5])
+    x̂ = updatestate!(mhe3b, [10, 50], [50, 30], [5])
+    @test x̂ ≈ zeros(6) atol=1e-9
+    @test mhe3b.x̂0 ≈ zeros(6) atol=1e-9
+    @test mhe3b.cov.invP̄ ≈ inv(covestim.cov.P̂)
+    preparestate!(mhe3b, [50, 30], [5])
+    info = getinfo(mhe3b)
+    @test info[:x̂] ≈ x̂ atol=1e-9
+    @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
 
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mhe3 = MovingHorizonEstimator(linmodel3, He=1)
@@ -1181,7 +1192,7 @@ end
     @test mhe.cov.invP̄ ≈ invP̄_copy
     @test_logs(
         (:error, "Arrival covariance P̄ is not invertible: keeping the old one"), 
-        ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L))
+        ModelPredictiveControl.invert_cov!(mhe, mhe.covestim)
     )
     mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback
     P̂arr_old_copy = deepcopy(mhe.P̂arr_old)