* ekf_control: Inhibit mag fusion when field magnitude is large
Move mag inhibition check in separate function
* ekf_control: pull out of functionalities out of controlMagFusion
- yaw abd mag bias observability checks
- mag 3D conditions
- load mag covariances
- set and clear mag control modes
* ekf_control: refactor mag heading/3D start/stop.
Move mag declination, mag 3d and mag heading fusion out of the main function
* ekf_control: extract mag yaw reset and mag declination fusion requirements
* ekf_control: use WMM in isStronMagneticField for mag fusion inhibition
- Correct units of WMM strength table
* ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom)
Also split inAirYawReset from onGroundYawReset
* ekf_control: extract mag automatic selection
- transform if-else into switch-case for parameter fusion type selection
* ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix
flag naming in Test script
* ekf_control: do not run mag fusion if tilt is not aligned.
Reset some variables on ground even if mag fusion is not running yet. It
could be that it runs later so we need to make sure that those variables
are properly set.
* ekf_control: move controlMagFusion and related functions to mag_control.cpp
* ekf control: check for validity of mag strength from WMM and falls back
to average earth mag field with larger gate if not valid
* ekf control: remove evyaw check for mag inhibition
* ekf control: change nested ternary operator into if-else if
* Ekf: create AlphaFilter template class for simple low-pass filtering
0.1/0.9 type low-pass filters are commonly used to smooth data, this
class is meant to abstract the computation of this filter
* ekf control: reset heading using mag_lpf data to avoid resetting on an outlier
fixes ecl issue #525
* ekf control: replace mag_states_only flag with mag_field_disturbed and
add parameter to enable or disable mag field strength check
* ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore
* ekf control: use start/stop mag functions everywhere instead of setting the flag
* ekf control: Run mag fusion depending on yaw_align instead of tilt_align
as there is no reason to fuse mag when the ekf isn't aligned
* AlphaFilter: add test for float and Vector3f
* EKF angle constants in degrees for readability
* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S
* EKF controlOpticalFlowFusion() use constants and update comments
* EKF controlMagFusion() use angle in degrees
* EKF move earth spin rate to geo and update usage
* EKF: Fix numerical constant error and clean up comments
Comments do not need to contain numerical values when the code makes these clear.
* EKF collect_imu take const imu sample and populate buffer
* EKF calculateOutputStates cleanup
* EKF add calculate_quaternion output predictor method
* EKF: update documentation
* EKF: remove unnecessary getter function
* EKF calculateOutputStates only apply dt correction to bias
* EKF pytest assert attitude validity, not update() return
* EKF: correct documentation
* EKF: Do not make attitude validity dependent on yaw alignment status
Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.
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.
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.
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.
