ekf2: use Joseph covariance update

[bresch]

Solved Problem

The conventional covariance update (P = P - KHP or P = P - KSK') is only correct when an optimal Kalman gain is used. EKF2 is closer to a Schmidt-Kalman filter as it "manually" clears part of the Kalman gain when states are poorly observable or when affecting a certain state can harm the estimate. For example, airspeed fusion is only allowed to affect the wind states when another velocity/position aiding source is active. Using the conventional update can be dangerous in that case as the covariances are not updated anymore while the state variance decreases; artificially increasing the correlation between the states.

Solution

Use the famous Joseph stabilized covariance update as this equation is correct with any Kalman gain (not only the optimal one). Another benefit is that this equation is numerically more stable. The implementation ensures to have a symmetrical matrix as only the upper triangular matrix is computed and copied to the lower part.

The CPU increase is about 1%, which is IMO acceptable given the benefits of this equation.

Changelog Entry

For release notes:

Use Joseph stabilized covariance update in EKF2
New parameter: -
Documentation: -

Alternatives

Clear the covariance of a state when the Kalman gain has been modified significantly.

[dagar]

As discussed one big piece to understand here is how much we can actually afford.

Then potentially one option could be doing this switch automatically if the Kalman gains are modified substantially (by clearInhibitedStateKalmanGains(K)).

[bresch]

The Joseph stabilized update is too expensive for an F7 processor (EKF2 takes 40% of CPU vs 10% for the simplified update).
Instead of using the state-of-the-art Schmidt-Kalman filter with consider states, we then need to clear out the covariance of the inhibited states to prevent the covariance matrix from developing incorrect high correlations. Next steps would be to minimize the amount of dynamic state inhibition in general (e.g.: run full mag 3D all the time, don't inhibit accel biases except in some extreme cases).

edit: I found a much faster implementation. Now the CPU increase is ~1%, which is totally acceptable given the gains in numerical stability and code cleanness.

[bresch]

@dagar I squashed the relevant commits together and rebased on main. Good to go on my side.