Avoid Matrix Inversion in the `MovingHorizonEstimator`

[franckgaga]

Right now I explicitly invert the covariance matrices in the MHE. I do dot(V̂, invR̂_Nk, V̂) in the objective function, which is very fast and non-allocating. The implementation is simple like that but it's probably not the most numerically robust solution.

I should probably store factorization objects of the covariances in the MHE struct, and use rdiv! or ldiv! with these objects in the objective function. We will not able to use the dot function however. I'm not aware of any method similar to the 3-args dot function that would work with factorization objects.

[franckgaga]

Matrix inversion with 3 arguments method of dot is drastically faster than using rdiv! with a Cholesky factorization object. Here some benchmarks:

using LinearAlgebra, BenchmarkTools

function dot_factor!(xᵀ_invA, A_chol, x)
    xᵀ_invA .= x'
    rdiv!(xᵀ_invA, A_chol)
    return dot(xᵀ_invA, x)
end

A = Hermitian(diagm(1.0:1000.0))
x = randn(1000)
A_chol = cholesky(A)
invA = inv(A)

xᵀ_invA = zeros(1,1000)

res1 = dot(x, invA, x)
res2 = dot_factor!(xᵀ_invA, A_chol, x)

println(res1 ≈ res2)
@btime dot($x, $invA, $x)
@btime dot_factor!($xᵀ_invA, $A_chol, $x)

results in:

true
  93.163 μs (0 allocations: 0 bytes)
  662.395 μs (0 allocations: 0 bytes)

any other idea @baggepinnen ?

edit: maybe it's one of the rare "good application" of explicit matrix inversion...

[baggepinnen]

I would stick with the matrix inversion here, when you multiply with it many more times than you invert it it usually pays off.

[franckgaga]

Alright, seems like a sensible conclusion!