ekf2: do not pre-compute airspeed Kalman gain

The generated code is not much faster than the simple matrix-vector
multiplication

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -44,7 +44,7 @@
 
 #include "ekf.h"
 
-#include <ekf_derivation/generated/compute_airspeed_h_and_k.h>
+#include <ekf_derivation/generated/compute_airspeed_h.h>
 #include <ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h>
 #include <ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h>
 
@@ -204,10 +204,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 
 	_fault_status.flags.bad_airspeed = false;
 
-	VectorState H; // Observation jacobian
-	VectorState K; // Kalman gain vector
-
-	sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K);
+	const VectorState H = sym::ComputeAirspeedH(_state.vector(), FLT_EPSILON);
+	VectorState K = P * H / aid_src.innovation_variance;
 
 	if (update_wind_only) {
 		const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);

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

@@ -268,10 +268,8 @@ def compute_airspeed_innov_and_innov_var(
 
     return (innov, innov_var)
 
-def compute_airspeed_h_and_k(
+def compute_airspeed_h(
         state: VState,
-        P: MTangent,
-        innov_var: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (VTangent, VTangent):
 
@@ -281,9 +279,7 @@ def compute_airspeed_h_and_k(
     airspeed_pred = vel_rel.norm(epsilon=epsilon)
     H = jacobian_chain_rule(airspeed_pred, state)
 
-    K = P * H.T / sf.Max(innov_var, epsilon)
-
-    return (H.T, K)
+    return H.T
 
 def compute_wind_init_and_cov_from_airspeed(
         v_local: sf.V3,
@@ -739,7 +735,7 @@ def compute_gravity_z_innov_var_and_h(
     generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
 
 if not args.disable_wind:
-    generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
+    generate_px4_function(compute_airspeed_h, output_names=None)
     generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
     generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])

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

@@ -0,0 +1,56 @@
+// -----------------------------------------------------------------------------
+// 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_airspeed_h
+ *
+ * Args:
+ *     state: Matrix25_1
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     res: Matrix24_1
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 24, 1> ComputeAirspeedH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                              const Scalar epsilon) {
+  // Total ops: 14
+
+  // Input arrays
+
+  // Intermediate terms (5)
+  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp1 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
+                                       epsilon + std::pow(state(6, 0), Scalar(2))),
+                                Scalar(Scalar(-1) / Scalar(2)));
+  const Scalar _tmp3 = _tmp1 * _tmp2;
+  const Scalar _tmp4 = _tmp0 * _tmp2;
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 24, 1> _res;
+
+  _res.setZero();
+
+  _res(3, 0) = _tmp3;
+  _res(4, 0) = _tmp4;
+  _res(5, 0) = _tmp2 * state(6, 0);
+  _res(21, 0) = -_tmp3;
+  _res(22, 0) = -_tmp4;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym