ekf2: new yaw derivation

Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -390,6 +390,24 @@ def compute_mag_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def compute_yaw_innov_var_and_h(
+        state: VState,
+        P: MTangent,
+        R: sf.Scalar
+) -> (sf.Scalar, VTangent):
+
+    state = vstate_to_state(state)
+    q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage())
+    r = sf.Quaternion.symbolic('r')
+    delta_q = q * r.conj() # create a quaternion error of the measurement at the origin
+    delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian
+
+    H = sf.V1(delta_meas_pred).jacobian(state)
+    H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
 def compute_yaw_321_innov_var_and_h(
         state: VState,
         P: MTangent,
@@ -678,6 +696,7 @@ if not args.disable_wind:
     generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
 
+generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h

@@ -0,0 +1,65 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_yaw_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix23_23
+ *     R: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix23_1
+ */
+template <typename Scalar>
+void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                            const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                            Scalar* const innov_var = nullptr,
+                            matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 36
+
+  // Input arrays
+
+  // Intermediate terms (5)
+  const Scalar _tmp0 = 2 * state(2, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0);
+  const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
+  const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) -
+                       std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) +
+                 _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) +
+                 _tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = _tmp2;
+    _h(1, 0) = _tmp3;
+    _h(2, 0) = _tmp4;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -35,6 +35,7 @@
 
 #include <ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h>
 #include <ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h>
+#include <ekf_derivation/generated/compute_yaw_innov_var_and_h.h>
 
 #include <mathlib/mathlib.h>
 
@@ -42,7 +43,7 @@
 bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
 {
 	VectorState H_YAW;
-	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
+	sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW);
 
 	return fuseYaw(aid_src_status, H_YAW);
 }
@@ -135,10 +136,5 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 
 void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
 {
-	if (shouldUse321RotationSequence(_R_to_earth)) {
-		sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-
-	} else {
-		sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-	}
+	sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
 }