Added support for resetting wind states to external observation (#23277)

* added support for resetting wind states to external observation

Signed-off-by: RomanBapst <[email protected]>

* moved wind related functions into separate file

Signed-off-by: RomanBapst <[email protected]>

* added VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE

Signed-off-by: RomanBapst <[email protected]>

* correctly compute variances

Signed-off-by: RomanBapst <[email protected]>

* ekf2: implement wind reset

Signed-off-by: RomanBapst <[email protected]>

* only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning
and increase horizontal velocity variance to allow velocity states to move
towards solution that is aligned with the newly set wind

Signed-off-by: RomanBapst <[email protected]>

* only reset wind on ground

* still use wind reset using airspeed when it wasn't initialized

* exclude func for rover, change reset interface

* handle wind reset in drag-fusion

* replace state reset with variance reset in sideslip/drag fusion

* remove resetWind function

---------

Signed-off-by: RomanBapst <[email protected]>
Co-authored-by: Marco Hauswirth <[email protected]>

msg/VehicleCommand.msg

@@ -102,6 +102,7 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibrati
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 
 uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning
+uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
 
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)

src/modules/ekf2/CMakeLists.txt

@@ -214,6 +214,10 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS EKF/terrain_control.cpp)
 endif()
 
+if(CONFIG_EKF2_WIND)
+	list(APPEND EKF_SRCS EKF/wind.cpp)
+endif ()
+
 add_subdirectory(EKF)
 
 px4_add_module(

src/modules/ekf2/EKF/CMakeLists.txt

@@ -138,6 +138,10 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS terrain_control.cpp)
 endif()
 
+if(CONFIG_EKF2_WIND)
+	list(APPEND EKF_SRCS wind.cpp)
+endif ()
+
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_library(ecl_EKF
 	${EKF_SRCS}

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

@@ -62,6 +62,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		_control_status.flags.wind = false;
 	}
 
+	if (_control_status.flags.wind && _external_wind_init) {
+		_external_wind_init = false;
+	}
+
 #if defined(CONFIG_EKF2_GNSS)
 
 	// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
@@ -128,10 +132,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
 				resetVelUsingAirspeed(airspeed_sample);
 
-			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
-				// If starting wind state estimation, reset the wind states and covariances before fusing any data
-				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
+			} else if (!_external_wind_init
+				   && (!_control_status.flags.wind
+				       || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) {
 				resetWindUsingAirspeed(airspeed_sample);
+				_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 			}
 
 			_control_status.flags.wind = true;

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -48,9 +48,11 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
 	if ((_params.drag_ctrl > 0) && _drag_buffer) {
 
 		if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
-			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
-			resetWindToZero();
+
+			if (!_external_wind_init) {
+				resetWindCov();
+			}
 		}
 
 		dragSample drag_sample;

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -64,16 +64,12 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 			updateSideslip(_aid_src_sideslip);
 			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
 
-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			if (!_control_status.flags.wind) {
-				// activate the wind states
+			if (fuseSideslip(_aid_src_sideslip)) {
 				_control_status.flags.wind = true;
-				// reset the timeout timers to prevent repeated resets
-				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
-				resetWindToZero();
-			}
 
-			fuseSideslip(_aid_src_sideslip);
+			} else if (!_external_wind_init && !_control_status.flags.wind) {
+				resetWindCov();
+			}
 		}
 	}
 }
@@ -96,10 +92,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 			      math::max(_params.beta_innov_gate, 1.f)); // innovation gate
 }
 
-void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
+bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 {
 	if (sideslip.innovation_rejected) {
-		return;
+		return false;
 	}
 
 	// determine if we need the sideslip fusion to correct states other than wind
@@ -114,7 +110,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		const char *action_string = nullptr;
 
 		if (update_wind_only) {
-			resetWind();
+			resetWindCov();
 			action_string = "wind";
 
 		} else {
@@ -125,7 +121,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 
 		ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
 
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_sideslip = false;
@@ -150,4 +146,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	if (is_fused) {
 		sideslip.time_last_fuse = _time_delayed_us;
 	}
+
+	return is_fused;
 }

src/modules/ekf2/EKF/common.h

@@ -640,6 +640,7 @@ bool yaw_aligned_to_imu_gps     :
 		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/covariance.cpp

@@ -200,15 +200,15 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_WIND)
 
-	if (_control_status.flags.wind) {
-		// wind vel: add process noise
-		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
-					    1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
+	// wind vel: add process noise
+	if (!_external_wind_init) {
+		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f)
+					    * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
 		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
 
 		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
 			const unsigned i = State::wind_vel.idx + index;
-			P(i, i) += wind_vel_process_noise;
+			P(i, i) = fminf(P(i, i) + wind_vel_process_noise, sq(_params.initial_wind_uncertainty));
 		}
 	}
 
@@ -348,11 +348,3 @@ void Ekf::resetMagCov()
 	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_WIND)
-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
-#endif // CONFIG_EKF2_WIND

src/modules/ekf2/EKF/ekf.h

@@ -526,6 +526,18 @@ class Ekf final : public EstimatorInterface
 	bool 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) to where the wind is heading in radians
+	* @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 radians
+	*/
+	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy,
+					    float wind_direction_accuracy);
+	bool _external_wind_init{false};
+
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
@@ -782,7 +794,7 @@ class Ekf final : public EstimatorInterface
 
 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
-	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
+	bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
@@ -808,6 +820,8 @@ class Ekf final : public EstimatorInterface
 	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);
@@ -1095,8 +1109,6 @@ class Ekf final : public EstimatorInterface
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
-	// perform a reset of the wind states and related covariances
-	void resetWind();
 	void resetWindCov();
 	void resetWindToZero();
 #endif // CONFIG_EKF2_WIND

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -864,32 +864,6 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER
 
-#if defined(CONFIG_EKF2_WIND)
-void Ekf::resetWind()
-{
-#if defined(CONFIG_EKF2_AIRSPEED)
-
-	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
-		resetWindUsingAirspeed(_airspeed_sample_delayed);
-		return;
-	}
-
-#endif // CONFIG_EKF2_AIRSPEED
-
-	resetWindToZero();
-}
-
-void Ekf::resetWindToZero()
-{
-	ECL_INFO("reset wind to zero");
-
-	// If we don't have an airspeed measurement, then assume the wind is zero
-	_state.wind_vel.setZero();
-
-	resetWindCov();
-}
-
-#endif // CONFIG_EKF2_WIND
 
 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 {

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

@@ -314,6 +314,20 @@ def compute_wind_init_and_cov_from_airspeed(
     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.V2):
+    wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction))
+    H = wind.jacobian([wind_speed, wind_direction])
+    R = sf.Matrix.diag([wind_speed_var, wind_direction_var])
+
+    P = H * R * H.T
+    P_diag = sf.V2(P[0,0], P[1,1])
+    return (wind, P_diag)
+
 def predict_sideslip(
         state: State,
         epsilon: sf.Scalar
@@ -721,6 +735,7 @@ def compute_gravity_z_innov_var_and_h(
     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,61 @@
+// -----------------------------------------------------------------------------
+// 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: Matrix21
+ */
+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, 1>* const P_wind = nullptr) {
+  // Total ops: 14
+
+  // Input arrays
+
+  // Intermediate terms (5)
+  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));
+
+  // 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, 1>& _p_wind = (*P_wind);
+
+    _p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4;
+    _p_wind(1, 0) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym