Support Joseph-form in Kalman filters

[franckgaga]

It would be nice to support the Joseph-form covariance update equation in KalmanFilter, ExtendedKalmanFilter and UnscentedKalmanFilter. It theoretically guarantee positive-definite covariance matrices, even with finite precision floating-point arithmetic.

In my experience although, I never had any problems with the classical equation, both with the Kalman filter and the unscented version. I personally don't consider that as a high priority feature.

[baggepinnen]

Another option is to have the QR form https://arxiv.org/abs/2208.06452
implemented here https://github.com/baggepinnen/LowLevelParticleFilters.jl/blob/master/src/sq_kalman.jl

[franckgaga]

I noticed that once and a while in the MovingHorizonEstimator tests, the estimation fails because of the arrival covariance $\mathbf{\bar{P}}$ estimation based on UnscentedKalmanFilter, that internally uses cholesky. The error is PosDefException: matrix is not positive definite; Factorization failed.

It raises two points:

1. Should I encapsulate arrival covariance estimation of the MHE in a try block ? If yes what would be the fallback ? Do not update the arrival covariance (keep the one from the last iteration $k-1$) and emit a @warn ? I think It would be a good idea since with a reasonably long He, the impact of the arrival estimate term in the cost function is typically quite low. Morever, I frequently used the MHE with a constant arrival covariance estimate in the past and it was simpler and working quite well.

edit: an alternative and presumably better fallback would be to modify the current arrival covariance matrix to the nearest symmetric one with P̄ = 0.5*(P̄ + P̄')

2. The Joseph-From should be implemented in the package and be the default for the MovingHorizonEstimator arrival covariance estimation. The "numerical noise" produced by the MHE optimization is more risky to produce negative eigenvalues in the covariance matrix.

Do you have an opinion @baggepinnen ?

Thanks!

[baggepinnen]

I include P̄ = 0.5*(P̄ + P̄') in more or less all code that modifies covariance matrices, I find that this is often all that is required to handle numerical issues in Kalman filters

[franckgaga]

I'll also add this line as another "saftey net" for the MHE.