changed: computing inv with cholesky in MovingHorizonEstimator

Author: franckgaga

Project.toml

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

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

@@ -479,9 +479,9 @@ end
 "Invert the covariance estimate at arrival `P̄`."
 function invert_cov!(estim::MovingHorizonEstimator, P̄)
     try
-        estim.invP̄ .= inv(P̄)
+        estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄)
     catch err
-        if err isa SingularException || err isa LAPACKException
+        if err isa PosDefException
             @warn("Arrival covariance is not invertible: keeping the old one")
         else
             rethrow()

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