WindEstimator: use SymForce auto-generated function for airspeed fusion

Tools/setup/requirements.txt

@@ -26,3 +26,5 @@ requests
 setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
+symforce>=0.5.0
+sympy>=1.10.1

src/lib/wind_estimator/WindEstimator.cpp

@@ -141,37 +141,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 
 	_time_last_airspeed_fuse = time_now;
 
-	// assign helper variables
-	const float v_n = velI(0);
-	const float v_e = velI(1);
-	const float v_d = velI(2);
-
-	// calculate airspeed from ground speed and wind states (without scale)
-	const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
-					     (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);
-
-	// compute state observation matrix H
-	const float HH0 = airspeed_predicted_raw;
-	const float HH1 = _state(INDEX_TAS_SCALE) / math::max(HH0, 0.1f);
-
 	matrix::Matrix<float, 1, 3> H_tas;
-	H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
+	matrix::Matrix<float, 3, 1> K;
-	H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
-	H_tas(0, 2) = HH0;
-
-	// compute innovation covariance S
-	const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
-
-	// compute Kalman gain
-	matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
-	K /= S(0, 0);
-
-	// compute innovation
-	const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw;
-	_tas_innov = true_airspeed - airspeed_pred;
 
-	// innovation variance
+	sym::FuseAirspeed(velI, _state, _P, true_airspeed, _tas_var, FLT_EPSILON,
-	_tas_innov_var = S(0, 0);
+			  &H_tas, &K, &_tas_innov_var, &_tas_innov);
 
 	bool reinit_filter = false;
 	bool meas_is_rejected = false;

src/lib/wind_estimator/WindEstimator.hpp

@@ -41,6 +41,8 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+#include "python/generated/fuse_airspeed.h"
+
 using namespace time_literals;
 
 class WindEstimator

src/lib/wind_estimator/python/derivation.py

@@ -0,0 +1,50 @@
+import os
+from symforce import symbolic as sm
+from symforce import geo
+from symforce import typing as T
+
+def fuse_airspeed(
+        v_local: geo.V3,
+        state: geo.V3,
+        P: geo.M33,
+        airspeed: T.Scalar,
+        R: T.Scalar,
+        epsilon: T.Scalar
+) -> geo.V3:
+
+    vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
+    airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2]
+
+    innov = airspeed - airspeed_pred
+
+    H = geo.V1(airspeed_pred).jacobian(state)
+    innov_var = (H * P * H.transpose() + R)[0,0]
+
+    K = P * H.transpose() / sm.Max(innov_var, epsilon)
+
+    return (geo.V3(H), K, innov_var, innov)
+
+from symforce.codegen import Codegen, CppConfig
+
+codegen = Codegen.function(
+        fuse_airspeed,
+        output_names=["H", "K", "innov_var", "innov"],
+        config=CppConfig())
+metadata = codegen.generate_function(
+        output_dir="generated",
+        skip_directory_nesting=True)
+
+print("Files generated in {}:\n".format(metadata.output_dir))
+for f in metadata.generated_files:
+    print("  |- {}".format(os.path.relpath(f, metadata.output_dir)))
+
+# Replace cstdlib and Eigen functions by PX4 equivalents
+import fileinput
+
+with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file:
+    for line in file:
+        line = line.replace("std::max", "math::max")
+        line = line.replace("std", "matrix")
+        line = line.replace("Eigen", "matrix")
+        line = line.replace("matrix/Dense", "matrix/math.hpp")
+        print(line, end='')

src/lib/wind_estimator/python/generated/fuse_airspeed.h

@@ -0,0 +1,90 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/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: fuse_airspeed
+ *
+ * Args:
+ *     v_local: Matrix31
+ *     state: Matrix31
+ *     P: Matrix33
+ *     airspeed: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     H: Matrix13
+ *     K: Matrix31
+ *     innov_var: Scalar
+ *     innov: Scalar
+ */
+template <typename Scalar>
+void FuseAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
+                  const matrix::Matrix<Scalar, 3, 1>& state, const matrix::Matrix<Scalar, 3, 3>& P,
+                  const Scalar airspeed, const Scalar R, const Scalar epsilon,
+                  matrix::Matrix<Scalar, 1, 3>* const H = nullptr,
+                  matrix::Matrix<Scalar, 3, 1>* const K = nullptr, Scalar* const innov_var = nullptr,
+                  Scalar* const innov = nullptr) {
+  // Total ops: 56
+
+  // Input arrays
+
+  // Intermediate terms (11)
+  const Scalar _tmp0 = -state(0, 0) + v_local(0, 0);
+  const Scalar _tmp1 = -state(1, 0) + v_local(1, 0);
+  const Scalar _tmp2 = matrix::sqrt(Scalar(matrix::pow(_tmp0, Scalar(2)) + matrix::pow(_tmp1, Scalar(2)) +
+                                        epsilon + matrix::pow(v_local(2, 0), Scalar(2))));
+  const Scalar _tmp3 = state(2, 0) / _tmp2;
+  const Scalar _tmp4 = _tmp0 * _tmp3;
+  const Scalar _tmp5 = _tmp1 * _tmp3;
+  const Scalar _tmp6 = -P(0, 0) * _tmp4;
+  const Scalar _tmp7 = -P(1, 1) * _tmp5;
+  const Scalar _tmp8 = P(2, 2) * _tmp2;
+  const Scalar _tmp9 = R + _tmp2 * (-P(0, 2) * _tmp4 - P(1, 2) * _tmp5 + _tmp8) -
+                       _tmp4 * (-P(1, 0) * _tmp5 + P(2, 0) * _tmp2 + _tmp6) -
+                       _tmp5 * (-P(0, 1) * _tmp4 + P(2, 1) * _tmp2 + _tmp7);
+  const Scalar _tmp10 = Scalar(1.0) / (math::max<Scalar>(_tmp9, epsilon));
+
+  // Output terms (4)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 1, 3>& _H = (*H);
+
+    _H(0, 0) = -_tmp4;
+    _H(0, 1) = -_tmp5;
+    _H(0, 2) = _tmp2;
+  }
+
+  if (K != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _K = (*K);
+
+    _K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
+    _K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
+    _K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = _tmp9;
+  }
+
+  if (innov != nullptr) {
+    Scalar& _innov = (*innov);
+
+    _innov = -_tmp2 * state(2, 0) + airspeed;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym