adjust for CI, save flash

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -37,6 +37,9 @@
  */
 
 #include "ekf.h"
+#include <ekf_derivation/generated/compute_ev_body_vel_hx.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
 
 void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
 			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
@@ -163,7 +166,43 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 
 			} else if (quality_sufficient) {
 				if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
-					fuseBodyVelocity(aid_src);
+
+					Vector3f body_vel_var;
+					VectorState H;
+					const float innov_gate = math::max(_params.ev_vel_innov_gate, 1.f);
+
+					estimator_aid_source1d_s current_aid_src;
+
+					for (uint8_t index = 0; index <= 2; index++) {
+						if (index == 0) {
+							sym::ComputeEvBodyVelHx(_state.vector(), &H);
+
+						} else if (index == 1) {
+							sym::ComputeEvBodyVelHy(_state.vector(), &H);
+
+						} else {
+							sym::ComputeEvBodyVelHz(_state.vector(), &H);
+						}
+
+						current_aid_src.innovation_variance = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index];
+						current_aid_src.innovation = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0);
+
+						setEstimatorAidStatusTestRatio(current_aid_src, innov_gate);
+
+						if (!current_aid_src.innovation_rejected) {
+							fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
+						}
+
+						aid_src.innovation[index] = current_aid_src.innovation;
+						aid_src.innovation_variance[index] = current_aid_src.innovation_variance;
+						aid_src.test_ratio[index] = current_aid_src.test_ratio;
+						aid_src.fused = current_aid_src.fused;
+						aid_src.innovation_rejected |= current_aid_src.innovation_rejected;
+
+						if (aid_src.fused) {
+							aid_src.time_last_fuse = _time_delayed_us;
+						}
+					}
 
 				} else {
 					fuseVelocity(aid_src);

src/modules/ekf2/EKF/ekf.h

@@ -829,7 +829,7 @@ class Ekf final : public EstimatorInterface
 	// 2d & 3d velocity fusion
 	void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
-	void fuseBodyVelocity(estimator_aid_source3d_s &aid_src);
+	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H);
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// terrain vertical position estimator

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

@@ -367,46 +367,32 @@ def predict_vel_body(
     R_to_body = state["quat_nominal"].inverse()
     return R_to_body * vel
 
-def compute_ev_body_vel_var_and_hx(
+def compute_ev_body_vel_hx(
         state: VState,
-        P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
     meas_pred = predict_vel_body(state)
     Hx = jacobian_chain_rule(meas_pred[0], state)
-    Hy = jacobian_chain_rule(meas_pred[1], state)
-    Hz = jacobian_chain_rule(meas_pred[2], state)
-    body_vel_var = sf.V3()
-    # attention: +R needs to be added to the variance in the calling function
-    body_vel_var[0] = (Hx * P * Hx.T)[0,0]
-    body_vel_var[1] = (Hy * P * Hy.T)[0,0]
-    body_vel_var[2] = (Hz * P * Hz.T)[0,0]
-    return (body_vel_var, Hx.T)
-
-def compute_ev_body_vel_var_and_hy(
+    return (Hx.T)
+
+def compute_ev_body_vel_hy(
         state: VState,
-        P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
     meas_pred = predict_vel_body(state)[1]
     Hy = jacobian_chain_rule(meas_pred, state)
-    # attention: +R needs to be added to the variance in the calling function
-    body_vel_var = (Hy * P * Hy.T)[0,0]
-    return (body_vel_var, Hy.T)
+    return (Hy.T)
 
-def compute_ev_body_vel_var_and_hz(
+def compute_ev_body_vel_hz(
         state: VState,
-        P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
     meas_pred = predict_vel_body(state)[2]
     Hz = jacobian_chain_rule(meas_pred, state)
-    # attention: +R needs to be added to the variance in the calling function
-    body_vel_var = (Hz * P * Hz.T)[0,0]
-    return (body_vel_var, Hz.T)
+    return (Hz.T)
 
 def predict_mag_body(state) -> sf.V3:
     mag_field_earth = state["mag_I"]
@@ -716,8 +702,8 @@ def compute_gravity_z_innov_var_and_h(
 generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
 generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
 generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
-generate_px4_function(compute_ev_body_vel_var_and_hx, output_names=["incomp_innov_var", "H"])
-generate_px4_function(compute_ev_body_vel_var_and_hy, output_names=["incomp_innov_var", "H"])
-generate_px4_function(compute_ev_body_vel_var_and_hz, output_names=["incomp_innov_var", "H"])
+generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])
 
 generate_px4_state(State, tangent_idx)

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

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// 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: compute_ev_body_vel_hx
+ *
+ * Args:
+ *     state: Matrix24_1
+ *
+ * Outputs:
+ *     H: Matrix23_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHx(const matrix::Matrix<Scalar, 24, 1>& state,
+                        matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 60
+
+  // Input arrays
+
+  // Intermediate terms (13)
+  const Scalar _tmp0 = 2 * state(5, 0);
+  const Scalar _tmp1 = 2 * state(6, 0);
+  const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0);
+  const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
+  const Scalar _tmp4 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0);
+  const Scalar _tmp5 = 4 * state(4, 0);
+  const Scalar _tmp6 = 2 * state(1, 0);
+  const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0);
+  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7;
+  const Scalar _tmp9 = 2 * state(0, 0);
+  const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0);
+  const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10;
+  const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2;
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0);
+    _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0);
+    _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0);
+    _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
+    _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0);
+    _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.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: compute_ev_body_vel_hy
+ *
+ * Args:
+ *     state: Matrix24_1
+ *
+ * Outputs:
+ *     H: Matrix23_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHy(const matrix::Matrix<Scalar, 24, 1>& state,
+                        matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 64
+
+  // Input arrays
+
+  // Intermediate terms (9)
+  const Scalar _tmp0 = 2 * state(4, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 =
+      -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0);
+  const Scalar _tmp5 = 4 * state(5, 0);
+  const Scalar _tmp6 = 2 * state(6, 0);
+  const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) -
+                       Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) +
+                       (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0);
+  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) -
+                       Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) +
+                       (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0);
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) =
+        -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0);
+    _h(1, 0) =
+        -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0);
+    _h(2, 0) =
+        -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0);
+    _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0);
+    _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
+    _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// 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: compute_ev_body_vel_hz
+ *
+ * Args:
+ *     state: Matrix24_1
+ *
+ * Outputs:
+ *     H: Matrix23_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHz(const matrix::Matrix<Scalar, 24, 1>& state,
+                        matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 60
+
+  // Input arrays
+
+  // Intermediate terms (13)
+  const Scalar _tmp0 = 2 * state(4, 0);
+  const Scalar _tmp1 = 2 * state(5, 0);
+  const Scalar _tmp2 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0);
+  const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0);
+  const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3;
+  const Scalar _tmp5 = 4 * state(6, 0);
+  const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0);
+  const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
+  const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0);
+  const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0);
+  const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8;
+  const Scalar _tmp11 = 2 * state(2, 0);
+  const Scalar _tmp12 = 2 * state(1, 0);
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9;
+    _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9;
+    _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0);
+    _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0);
+    _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0);
+    _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym