* ekf2_derivation: use single source of state definition
The state is defined as an ordered dictionary of group elements and
everything else is generated using that state definition
* ekf2: generated state sample add const reference getter
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Co-authored-by: Daniel Agar <daniel@agar.ca>
The sparse vector template requires to know which states are non-zero in
the observation jacobian. This complicates the modularity of the code
when the state vector or the derivation is changed.
The computation cost difference is almost negligible for this size.
* ekf2-test: remove outdated codegen comparison
The definition of states changed so the comparison with the old
derivation cannot work anymore.
---------
Co-authored-by: bresch <brescianimathieu@gmail.com>
- refactor all EKF backend output predictor pieces into new OutputPredictor class
- output states are now calculated immediately with new high rate IMU rather than after EKF update
- IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
- if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
- handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
- isRecent() and isTimedOut() helpers use delayed time
- add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data
- 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
A constant large value in the (signed) normalized innovation test ratio is a sign
of bias in the state estimate. This metric can be used to trigger a
covariance boost or reset
bresch