added: inv! for MHE covariance matrices

allocation-free with `LinearAlgebra` standard types (and numerically robust since it relies on a cholesky factorization)
there is also a fallback method with other `AbstractMatrix` with some allocations

Author: franckgaga

src/estimator/mhe/construct.jl

@@ -159,10 +159,16 @@ struct MovingHorizonEstimator{
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
         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̂)
+
+        invP̄ = Hermitian(buffer.P̂, :L)
+        invP̄ .= P̂_0
+        inv!(invP̄)
+        invQ̂ = Hermitian(buffer.Q̂, :L)
+        invQ̂ .= Q̂
+        inv!(invQ̂)
+        invR̂ = Hermitian(buffer.R̂, :L)
+        invR̂ .= R̂
+        inv!(invR̂)
         invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L)
         invR̂_He = Hermitian(repeatdiag(invR̂, He), :L)
         x̂0arr_old = zeros(NT, nx̂)

src/estimator/mhe/execute.jl

@@ -528,8 +528,10 @@ end
 
 "Invert the covariance estimate at arrival `P̄`."
 function invert_cov!(estim::MovingHorizonEstimator, P̄)
+    invP̄  = Hermitian(estim.buffer.P̂, :L)
+    invP̄ .= P̄
     try
-        estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄)
+        inv!(invP̄)
     catch err
         if err isa PosDefException
             @error("Arrival covariance P̄ is not invertible: keeping the old one")
@@ -792,11 +794,17 @@ function setmodel_estimator!(
     # --- covariance matrices ---
     if !isnothing(Q̂)
         estim.Q̂ .= to_hermitian(Q̂)
-        estim.invQ̂_He .= repeatdiag(inv(estim.Q̂), He)
+        invQ̂  = Hermitian(estim.buffer.Q̂, :L)
+        invQ̂ .= estim.Q̂
+        inv!(invQ̂)
+        estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L)
     end
     if !isnothing(R̂) 
         estim.R̂ .= to_hermitian(R̂)
-        estim.invR̂_He .= repeatdiag(inv(estim.R̂), He)
+        invR̂  = Hermitian(estim.buffer.R̂, :L)
+        invR̂ .= estim.R̂
+        inv!(invR̂)
+        estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L)
     end
     return nothing
 end

src/general.jl

@@ -84,15 +84,30 @@ to_hermitian(A::Hermitian) = A
 to_hermitian(A) = A
 
 """
-Compute the inverse of a the Hermitian positive definite matrix `A` using `cholesky`.
+Compute the inverse of a the Hermitian positive definite matrix `A` in-place and return it.
 
-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.
+There is 3 methods for this function:
+- If `A` is a `Hermitian{<Real, Matrix{<:Real}}`, it uses `LAPACK.potrf!` and 
+  `LAPACK.potri!` functions to compute the Cholesky factor and then the inverse. This is
+  allocation-free. See <https://tinyurl.com/4pwdwbcj> for the source.
+- If `A` is a `Hermitian{<Real, Diagonal{<:Real, Vector{<:Real}}}`, it computes the 
+  inverse of the diagonal elements in-place (allocation-free).
+- Else if `A` is a `Hermitian{<:Real, <:AbstractMatrix}`, it computes the Cholesky factor
+  with `cholesky!` and then the inverse with `inv`, which allocates memory.
 """
-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
+function inv!(A::Hermitian{NT, Matrix{NT}}) where {NT<:Real}
+    _, info = LAPACK.potrf!(A.uplo, A.data)
+    (info == 0) || throw(PosDefException(info))
+    LAPACK.potri!(A.uplo, A.data)
+    return A
+end
+function inv!(A::Hermitian{NT, Diagonal{NT, Vector{NT}}}) where {NT<:Real}
+    A.data.diag .= 1 ./ A.data.diag
+    return A
+end
+function inv!(A::Hermitian{<:Real, <:AbstractMatrix})
+    Achol = cholesky!(A)
+    invA = inv(Achol)
+    A .= Hermitian(invA, :L)
+    return A
 end