Add calculation of a vertical position derivative to the output predictor. This will have degraded tracking relative to the EKF states, but the velocity will be closer to the first derivative of the position and reduce the effect inertial prediction errors on control loops that are operating in a pure velocity feedback mode.
Move calculation of IMU offset angular rate correction out of velocity accessor and into output predictor.
Provide separate accessor for vertical position derivative.
Add ability to start before GPS checks pass.
Add ability to turn GPS fusion off and on during replay.
Add ability to turn Optical Flow fusion off and on during replay.
Add ability to turn Visual Odometry fusion off and on during replay.
Convert miscellaneous constants to parameters
Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.
Add a bitmask parameter to control bias learning for individual axes. This is achieved by setting the disabled states to zero together with their corresponding covariances.
Minor cleanup of the covariance prediction comments.
Removal of unnecessary variable copy operations.
Replace index operations to initialise covariance to zero with the more efficient memset.
Use vertical velocity and position innovation failure to detect bad accelerometer data caused by clipping or aliasing which can cause large vertical acceleration errors and loss of height estimation. When bad accel data is detected:
1) Inhibit accelerometer bias learning
2) Force fusion of vertical velocity and height data
3) Increase accelerometer process noise
The previous practice of relying on the off-diagonals being zero caused problems with conditioning of the magnetometer fusion on one flight. By storing the variances when the learning inhibit becomes active and ensuring that the rows and columns in the covariance matrix for the inhibited states are always zero, the observed numerical conditioning error has been eliminated for replay of the problem flight log .
Doing so can casue large jumps in GPS position and innovation check errors after landing and also reduces the effectiveness of pre-flight innovation consistency checks.
Previously GPS quality checks were only run until the EKF origin was set. This meant that they could not be used by other pre-flight checks.
This change ensures that checks will always be run when the vehicle on-ground or not using GPS to enable use by external preflight checks.
When using a union of flags and integer value it is safer to initialise the value to 0 rather than memset the flags because the flags may not define all bits in the integer.
This bug caused X and Y delta velocity bias state variance to be reset to the same value as the Z axis when learning was inhibited.
Documentation has also been updated.
CarlOlsson
Paul Riseborough
Paul Riseborough
Julien Lecoeur
Daniel Agar
ChristophTobler