wind_est: auto-generate initialization of state and cov matrix

src/lib/wind_estimator/WindEstimator.cpp

@@ -37,6 +37,7 @@
  */
 
 #include "WindEstimator.hpp"
+#include "python/generated/init_wind_using_airspeed.h"
 
 bool
 WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
@@ -44,34 +45,19 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
 {
 	if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
 
-		const float cos_heading = cosf(heading_rad);
-		const float sin_heading = sinf(heading_rad);
+		constexpr float initial_heading_var = sq(math::radians(INITIAL_HEADING_ERROR_DEG));
+		constexpr float initial_sideslip_var = sq(math::radians(INITIAL_BETA_ERROR_DEG));
 
-		// initialise wind states assuming zero side slip and horizontal flight
-		_state(INDEX_W_N) = velI(0) - tas_meas * cos_heading;
-		_state(INDEX_W_E) = velI(1) - tas_meas * sin_heading;
+		matrix::SquareMatrix<float, 2> P_wind_init;
+		matrix::Vector2f wind_init;
+
+		sym::InitWindUsingAirspeed(velI, heading_rad, tas_meas, hor_vel_variance, initial_heading_var, initial_sideslip_var,
+					   tas_variance,
+					   &wind_init, &P_wind_init);
+
+		_state.xy() = wind_init;
 		_state(INDEX_TAS_SCALE) = _scale_init;
-
-		constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG);
-		const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
-		constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG));
-
-		// rotate wind velocity into earth frame aligned with vehicle yaw
-		const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
-		const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
-
-		_P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading)
-					   + initial_wind_var_body_y * sq(sin_heading);
-		_P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance *
-					   (-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) -
-					   initial_wind_var_body_y * sin_heading * cos_heading;
-		_P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E);
-		_P(INDEX_W_E, INDEX_W_E) = tas_variance * sq(sin_heading) + heading_variance * sq(Wx * cos_heading - Wy * sin_heading) +
-					   initial_wind_var_body_y * sq(cos_heading);
-
-		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
-		_P(INDEX_W_N, INDEX_W_N) += hor_vel_variance;
-		_P(INDEX_W_E, INDEX_W_E) += hor_vel_variance;
+		_P.slice<2, 2>(0, 0) = P_wind_init;
 
 	} else {
 		// no airspeed available

src/lib/wind_estimator/WindEstimator.hpp

@@ -45,6 +45,7 @@
 
 #include "python/generated/fuse_airspeed.h"
 #include "python/generated/fuse_beta.h"
+#include "python/generated/init_wind_using_airspeed.h"
 
 using namespace time_literals;
 

src/lib/wind_estimator/python/derivation.py

@@ -73,6 +73,31 @@ def fuse_beta(
 
     return (geo.V3(H), K, innov_var, innov)
 
+def init_wind_using_airspeed(
+        v_local: geo.V3,
+        heading: T.Scalar,
+        airspeed: T.Scalar,
+        v_var: T.Scalar,
+        heading_var: T.Scalar,
+        sideslip_var: T.Scalar,
+        airspeed_var: T.Scalar,
+) -> (geo.V2, geo.M22):
+
+    # Initialise wind states assuming zero side slip and horizontal flight
+    wind = geo.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading))
+    # Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading
+    J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed])
+
+    R = geo.M55()
+    R[0,0] = v_var
+    R[1,1] = v_var
+    R[2,2] = heading_var
+    R[3,3] = sideslip_var
+    R[4,4] = airspeed_var
+
+    P = J * R * J.T
+
+    return (wind, P)
 
 def generatePx4Function(function_name, output_names):
     from symforce.codegen import Codegen, CppConfig
@@ -111,5 +136,7 @@ def generatePythonFunction(function_name, output_names):
             skip_directory_nesting=True)
 
 generatePx4Function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])
-generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])
 generatePx4Function(fuse_beta, output_names=["H", "K", "innov_var", "innov"])
+generatePx4Function(init_wind_using_airspeed, output_names=["wind", "P"])
+
+generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])

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

@@ -0,0 +1,69 @@
+// -----------------------------------------------------------------------------
+// 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: init_wind_using_airspeed
+ *
+ * Args:
+ *     v_local: Matrix31
+ *     heading: Scalar
+ *     airspeed: Scalar
+ *     v_var: Scalar
+ *     heading_var: Scalar
+ *     sideslip_var: Scalar
+ *     airspeed_var: Scalar
+ *
+ * Outputs:
+ *     wind: Matrix21
+ *     P: Matrix22
+ */
+template <typename Scalar>
+void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Scalar heading,
+                           const Scalar airspeed, const Scalar v_var, const Scalar heading_var,
+                           const Scalar sideslip_var, const Scalar airspeed_var,
+                           matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
+                           matrix::Matrix<Scalar, 2, 2>* const P = nullptr) {
+  // Total ops: 22
+
+  // Input arrays
+
+  // Intermediate terms (7)
+  const Scalar _tmp0 = std::cos(heading);
+  const Scalar _tmp1 = std::sin(heading);
+  const Scalar _tmp2 = std::pow(_tmp1, Scalar(2));
+  const Scalar _tmp3 = std::pow(airspeed, Scalar(2)) * sideslip_var;
+  const Scalar _tmp4 = std::pow(_tmp0, Scalar(2));
+  const Scalar _tmp5 = _tmp0 * _tmp1;
+  const Scalar _tmp6 = -_tmp3 * _tmp5 + _tmp5 * airspeed_var;
+
+  // Output terms (2)
+  if (wind != nullptr) {
+    matrix::Matrix<Scalar, 2, 1>& _wind = (*wind);
+
+    _wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0);
+    _wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0);
+  }
+
+  if (P != nullptr) {
+    matrix::Matrix<Scalar, 2, 2>& _P = (*P);
+
+    _P(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
+    _P(1, 0) = _tmp6;
+    _P(0, 1) = _tmp6;
+    _P(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym