- refactor updateQuaternion() to compute the yaw jacobian directly (respecting the rotation sequence determination)
- fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
- clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
- when at rest continue fusing last static heading with very low variance even if other heading sources are active
At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
- in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
- new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
- sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
- existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
- ekf2 single instance mode is still fully supported and the default
Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
* ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowpass filtering
* ekf2: use InnovLpf filter class in preflight checks
* ekf2: move selection of yaw test limit for pre-flight check in function
* ekf2: Move pre-flight checks into separate function
* ekf2: use static constexpr insetead of inline for sq (square) function
* ekf2: Split pre-flight checks in separate functions
Also use the same check for all the innovations:
innov_lpf < test and innov < 2xtest
* ekf2: Add optical flow pre-flight check
* ekf2: Combine FirstOrderLpf and InnovationLpf in single class
* ekf2: check vel_pos_innov when ev_pos is active as well
* ekf2: transform InnovationLpf into a header only library and pass the
spike limit during the update call to avoid storing it here
* ekf2: Static and const cleanup
- set spike_lim constants as static constexpr, set innovation
- set checker helper functions as static
- rename the mix of heading and yaw as heading to avoid confusion
* ekf2: use ternary operator in selectHeadingTestLimit instead of if-else
* ekf2: store intermediate redults in const bool flags. Those will be used for logging
* ekf2: set variable const whenever possible
* ekf2: create PreFlightChecker class that handle all the innovation
pre-flight checks.
Add simple unit testing
Use bitmask instead of general flag to have more granularity
* PreFlightChecker: use setter for the innovations to check instead of sending booleans in the update function
This makes it more scalable as more checks will be added
* ekf: Use booleans instead of bitmask for ekf preflt checks
Rename "down" to "vert"
It makes more sense to set the optimization flags on a platform basis
instead of individually for each module. This allows for different
optimization options for SITL, NuttX, Snapdragon, etc.
bresch