ekf2: calcRotVecVariances using SymForce

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include "python/ekf_derivation/generated/quat_var_to_rot_var.h"
 
 #include <mathlib/mathlib.h>
 #include <cstdlib>
@@ -882,46 +883,9 @@ void Ekf::updateVerticalDeadReckoningStatus()
 // calculate the variances for the rotation vector equivalent
 Vector3f Ekf::calcRotVecVariances() const
 {
-	Vector3f rot_var_vec;
-	float q0, q1, q2, q3;
-
-	if (_state.quat_nominal(0) >= 0.0f) {
-		q0 = _state.quat_nominal(0);
-		q1 = _state.quat_nominal(1);
-		q2 = _state.quat_nominal(2);
-		q3 = _state.quat_nominal(3);
-
-	} else {
-		q0 = -_state.quat_nominal(0);
-		q1 = -_state.quat_nominal(1);
-		q2 = -_state.quat_nominal(2);
-		q3 = -_state.quat_nominal(3);
-	}
-	float t2 = q0*q0;
-	float t3 = acosf(q0);
-	float t4 = -t2+1.0f;
-	float t5 = t2-1.0f;
-	if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
-		float t6 = 1.0f/t5;
-		float t7 = q1*t6*2.0f;
-		float t8 = 1.0f/powf(t4,1.5f);
-		float t9 = q0*q1*t3*t8*2.0f;
-		float t10 = t7+t9;
-		float t11 = 1.0f/sqrtf(t4);
-		float t12 = q2*t6*2.0f;
-		float t13 = q0*q2*t3*t8*2.0f;
-		float t14 = t12+t13;
-		float t15 = q3*t6*2.0f;
-		float t16 = q0*q3*t3*t8*2.0f;
-		float t17 = t15+t16;
-		rot_var_vec(0) = t10*(P(0,0)*t10+P(1,0)*t3*t11*2.0f)+t3*t11*(P(0,1)*t10+P(1,1)*t3*t11*2.0f)*2.0f;
-		rot_var_vec(1) = t14*(P(0,0)*t14+P(2,0)*t3*t11*2.0f)+t3*t11*(P(0,2)*t14+P(2,2)*t3*t11*2.0f)*2.0f;
-		rot_var_vec(2) = t17*(P(0,0)*t17+P(3,0)*t3*t11*2.0f)+t3*t11*(P(0,3)*t17+P(3,3)*t3*t11*2.0f)*2.0f;
-	} else {
-		rot_var_vec = 4.0f * P.slice<3,3>(1,1).diag();
-	}
-
-	return rot_var_vec;
+	Vector3f rot_var;
+	sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var);
+	return rot_var;
 }
 
 // initialise the quaternion covariances using rotation vector variances

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