ekf2: fix function to increase yaw variance

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

@@ -511,6 +511,25 @@ def quat_var_to_rot_var(
     rot_cov = J * P * J.T
     return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2])
 
+def yaw_var_to_lower_triangular_quat_cov(
+        state: VState,
+        yaw_var: sf.Scalar
+):
+    q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    attitude = state_to_rot3(state)
+    J = q.jacobian(attitude)
+
+    # Convert yaw uncertainty from NED to body frame
+    yaw_cov_ned = sf.M33.diag([0, 0, yaw_var])
+    adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself
+    yaw_cov_body = adjoint.T * yaw_cov_ned * adjoint
+
+    # Convert yaw (body) to quaternion parameter uncertainty
+    q_var = J * yaw_cov_body * J.T
+
+    # Generate lower trangle only and copy it to the upper part in implementation (produces less code)
+    return q_var.lower_triangle()
+
 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"])
@@ -533,3 +552,4 @@ generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var",
 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"])
+generate_px4_function(yaw_var_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"])

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

@@ -0,0 +1,101 @@
+// -----------------------------------------------------------------------------
+// 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: yaw_var_to_lower_triangular_quat_cov
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     yaw_var: Scalar
+ *
+ * Outputs:
+ *     q_cov_lower_triangle: Matrix44
+ */
+template <typename Scalar>
+void YawVarToLowerTriangularQuatCov(
+    const matrix::Matrix<Scalar, 24, 1>& state, const Scalar yaw_var,
+    matrix::Matrix<Scalar, 4, 4>* const q_cov_lower_triangle = nullptr) {
+  // Total ops: 136
+
+  // Input arrays
+
+  // Intermediate terms (39)
+  const Scalar _tmp0 = (Scalar(1) / Scalar(2)) * state(3, 0);
+  const Scalar _tmp1 =
+      -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
+  const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)) * yaw_var;
+  const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
+  const Scalar _tmp4 = 2 * state(2, 0);
+  const Scalar _tmp5 = 2 * state(1, 0);
+  const Scalar _tmp6 = -_tmp4 * state(0, 0) + _tmp5 * state(3, 0);
+  const Scalar _tmp7 = _tmp1 * _tmp6 * yaw_var;
+  const Scalar _tmp8 = -_tmp3 * _tmp7;
+  const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(2, 0);
+  const Scalar _tmp10 = _tmp4 * state(3, 0) + _tmp5 * state(0, 0);
+  const Scalar _tmp11 = _tmp10 * yaw_var;
+  const Scalar _tmp12 = _tmp1 * _tmp11;
+  const Scalar _tmp13 = _tmp12 * _tmp9;
+  const Scalar _tmp14 = std::pow(_tmp10, Scalar(2)) * yaw_var;
+  const Scalar _tmp15 = _tmp11 * _tmp6;
+  const Scalar _tmp16 = _tmp15 * _tmp3;
+  const Scalar _tmp17 = -_tmp0 * _tmp12;
+  const Scalar _tmp18 = std::pow(_tmp6, Scalar(2)) * yaw_var;
+  const Scalar _tmp19 = _tmp0 * _tmp7;
+  const Scalar _tmp20 = -_tmp15 * _tmp9;
+  const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * state(0, 0);
+  const Scalar _tmp22 = _tmp21 * _tmp7;
+  const Scalar _tmp23 = _tmp17 + _tmp2 * _tmp9 + _tmp22;
+  const Scalar _tmp24 = _tmp15 * _tmp21;
+  const Scalar _tmp25 = -_tmp0 * _tmp14 + _tmp13 + _tmp24;
+  const Scalar _tmp26 = _tmp7 * _tmp9;
+  const Scalar _tmp27 = _tmp0 * _tmp15;
+  const Scalar _tmp28 = _tmp18 * _tmp21 + _tmp26 - _tmp27;
+  const Scalar _tmp29 = _tmp12 * _tmp21;
+  const Scalar _tmp30 = _tmp19 - _tmp2 * _tmp3 + _tmp29;
+  const Scalar _tmp31 = _tmp12 * _tmp3;
+  const Scalar _tmp32 = _tmp14 * _tmp21 + _tmp27 - _tmp31;
+  const Scalar _tmp33 = _tmp0 * _tmp18 + _tmp24 + _tmp8;
+  const Scalar _tmp34 = _tmp2 * _tmp21 - _tmp26 + _tmp31;
+  const Scalar _tmp35 = _tmp14 * _tmp3 + _tmp20 + _tmp29;
+  const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp35;
+  const Scalar _tmp37 = _tmp16 - _tmp18 * _tmp9 + _tmp22;
+  const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp34;
+
+  // Output terms (1)
+  if (q_cov_lower_triangle != nullptr) {
+    matrix::Matrix<Scalar, 4, 4>& _q_cov_lower_triangle = (*q_cov_lower_triangle);
+
+    _q_cov_lower_triangle(0, 0) = -_tmp0 * (-_tmp0 * _tmp2 - _tmp13 + _tmp8) -
+                                  _tmp3 * (-_tmp18 * _tmp3 - _tmp19 + _tmp20) -
+                                  _tmp9 * (-_tmp14 * _tmp9 - _tmp16 + _tmp17);
+    _q_cov_lower_triangle(1, 0) = -_tmp0 * _tmp23 - _tmp25 * _tmp9 - _tmp28 * _tmp3;
+    _q_cov_lower_triangle(2, 0) = -_tmp0 * _tmp30 - _tmp3 * _tmp33 - _tmp32 * _tmp9;
+    _q_cov_lower_triangle(3, 0) = -_tmp0 * _tmp34 - _tmp3 * _tmp37 - _tmp36 * state(2, 0);
+    _q_cov_lower_triangle(0, 1) = 0;
+    _q_cov_lower_triangle(1, 1) = -_tmp0 * _tmp25 + _tmp21 * _tmp28 + _tmp23 * _tmp9;
+    _q_cov_lower_triangle(2, 1) = -_tmp0 * _tmp32 + _tmp21 * _tmp33 + _tmp30 * _tmp9;
+    _q_cov_lower_triangle(3, 1) = -_tmp0 * _tmp35 + _tmp21 * _tmp37 + _tmp34 * _tmp9;
+    _q_cov_lower_triangle(0, 2) = 0;
+    _q_cov_lower_triangle(1, 2) = 0;
+    _q_cov_lower_triangle(2, 2) = _tmp0 * _tmp33 + _tmp21 * _tmp32 - _tmp3 * _tmp30;
+    _q_cov_lower_triangle(3, 2) = _tmp0 * _tmp37 + _tmp36 * state(0, 0) - _tmp38 * state(1, 0);
+    _q_cov_lower_triangle(0, 3) = 0;
+    _q_cov_lower_triangle(1, 3) = 0;
+    _q_cov_lower_triangle(2, 3) = 0;
+    _q_cov_lower_triangle(3, 3) = _tmp36 * state(1, 0) - _tmp37 * _tmp9 + _tmp38 * state(0, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym