ekf2: migrate mag declination to SymForce

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

@@ -321,6 +321,22 @@ def compute_yaw_312_innov_var_and_h_alternate(
 
     return (innov_var, H.T)
 
+def compute_mag_declination_innov_innov_var_and_h(
+        state: VState,
+        P: MState,
+        meas: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+
+    meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon)
+    innov = meas_pred - meas
+
+    H = sf.V1(meas_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov, 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"])
@@ -335,3 +351,4 @@ generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var"
 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"])
+generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])

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

@@ -0,0 +1,74 @@
+// -----------------------------------------------------------------------------
+// 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_mag_declination_innov_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     meas: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov: Scalar
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                            const matrix::Matrix<Scalar, 24, 24>& P,
+                                            const Scalar meas, const Scalar R, const Scalar epsilon,
+                                            Scalar* const innov = nullptr,
+                                            Scalar* const innov_var = nullptr,
+                                            matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 23
+
+  // Input arrays
+
+  // Intermediate terms (4)
+  const Scalar _tmp0 =
+      epsilon * ((((state(16, 0)) > 0) - ((state(16, 0)) < 0)) + Scalar(0.5)) + state(16, 0);
+  const Scalar _tmp1 =
+      Scalar(1.0) / (std::pow(_tmp0, Scalar(2)) + std::pow(state(17, 0), Scalar(2)));
+  const Scalar _tmp2 = _tmp1 * state(17, 0);
+  const Scalar _tmp3 = _tmp0 * _tmp1;
+
+  // Output terms (3)
+  if (innov != nullptr) {
+    Scalar& _innov = (*innov);
+
+    _innov = -meas + std::atan2(state(17, 0), _tmp0);
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R - _tmp2 * (-P(16, 16) * _tmp2 + P(17, 16) * _tmp3) +
+                 _tmp3 * (-P(16, 17) * _tmp2 + P(17, 17) * _tmp3);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(16, 0) = -_tmp2;
+    _H(17, 0) = _tmp3;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym