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"])