diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4958256f0..4af87b140 100644 --- a/src/estimator/mhe/construct.jl +++ b/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) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c0d294434..ec79a783b 100644 --- a/src/estimator/mhe/execute.jl +++ b/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 diff --git a/src/general.jl b/src/general.jl index 94ed3e63f..6567b0db5 100644 --- a/src/general.jl +++ b/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{ for the source. +- If `A` is a `Hermitian{