* EKF: add test comparison program for GPS yaw fusion equations
* EKF: convert GPS yaw fusion method to use SymPy generated equations
* Replace if/else with simple if
* EKF: fix formatting of GPS yaw fusion auto-code comparison test
* EKF: Use SparseVector type for GPS yaw fusion Jacobians
* EKF: Fix bug in GPS yaw derivation comparison check
* Add gps_yaw auto generated code file
Co-authored-by: kamilritz <kritz@ethz.ch>
When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.
As the name can be ambiguous, it gets renamed
"uncorrelateQuatFromOtherStates".
Also replace the loops storing the values and reapplying them by simply
zeroing two slices (this also saves 130B of flash).
This operation is expensive when done to the whole covariance matrix and unnecessary after covariance prediction because we calculate the upper diagonal and copy across so it is already symmetric.
Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only.
Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
kamilritz
Paul Riseborough
Paul Riseborough
bresch
Daniel Agar
Paul Riseborough
Carl Olsson