Use the joseph form for the posteriori estimate covariance matrix

[jespersmith]

Using the joseph form to calculate the posteriori estimate covariance matrix could result in a more stable filter.

Source (search for joseph in the text):
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb

[georgwi]

If I remember correctly I was considering that at the time but was hesitant since we where very concerned with speed and that form adds some more calculations. I should have made this an optional feature. You probably found the spot where the correction of the covariance is done:
nativeEKF/NativeFilterMatrixOps.cpp - line 69

If you want to switch it out and you find it is not significantly slower I'd be in favor of swapping them. However, those matrices get bigger with more DoFs so the speed might suffer more for larger robots.

[jespersmith]

I will take that in consideration and potentially make it optional