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 .
Make the target EKF rate an integer multiple of the IMU rate. This slightly increases the average prediction time step for the EKF from just over 10msec to 12msec, but the variation reduces significantly which makes filter tuning more deterministic.
Improve the algorithm used to adjust the collection time criteria to reduce jitter in the correction.
This is a functionally equivalent. It moves all of the code for the terrain estimator into a single function call from the main filter update, making it clear that it is independent of the main filter.
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon.
Information and warning messages are improved.
Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation.
If EV height selected ensure switch to correct height mode as soon as EV data is received
The 0 height datum is not at the initialisation position, so the height state needs to be reset to the measurement on startup
Setting the velocity tracking tine constant to a negative number causes the output predictor to use a different method of correcting the velocity which provides a velocity output that is kinematically consistent with the position output.
This may improve height controller performance under some circumstances
Paul Riseborough
Paul Riseborough
Roman
devbharat
Daniel Agar
Nic
CarlOlsson
waltjohnson