Merge pull request #151 from JuliaControl/try_mhe

debug: fallback if `MovingHorizonEstimator` arrival covariance update fails

Project.toml

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

src/estimator/luenberger.jl

@@ -137,7 +137,7 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0)
 end
 
 "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values."
-function setmodel!(estim::Luenberger, model, args...)
+function setmodel_estimator!(estim::Luenberger, model, args...)
     if estim.model !== model
         error("Luenberger does not support setmodel!")
     end

src/estimator/mhe/construct.jl

@@ -123,11 +123,6 @@ struct MovingHorizonEstimator{
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
         lastu0 = zeros(NT, nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
-        P̂_0 = Hermitian(P̂_0, :L)
-        Q̂, R̂ = Hermitian(Q̂, :L),  Hermitian(R̂, :L)
-        invP̄ = Hermitian(inv(P̂_0), :L)
-        invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L)
-        invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L)
         r = direct ? 0 : 1
         E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe(
             model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r
@@ -146,10 +141,18 @@ struct MovingHorizonEstimator{
         nD0 = direct ? nd*(He+1) : nd*He
         U0, D0  = zeros(NT, nu*He), zeros(NT, nD0) 
         Ŵ = zeros(NT, nx̂*He)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
+        P̂_0 = Hermitian(P̂_0, :L)
+        Q̂, R̂ = Hermitian(Q̂, :L),  Hermitian(R̂, :L)
+        P̂_0 = Hermitian(P̂_0, :L)
+        invP̄ = inv_cholesky!(buffer.P̂, P̂_0)
+        invQ̂ = inv_cholesky!(buffer.Q̂, Q̂)
+        invR̂ = inv_cholesky!(buffer.R̂, R̂)
+        invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L)
+        invR̂_He = Hermitian(repeatdiag(invR̂, He), :L)
         x̂0arr_old = zeros(NT, nx̂)
         P̂arr_old = copy(P̂_0)
         Nk = [0]
-        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         corrected = [false]
         estim = new{NT, SM, JM, CE}(
             model, optim, con, covestim,  

src/estimator/mhe/execute.jl

@@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
         estim.D0[1:estim.model.nd] .= d0
     end
     # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
-    estim.invP̄      .= inv(estim.P̂_0)
+    invert_cov!(estim, estim.P̂_0)
     estim.P̂arr_old  .= estim.P̂_0
     estim.x̂0arr_old .= 0
     return nothing
@@ -442,9 +442,17 @@ function correct_cov!(estim::MovingHorizonEstimator)
     y0marr, d0arr = @views estim.Y0m[1:nym], estim.D0[1:nd]
     estim.covestim.x̂0 .= estim.x̂0arr_old
     estim.covestim.P̂  .= estim.P̂arr_old
-    correct_estimate!(estim.covestim, y0marr, d0arr)
-    estim.P̂arr_old .= estim.covestim.P̂
-    estim.invP̄     .= inv(estim.P̂arr_old)
+    try
+        correct_estimate!(estim.covestim, y0marr, d0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.P̂arr_old)
+    catch err
+        if err isa PosDefException
+            @warn("Arrival covariance is not positive definite: keeping the old one")
+        else
+            rethrow()
+        end
+    end
     return nothing
 end
 
@@ -454,9 +462,31 @@ function update_cov!(estim::MovingHorizonEstimator)
     u0arr, y0marr, d0arr = @views estim.U0[1:nu], estim.Y0m[1:nym], estim.D0[1:nd]
     estim.covestim.x̂0 .= estim.x̂0arr_old
     estim.covestim.P̂  .= estim.P̂arr_old
-    update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
-    estim.P̂arr_old    .= estim.covestim.P̂
-    estim.invP̄        .= inv(estim.P̂arr_old)
+    try
+        update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.P̂arr_old)
+    catch err
+        if err isa PosDefException
+            @warn("Arrival covariance is not positive definite: keeping the old one")
+        else
+            rethrow()
+        end
+    end
+    return nothing
+end
+
+"Invert the covariance estimate at arrival `P̄`."
+function invert_cov!(estim::MovingHorizonEstimator, P̄)
+    try
+        estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄)
+    catch err
+        if err isa PosDefException
+            @warn("Arrival covariance is not invertible: keeping the old one")
+        else
+            rethrow()
+        end
+    end
     return nothing
 end
 

src/general.jl

@@ -59,4 +59,18 @@ end
 to_hermitian(A::AbstractVector) = Hermitian(reshape(A, 1, 1), :L)
 to_hermitian(A::AbstractMatrix) = Hermitian(A, :L)
 to_hermitian(A::Hermitian) = A
-to_hermitian(A) = A
+to_hermitian(A) = A
+
+"""
+Compute the inverse of a the Hermitian positive definite matrix `A` using `cholesky`.
+
+Builtin `inv` function uses LU factorization which is not the best choice for Hermitian
+positive definite matrices. The function will mutate `buffer` to reduce memory allocations.
+"""
+function inv_cholesky!(buffer::Matrix, A::Hermitian)
+    Achol  = Hermitian(buffer, :L)
+    Achol .= A
+    chol_obj = cholesky!(Achol)
+    invA = Hermitian(inv(chol_obj), :L)
+    return invA
+end

test/test_state_estim.jl

@@ -975,6 +975,35 @@ end
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
 end
 
+@testset "MovingHorizonEstimator fallbacks for arrival covariance estimation" begin
+    linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
+    f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
+    h(x,d,_)   = linmodel.C*x + linmodel.Dd*d
+    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5])
+    mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1)
+    preparestate!(mhe, [50, 30], [5])
+    updatestate!(mhe, [10, 50], [50, 30], [5])
+    mhe.P̂arr_old[1, 1] = -1e-3 # negative eigenvalue to trigger fallback
+    P̂arr_old_copy = deepcopy(mhe.P̂arr_old)
+    invP̄_copy = deepcopy(mhe.invP̄)
+    @test_logs(
+        (:warn, "Arrival covariance is not positive definite: keeping the old one"), 
+        preparestate!(mhe, [50, 30], [5])
+    )
+    @test mhe.P̂arr_old ≈ P̂arr_old_copy
+    @test mhe.invP̄ ≈ invP̄_copy
+    @test_logs(
+        (:warn, "Arrival covariance is not positive definite: keeping the old one"), 
+        updatestate!(mhe, [10, 50], [50, 30], [5])
+    )
+    @test mhe.P̂arr_old ≈ P̂arr_old_copy
+    @test mhe.invP̄ ≈ invP̄_copy
+    @test_logs(
+        (:warn, "Arrival covariance is not invertible: keeping the old one"), 
+        ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L))
+    )
+end
+
 @testset "MovingHorizonEstimator set constraints" begin
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0, Cwt=1e3)