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.
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.