EKF covariance prediction stability improvements

[bresch]

Issue

A recent unit test I created to replicate a real-life issue (#1022) revealed a weakness in the covariance prediction code, creating correlation between physically decoupled states.

The fix

By incorporating the unit quaternion constraint in the quat2Rot conversion, the covariance prediction gets more stable and avoids having correlation between states that should be absolutely separated in practice (e.g. Z gyro bias -> Z accel bias).

This was most likely the main cause of the accel bias divergence we've seen in EKF2.

The generated code for the aiding sources (fusions) is also slightly different but I will update them later as most of the issues were apparently due to the covariance prediction and seems to be fixed now.

SITL tests showed that IMU biases are now really well estimated.

Furthermore, it seems that the heading converges much faster than before in mag-less operation during accelerations of the vehicle.

[priseborough]

I've independently generated the auto-code, imported it and diffed against this PR. Also this derivation change has solved replay of a problem log showing bad covariance conditioning in the ardupilot EKF3 that uses the same math.

[MaEtUgR]

Nice find!! 👍
I wouldn't have thought this makes a difference when you use it in a symbolic context.