This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.
Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter.
1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed
2) The velocity states were not being reset
3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
Remember the mag bias and earth field states learned during flight when the vehicle lands.
This improves performance for vehicles that do multiple flight on one power cycle
IMU data was being corrected before being used by the co-variance prediction, whereas the delta angles and velocities in the derivation were supposed to be uncorrected.
This patch creates separate variable for the corrected data
The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe.
A check for yaw errors due to a ground based magnetic anomaly has been introduced.
The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
Implements the following techniques to enable planes to operate without magnetometers.
1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth.
2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw.
3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
Use an Euler yaw heading that switches between a 321 and 312 rotation
sequence to avoid areas of singularity. Using Euler yaw decouples the
observation from the roll and pitch states and prevents magnetic
disturbances from affecting roll and pitch via the magnetometer fusion
process.
The use of yaw angle fusion during startup and ground operation causes problems with tail-sitter vehicle types.
Instead of observing an Euler yaw angle, we now observe the yaw angle obtained by projecting the measured magnetic field onto the the horizontal plain.
This avoids the singularities associated with the observation of Euler yaw angle.
The innovation calculation should have been updated when the heading fusion maths was updated.
We now use a direct heading or yaw angle measurement in the derivation, not the difference between observed and published declination.
Paul Riseborough
Peter Barker
Randy Mackay
Dr.-Ing. Amilcar Do Carmo Lucas
Mathieu OTHACEHE
Jonathan Challinger
Andrew Tridgell
Ricardo de Almeida Gonzaga
dgrat