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/controller/execute.jl

@@ -114,7 +114,7 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3)
 function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     model    = mpc.estim.model
     nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu 
-    info = Dict{Symbol, Union{JuMP._SolutionSummary, Vector{NT}, NT}}()
+    info = Dict{Symbol, Any}()
     Ŷ0, u0, û0  = similar(mpc.Yop), similar(model.uop), similar(model.uop)
     Ŷs          = similar(mpc.Yop)
     x̂0, x̂0next  = similar(mpc.estim.x̂0), similar(mpc.estim.x̂0)
@@ -491,7 +491,8 @@ If supported by `mpc.optim`, it warm-starts the solver at:
 where ``\mathbf{Δu}_{k-1}(k+j)`` is the input increment for time ``k+j`` computed at the 
 last control period ``k-1``. It then calls `JuMP.optimize!(mpc.optim)` and extract the
 solution. A failed optimization prints an `@error` log in the REPL and returns the 
-warm-start value.
+warm-start value. A failed optimization also prints [`getinfo`](@ref) results in
+the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages).
 """
 function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     model, optim = mpc.estim.model, mpc.optim
@@ -518,13 +519,22 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     if !issolved(optim)
         status = JuMP.termination_status(optim)
         if iserror(optim)
-            @error("MPC terminated without solution: returning last solution shifted", 
-                   status)
+            @error(
+                "MPC terminated without solution: estimation in open-loop "*
+                "(more info in debug log)",
+                status
+            )
         else
-            @warn("MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping "*
-                  "solution anyway", status)
+            @warn(
+                "MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "*
+                "anyway (more info in debug log)", 
+                status
+            )
         end
-        @debug JuMP.solution_summary(optim, verbose=true)
+        @debug(
+            "calling getinfo (use logger with show_limited=false if values are truncated)", 
+            getinfo(mpc)
+        )
     end
     if iserror(optim)
         mpc.ΔŨ .= ΔŨ0

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
@@ -108,8 +108,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     nu, ny, nd = model.nu, model.ny, model.nd
     nx̂, nym, nŵ, nϵ = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ
     nx̃ = nϵ + nx̂
-    MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT}
-    info = Dict{Symbol, MyTypes}()
+    info = Dict{Symbol, Any}()
     V̂,  X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk])
     û0, ŷ0 = similar(model.uop), similar(model.yop)
     V̂,  X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃)
@@ -370,7 +369,8 @@ If supported by `estim.optim`, it warm-starts the solver at:
 where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` computed at the 
 last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the
 solution. A failed optimization prints an `@error` log in the REPL and returns the 
-warm-start value.
+warm-start value. A failed optimization also prints [`getinfo`](@ref) results in
+the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages).
 """
 function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, optim = estim.model, estim.optim
@@ -406,13 +406,22 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     if !issolved(optim)
         status = JuMP.termination_status(optim)
         if iserror(optim)
-            @error("MHE terminated without solution: estimation in open-loop", 
-                   status)
+            @error(
+                "MHE terminated without solution: estimation in open-loop "*
+                "(more info in debug log)",
+                status
+            )
         else
-            @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "*
-                  "solution anyway", status)
+            @warn(
+                "MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "*
+                "anyway (more info in debug log)", 
+                status
+            )
         end
-        @debug JuMP.solution_summary(optim, verbose=true)
+        @debug(
+            "calling getinfo (use logger with show_limited=false if values are truncated)", 
+            getinfo(estim)
+        )
     end
     if iserror(optim)
         estim.Z̃ .= Z̃_0
@@ -433,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
 
@@ -445,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)