ekf2: generate yaw fusion jacobians using symforce

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

@@ -253,6 +253,74 @@ def compute_mag_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def compute_yaw_321_innov_var_and_h(
+        state: VState,
+        P: MState,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_earth = quat_to_rot(q_att)
+    # Fix the singularity at pi/2 by inserting epsilon
+    meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
+def compute_yaw_321_innov_var_and_h_alternate(
+        state: VState,
+        P: MState,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_earth = quat_to_rot(q_att)
+    # Alternate form that has a singularity at yaw 0 instead of pi/2
+    meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
+def compute_yaw_312_innov_var_and_h(
+        state: VState,
+        P: MState,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_earth = quat_to_rot(q_att)
+    # Alternate form to be used when close to pitch +-pi/2
+    meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
+def compute_yaw_312_innov_var_and_h_alternate(
+        state: VState,
+        P: MState,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_earth = quat_to_rot(q_att)
+    # Alternate form to be used when close to pitch +-pi/2
+    meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -263,3 +331,7 @@ generate_px4_function(predict_covariance, output_names=["P_new"])
 generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
 generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
+generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])
+generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, 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"])

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

@@ -0,0 +1,75 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/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_312_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                               const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                               const Scalar epsilon, Scalar* const innov_var = nullptr,
+                               matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 75
+
+  // Input arrays
+
+  // Intermediate terms (11)
+  const Scalar _tmp0 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
+  const Scalar _tmp1 = 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));
+  const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
+  const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
+  const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
+  const Scalar _tmp5 = 2 / _tmp2;
+  const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
+  const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
+  const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) - _tmp5 * state(1, 0));
+  const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
+  const Scalar _tmp10 = _tmp6 * (_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R +
+                 _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp7 + P(2, 0) * _tmp8 + P(3, 0) * _tmp9) +
+                 _tmp7 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp7 + P(2, 1) * _tmp8 + P(3, 1) * _tmp9) +
+                 _tmp8 * (P(0, 2) * _tmp10 + P(1, 2) * _tmp7 + P(2, 2) * _tmp8 + P(3, 2) * _tmp9) +
+                 _tmp9 * (P(0, 3) * _tmp10 + P(1, 3) * _tmp7 + P(2, 3) * _tmp8 + P(3, 3) * _tmp9);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = _tmp10;
+    _H(1, 0) = _tmp7;
+    _H(2, 0) = _tmp8;
+    _H(3, 0) = _tmp9;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -0,0 +1,75 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/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_312_innov_var_and_h_alternate
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+                                        const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                                        const Scalar epsilon, Scalar* const innov_var = nullptr,
+                                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 83
+
+  // Input arrays
+
+  // Intermediate terms (11)
+  const Scalar _tmp0 = 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));
+  const Scalar _tmp1 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
+  const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
+  const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
+  const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
+  const Scalar _tmp5 = 2 / _tmp2;
+  const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
+  const Scalar _tmp7 = _tmp6 * (_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
+  const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
+  const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
+  const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(3, 0) - _tmp5 * state(0, 0));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var =
+        R - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp8 - P(2, 0) * _tmp7 - P(3, 0) * _tmp9) -
+        _tmp7 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp8 - P(2, 2) * _tmp7 - P(3, 2) * _tmp9) -
+        _tmp8 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp8 - P(2, 1) * _tmp7 - P(3, 1) * _tmp9) -
+        _tmp9 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp8 - P(2, 3) * _tmp7 - P(3, 3) * _tmp9);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = -_tmp10;
+    _H(1, 0) = -_tmp8;
+    _H(2, 0) = -_tmp7;
+    _H(3, 0) = -_tmp9;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -0,0 +1,75 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/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_321_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                               const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                               const Scalar epsilon, Scalar* const innov_var = nullptr,
+                               matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 74
+
+  // Input arrays
+
+  // Intermediate terms (11)
+  const Scalar _tmp0 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
+  const Scalar _tmp1 = 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));
+  const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
+  const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
+  const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
+  const Scalar _tmp5 = 2 / _tmp2;
+  const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
+  const Scalar _tmp7 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
+  const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(1, 0) + _tmp5 * state(2, 0));
+  const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
+  const Scalar _tmp10 = _tmp6 * (_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R +
+                 _tmp10 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + P(3, 3) * _tmp10) +
+                 _tmp7 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 + P(2, 2) * _tmp7 + P(3, 2) * _tmp10) +
+                 _tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 + P(2, 1) * _tmp7 + P(3, 1) * _tmp10) +
+                 _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + P(3, 0) * _tmp10);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = _tmp9;
+    _H(1, 0) = _tmp8;
+    _H(2, 0) = _tmp7;
+    _H(3, 0) = _tmp10;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -0,0 +1,75 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/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_321_innov_var_and_h_alternate
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+                                        const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                                        const Scalar epsilon, Scalar* const innov_var = nullptr,
+                                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 84
+
+  // Input arrays
+
+  // Intermediate terms (11)
+  const Scalar _tmp0 = 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));
+  const Scalar _tmp1 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
+  const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
+  const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
+  const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
+  const Scalar _tmp5 = 2 / _tmp2;
+  const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
+  const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(0, 0) - _tmp5 * state(3, 0));
+  const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
+  const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
+  const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var =
+        R - _tmp10 * (-P(0, 2) * _tmp9 - P(1, 2) * _tmp8 - P(2, 2) * _tmp10 - P(3, 2) * _tmp7) -
+        _tmp7 * (-P(0, 3) * _tmp9 - P(1, 3) * _tmp8 - P(2, 3) * _tmp10 - P(3, 3) * _tmp7) -
+        _tmp8 * (-P(0, 1) * _tmp9 - P(1, 1) * _tmp8 - P(2, 1) * _tmp10 - P(3, 1) * _tmp7) -
+        _tmp9 * (-P(0, 0) * _tmp9 - P(1, 0) * _tmp8 - P(2, 0) * _tmp10 - P(3, 0) * _tmp7);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = -_tmp9;
+    _H(1, 0) = -_tmp8;
+    _H(2, 0) = -_tmp10;
+    _H(3, 0) = -_tmp7;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym