The previous gain from rate to magnetometer error was excessive. The revised value is equivalent to a magnetic field length of 0.5 with a timing uncertainty of 0.01 sec
Extended GPS loss can result in the earth field states becoming rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
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.
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.
Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference.
This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
Paul Riseborough
Andrew Tridgell
Siddharth Bharat Purohit