added support for resetting wind states to external observation

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/ekf2/EKF/common.h

@@ -637,6 +637,7 @@ union information_event_status_u {
 		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
 		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
 		bool reset_pos_to_ext_obs       : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
+		bool reset_wind_to_ext_obs 	: 1; ///< 18 - true when wind states were reset to an external observation
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/ekf.cpp

@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include <ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h>
 
 #include <mathlib/mathlib.h>
 
@@ -299,6 +300,38 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 	ECL_INFO("reset position to external observation");
 	_information_events.flags.reset_pos_to_ext_obs = true;
 }
+void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy)
+{
+	const float wind_speed_constrained = math::max(wind_speed, 0.0f);
+
+	// wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO
+	const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F);
+
+	matrix::SquareMatrix<float, 2> P_wind;
+	Vector2f wind;
+
+	sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_accuracy, wind_direction_accuracy, &wind, &P_wind);
+
+	const Vector2f wind_var = P_wind.diag();
+
+	ECL_INFO("reset wind states to external observation");
+	_information_events.flags.reset_wind_to_ext_obs = true;
+
+	resetWindTo(wind, wind_var);
+}
+
+void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)
+{
+	_state.wind_vel = wind;
+
+	if (PX4_ISFINITE(wind_var(0))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0)));
+	}
+
+	if (PX4_ISFINITE(wind_var(1))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1)));
+	}
+}
 
 void Ekf::updateParameters()
 {

src/modules/ekf2/EKF/ekf.h

@@ -503,6 +503,16 @@ public:
 
 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
 
+	/**
+	* @brief Resets the wind states to an external observation
+	*
+	* @param wind_speed The wind speed in m/s
+	* @param wind_direction The azimuth (from true north) from where the wind is flowing from in degrees (0 to 360)
+	* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
+	* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in degrees
+	*/
+	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy);
+
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
@@ -788,6 +798,8 @@ private:
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
 
+	void resetWindTo(const Vector2f &wind, const Vector2f &wind_var);
+
 	bool isHeightResetRequired() const;
 
 	void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);

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

@@ -313,6 +313,23 @@ def compute_wind_init_and_cov_from_airspeed(
     P = P.subs({sideslip: 0.0})
     wind = wind.subs({sideslip: 0.0})
     return (wind, P)
+def compute_wind_init_and_cov_from_wind_speed_and_direction(
+        wind_speed: sf.Scalar,
+        wind_direction: sf.Scalar,
+        wind_speed_var: sf.Scalar,
+        wind_direction_var: sf.Scalar
+)-> (sf.V2, sf.M22):
+    wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction))
+    J = wind.jacobian([wind_speed, wind_direction])
+
+    R = sf.M22()
+    R[0,0] = wind_speed_var
+    R[1,1] = wind_direction_var
+
+    P = J * R * J.T
+
+    return (wind, P)
+
 
 def predict_sideslip(
         state: State,
@@ -687,6 +704,7 @@ if not args.disable_wind:
     generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
     generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
+    generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"])
 
 generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])

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

@@ -0,0 +1,65 @@
+// -----------------------------------------------------------------------------
+// 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_wind_speed_and_direction
+ *
+ * Args:
+ *     wind_speed: Scalar
+ *     wind_direction: Scalar
+ *     wind_speed_var: Scalar
+ *     wind_direction_var: Scalar
+ *
+ * Outputs:
+ *     wind: Matrix21
+ *     P_wind: Matrix22
+ */
+template <typename Scalar>
+void ComputeWindInitAndCovFromWindSpeedAndDirection(
+    const Scalar wind_speed, const Scalar wind_direction, const Scalar wind_speed_var,
+    const Scalar wind_direction_var, matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
+    matrix::Matrix<Scalar, 2, 2>* const P_wind = nullptr) {
+  // Total ops: 18
+
+  // Input arrays
+
+  // Intermediate terms (7)
+  const Scalar _tmp0 = std::cos(wind_direction);
+  const Scalar _tmp1 = std::sin(wind_direction);
+  const Scalar _tmp2 = std::pow(_tmp0, Scalar(2));
+  const Scalar _tmp3 = std::pow(_tmp1, Scalar(2));
+  const Scalar _tmp4 = wind_direction_var * std::pow(wind_speed, Scalar(2));
+  const Scalar _tmp5 = _tmp0 * _tmp1;
+  const Scalar _tmp6 = -_tmp4 * _tmp5 + _tmp5 * wind_speed_var;
+
+  // Output terms (2)
+  if (wind != nullptr) {
+    matrix::Matrix<Scalar, 2, 1>& _wind = (*wind);
+
+    _wind(0, 0) = _tmp0 * wind_speed;
+    _wind(1, 0) = _tmp1 * wind_speed;
+  }
+
+  if (P_wind != nullptr) {
+    matrix::Matrix<Scalar, 2, 2>& _p_wind = (*P_wind);
+
+    _p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4;
+    _p_wind(1, 0) = _tmp6;
+    _p_wind(0, 1) = _tmp6;
+    _p_wind(1, 1) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym