ekf2: cleanup zero innovation heading fusion

- 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
This commit is contained in:
Daniel Agar
2022-07-19 11:58:27 -04:00
parent ed14151734
commit 39453405a0
11 changed files with 948 additions and 990 deletions
+1
View File
@@ -72,6 +72,7 @@ px4_add_module(
EKF/terrain_estimator.cpp
EKF/utils.cpp
EKF/vel_pos_fusion.cpp
EKF/zero_innovation_heading_update.cpp
EKF/zero_velocity_update.cpp
EKF2.cpp