Because we have changed the yaw angle and have taken a point sample on the magnetic field, covariances associated with the magnetic field states will be invalid and subsequent innovations could cause an unwanted disturbance in roll and pitch.
The reset of the Euler angles to a new yaw orientation was being done using roll and pitch from the output observer states, not the EKF state vector which meant that when roll and pitch were changing, the reset to a new yaw angle would also cause a roll and pitch disturbance.
The legacy EKF switches GPs aiding on on arming, whereas the new EKF switches it on based on GPS data quality.
This means the decision to arm and therefore the predicted solution flags must now reflect the actual status of the navigation solution as it will no longer change when motor arming occurs.
If high vibration levels cause offsets between the two, it switches to the accelerometer with lower vibration levels. The default behaviour is to use the average of both accelerometers.
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
The failure to initialise the magnetometer bias states to zero can result in a large jump in yaw gyro bias and heading when a heading reset is performed.
MPU6000 data sheet indicates that variation on gyro ZRO across temperature range from -40 to +85 is +-20 deg/sec.
The limits on the gyro bias states have been increased to allow for this.
To enable the EKF to accommodate such large gyro bias values in yaw without the yaw error wrapping, leading to continual heading drift, an unwrap function has been applied to the compass heading error.
Paul Riseborough
Andrew Tridgell
Randy Mackay
Siddharth Bharat Purohit
Paul Riseborough