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.

[1-Bart-1]

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

I have gotten this error several times as well.

[franckgaga]

@1-Bart-1 If you have a MWE that systematically produces this error, send it to me. I will try to investigate on the error. Fredrik and I emit the hypothesis that It may be caused by the default values of the UKF $\alpha, \beta, \kappa$ weights (the defaults are not rigorously justified, it's just values that many people use without a real justification).

If that's really the case, a breaking change may be worthwhile.

[baggepinnen]

@1-Bart-1 see this comment and the linked article for more details on the UT parameters
baggepinnen/LowLevelParticleFilters.jl#191 (comment)

[1-Bart-1]

I don't have a MWE... If I remember correctly, the error tends to disappear when changing some of the estimator weights. But I will definitely come back here if the error appears again.

[1-Bart-1]

I am getting this issue again. I don't have a lightweight example, but you can recreate the issue by running mwe/online_lin_simple_kite.jl from KitePredictiveControl.jl#cholesky. Let me know if you have any issues recreating this. Is it possible to play around with the cholesky weights in MPC.jl currently?
Also note that I use the KalmanFilter in this example, not the UnscentedKalmanFilter.