ekf2: calcRotVecVariances using SymForce

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

@@ -502,6 +502,15 @@ def compute_gravity_innov_var_and_k_and_h(
 
     return (innov, innov_var, K[0], K[1], K[2])
 
+def quat_var_to_rot_var(
+        state: VState,
+        P: MState,
+        epsilon: sf.Scalar
+):
+    J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state)
+    rot_cov = J * P * J.T
+    return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2])
+
 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"])
@@ -523,3 +532,4 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
+generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"])

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

@@ -0,0 +1,67 @@
+// -----------------------------------------------------------------------------
+// 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: quat_var_to_rot_var
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     rot_var: Matrix31
+ */
+template <typename Scalar>
+void QuatVarToRotVar(const matrix::Matrix<Scalar, 24, 1>& state,
+                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar epsilon,
+                     matrix::Matrix<Scalar, 3, 1>* const rot_var = nullptr) {
+  // Total ops: 61
+
+  // Input arrays
+
+  // Intermediate terms (17)
+  const Scalar _tmp0 = std::fabs(state(0, 0));
+  const Scalar _tmp1 = 1 - epsilon;
+  const Scalar _tmp2 = math::min<Scalar>(_tmp0, _tmp1);
+  const Scalar _tmp3 = 1 - std::pow(_tmp2, Scalar(2));
+  const Scalar _tmp4 = (((state(0, 0)) > 0) - ((state(0, 0)) < 0));
+  const Scalar _tmp5 = 2 * math::min<Scalar>(0, _tmp4) + 1;
+  const Scalar _tmp6 = _tmp5 * std::acos(_tmp2);
+  const Scalar _tmp7 = 2 * _tmp6 / std::sqrt(_tmp3);
+  const Scalar _tmp8 = _tmp4 * ((((-_tmp0 + _tmp1) > 0) - ((-_tmp0 + _tmp1) < 0)) + 1);
+  const Scalar _tmp9 = _tmp8 * state(1, 0);
+  const Scalar _tmp10 = _tmp2 * _tmp6 / (_tmp3 * std::sqrt(_tmp3));
+  const Scalar _tmp11 = _tmp5 / _tmp3;
+  const Scalar _tmp12 = _tmp10 * _tmp9 - _tmp11 * _tmp9;
+  const Scalar _tmp13 = _tmp10 * _tmp8;
+  const Scalar _tmp14 = _tmp11 * _tmp8;
+  const Scalar _tmp15 = _tmp13 * state(2, 0) - _tmp14 * state(2, 0);
+  const Scalar _tmp16 = _tmp13 * state(3, 0) - _tmp14 * state(3, 0);
+
+  // Output terms (1)
+  if (rot_var != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _rot_var = (*rot_var);
+
+    _rot_var(0, 0) = _tmp12 * (P(0, 0) * _tmp12 + P(1, 0) * _tmp7) +
+                     _tmp7 * (P(0, 1) * _tmp12 + P(1, 1) * _tmp7);
+    _rot_var(1, 0) = _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp7) +
+                     _tmp7 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp7);
+    _rot_var(2, 0) = _tmp16 * (P(0, 0) * _tmp16 + P(3, 0) * _tmp7) +
+                     _tmp7 * (P(0, 3) * _tmp16 + P(3, 3) * _tmp7);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym