Merge pull request #214 from JuliaControl/mhe_inv_inplace

added: `inv!` for `MovingHorizonEstimator` covariance matrices

Author: franckgaga

src/estimator/mhe/construct.jl

@@ -159,10 +159,15 @@ 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̂)
@@ -684,7 +689,7 @@ function setconstraint!(
             con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max
         )
         A = con.A[con.i_b, :]
-        b = con.b[con.i_b]
+        b = zeros(count(con.i_b)) # dummy value, updated before optimization (avoid ±Inf)
         Z̃var = optim[:Z̃var]
         JuMP.delete(optim, optim[:linconstraint])
         JuMP.unregister(optim, :linconstraint)

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")
@@ -759,7 +761,7 @@ function setmodel_estimator!(
         con.A_V̂max
     ]
     A = con.A[con.i_b, :]
-    b = con.b[con.i_b]
+    b = zeros(count(con.i_b)) # dummy value, updated before optimization (avoid ±Inf)
     Z̃var::Vector{JuMP.VariableRef} = estim.optim[:Z̃var]
     JuMP.delete(estim.optim, estim.optim[:linconstraint])
     JuMP.unregister(estim.optim, :linconstraint)
@@ -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

test/2_test_state_estim.jl

@@ -1318,7 +1318,8 @@ end
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0))
     linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0])
-    mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, direct=false)
+    He = 5
+    mhe = MovingHorizonEstimator(linmodel; He, nint_ym=0, direct=false)
     setconstraint!(mhe, x̂min=[-1000], x̂max=[1000])
     @test mhe. ≈ [0.5]
     @test evaloutput(mhe) ≈ [50.0]
@@ -1331,19 +1332,18 @@ end
     @test mhe. ≈ [0.2]
     @test evaloutput(mhe) ≈ [55.0]
     @test mhe.lastu0 ≈ [2.0 - 3.0]
-    @test mhe.U0 ≈ [2.0 - 3.0]
-    @test mhe.Y0m ≈ [50.0 - 55.0]
-    preparestate!(mhe, [55.0])
-    x̂ = updatestate!(mhe, [3.0], [55.0])
+    @test mhe.U0 ≈ repeat([2.0 - 3.0], He)
+    @test mhe.Y0m ≈ repeat([50.0 - 55.0], He)
+    x̂ = preparestate!(mhe, [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
     setmodel!(mhe, newlinmodel)
     @test mhe.x̂0   ≈ [3.0 - 8.0]
     @test mhe.Z̃[1] ≈ 3.0 - 8.0
-    @test mhe.X̂0   ≈ [3.0 - 8.0]
+    @test mhe.X̂0   ≈ repeat([3.0 - 8.0], He)
     @test mhe.x̂0arr_old ≈ [3.0 - 8.0]
-    @test mhe.con.X̂0min ≈ [-1000 - 8.0]
-    @test mhe.con.X̂0max ≈ [+1000 - 8.0]
+    @test mhe.con.X̂0min ≈ repeat([-1000 - 8.0], He)
+    @test mhe.con.X̂0max ≈ repeat([+1000 - 8.0], He)
     @test mhe.con.x̃0min ≈ [-1000 - 8.0]
     @test mhe.con.x̃0max ≈ [+1000 - 8.0]
     setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6])
@@ -1352,7 +1352,7 @@ end
     f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
     h(x,d,model)   = model.C*x + model.Du*d
     nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing)
-    mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
+    mhe2 = MovingHorizonEstimator(nonlinmodel; He, nint_ym=0)
     setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6])
     @test mhe2.Q̂ ≈ [1e-3]
     @test mhe2.R̂ ≈ [1e-6]