While a yaw test ration > 1 rejects the mag data in air, the fusion is
forced with a limited innovation on ground to fix gyro bias and local
magnetic effects problems pre-takeoff. However, the same logic is
applied after landing and immediately after the land detector detects
landing. In some cases, this can disturb the other EKF states which can
lead to a state change of the land detector.
To avoid such a loop, the start of this forced yaw fusion is delayed by
5 seconds after landing.
These changes fix a bug that allowed IMU gyro errors that cause large tilt errors prior to start of aiding not resulting in large innovation test ratios.
- extract motion checks performed on ground
- move all non-timing check to controlOpticalFlowFusion. This simplifies and standardirzes the setOpticalFlowData function. Furthermore, in some cases (SITL, PX4Flow), the dt is forced to 0 when the quality is 0 (which is usual on ground) so the ekf needs to ignore those values on ground to initialize properly the flow fusion.
- add filter convergence test
- move check for dt time max in setOpticalFlowData
- in the simulator, do not update dt as this is the sensor integration
period.
* EKF: Add comparison test for mag field fusion generated code
* EKF: convert mag field fusion to use SymPy generated code
* EKF: Add test comparison program for yaw fusion equations
* Stop setting 0 to 0
* Reduce if/else statement to only if
* EKF: more accurate implementation for sequential fusion of magnetometer data
* test: update change indicator
* Use matrix::SparseVector<float, 24, ...> for observation jacobian
* Adapt the auto code generation to allow for different bracket styles
* Add auto generated code for mag fusion
* Add generic computation of KHP
* Apply generic computation of KHP to mag fusion
* tests: update change indicator
* tests: update change indicator
Co-authored-by: kamilritz <kritz@ethz.ch>
If the user selects GPS yaw fusion but that there is no GPS yaw data in
the GPS message or if the fusion is rejected for some time, the GPS yaw
data is declared faulty and the fusion is stopped to allow an other
source of yaw aiding to start.
Daniel Agar