This enables the initial uncertainty to be set based on application and also ensures that the max allowed growth in wind state variance is consistent with the initial uncertainty specified.
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 .
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.
Enables wind estimation without an airspeed sensor and enables synthetic sideslip to be used with an airspeed sensor for improved wind state estimation.
Wind states and covariances are reset differently depending on whether airspeed is available.
Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions.
Enable setting of initial angle uncertainty via a parameter
Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
Noise variance for stationary states was being overwritten by the covariance prediction operations for the kinematic states
Unwanted states could end up with non-zero covariance values
The forced symmetry operation was being applied too late to be effective
Parameters needed to be changed when covariance prediction error was corrected.
The magnetic earth and body field process noise now use separate parameters to help with tuning
ChristophTobler
Paul Riseborough
Daniel Agar
CarlOlsson
Roman Bapst