ekf2: initialize wind covariance using symforce

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

@@ -175,6 +175,35 @@ def compute_airspeed_h_and_k(
 
     return (H.T, K)
 
+def compute_wind_init_and_cov_from_airspeed(
+        v_local: sf.V3,
+        heading: sf.Scalar,
+        airspeed: sf.Scalar,
+        v_var: sf.V3,
+        heading_var: sf.Scalar,
+        sideslip_var: sf.Scalar,
+        airspeed_var: sf.Scalar,
+) -> (sf.V2, sf.M22):
+
+    # Initialise wind states assuming horizontal flight
+    sideslip = sf.Symbol("beta")
+    wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip))
+    J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
+
+    R = sf.M55()
+    R[0,0] = v_var[0]
+    R[1,1] = v_var[1]
+    R[2,2] = heading_var
+    R[3,3] = sideslip_var
+    R[4,4] = airspeed_var
+
+    P = J * R * J.T
+
+    # Assume zero sideslip
+    P = P.subs({sideslip: 0.0})
+    wind = wind.subs({sideslip: 0.0})
+    return (wind, P)
+
 def predict_sideslip(
         state: State,
         epsilon: sf.Scalar
@@ -568,6 +597,7 @@ print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=["P_new"])
 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"])
+generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
 generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
 generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])

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

@@ -0,0 +1,73 @@
+// -----------------------------------------------------------------------------
+// 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_wind_init_and_cov_from_airspeed
+ *
+ * Args:
+ *     v_local: Matrix31
+ *     heading: Scalar
+ *     airspeed: Scalar
+ *     v_var: Matrix31
+ *     heading_var: Scalar
+ *     sideslip_var: Scalar
+ *     airspeed_var: Scalar
+ *
+ * Outputs:
+ *     wind: Matrix21
+ *     P_wind: Matrix22
+ */
+template <typename Scalar>
+void ComputeWindInitAndCovFromAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
+                                       const Scalar heading, const Scalar airspeed,
+                                       const matrix::Matrix<Scalar, 3, 1>& 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_wind = nullptr) {
+  // Total ops: 29
+
+  // Input arrays
+
+  // Intermediate terms (9)
+  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));
+  const Scalar _tmp4 = _tmp3 * sideslip_var;
+  const Scalar _tmp5 = _tmp3 * heading_var;
+  const Scalar _tmp6 = std::pow(_tmp0, Scalar(2));
+  const Scalar _tmp7 = _tmp0 * _tmp1;
+  const Scalar _tmp8 = -_tmp4 * _tmp7 - _tmp5 * _tmp7 + _tmp7 * 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_wind != nullptr) {
+    matrix::Matrix<Scalar, 2, 2>& _p_wind = (*P_wind);
+
+    _p_wind(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var(0, 0);
+    _p_wind(1, 0) = _tmp8;
+    _p_wind(0, 1) = _tmp8;
+    _p_wind(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var(1, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym