adjustments for wind fusion

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 <bapstroman@gmail.com>

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

@@ -216,6 +216,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

@@ -127,7 +127,7 @@ 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)) {
+			} else if (!_control_status.flags.wind) {
 				// 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.
 				resetWindUsingAirspeed(airspeed_sample);

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

@@ -57,6 +57,10 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
 
 		if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
 			fuseDrag(drag_sample);
+
+			if (!_aid_src_drag.fused && !_control_status.flags.fuse_aspd && !_control_status.flags.fuse_beta) {
+				resetWind();
+			}
 		}
 	}
 }

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

@@ -64,16 +64,10 @@ 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
-				_control_status.flags.wind = true;
-				// reset the timeout timers to prevent repeated resets
-				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
+			if (!fuseSideslip(_aid_src_sideslip) && !_control_status.flags.wind) {
 				resetWindToZero();
 			}
-
-			fuseSideslip(_aid_src_sideslip);
+			_control_status.flags.wind = true;
 		}
 	}
 }
@@ -96,10 +90,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
@@ -125,7 +119,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;
@@ -149,4 +143,5 @@ 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

@@ -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/covariance.cpp

@@ -195,15 +195,14 @@ 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));
-		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
+	// 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));
+	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;
-		}
+	for (unsigned index = 0; index < State::wind_vel.dof; index++) {
+		const unsigned i = State::wind_vel.idx + index;
+		P(i, i) = fminf(P(i, i) + wind_vel_process_noise, _params.initial_wind_uncertainty);
 	}
 #endif // CONFIG_EKF2_WIND
 
@@ -334,11 +333,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

@@ -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;
@@ -762,7 +772,7 @@ private:
 
 	// 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)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -683,30 +683,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

@@ -298,16 +298,10 @@ def compute_wind_init_and_cov_from_airspeed(
     # 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])
+    H = 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
+    P = sf.Matrix.diag([v_var[0], v_var[1], heading_var, sideslip_var, airspeed_var])
+    P = H * P * H.T
 
     # Assume zero sideslip
     P = P.subs({sideslip: 0.0})

src/modules/ekf2/EKF/wind.cpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file wind.cpp
+ * Helper functions for wind states
+ */
+
+#include "ekf.h"
+
+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);
+	Vector2f wind = wind_speed_constrained * Vector2f(cosf(wind_direction_rad), sinf(wind_direction_rad));
+
+	ECL_INFO("reset wind states to external observation");
+	_information_events.flags.reset_wind_to_ext_obs = true;
+
+	Vector2f innov = _state.wind_vel - wind;
+	float R = sq(0.1f);
+	Vector2f innov_var{P(State::wind_vel.idx, State::wind_vel.idx) + R, P(State::wind_vel.idx + 1, State::wind_vel.idx + 1) + R};
+	const bool control_status_wind_prev = _control_status.flags.wind;
+	_control_status.flags.wind = true;
+	fuseDirectStateMeasurement(innov(0), innov_var(0), R, State::wind_vel.idx);
+	fuseDirectStateMeasurement(innov(1), innov_var(1), R, State::wind_vel.idx + 1);
+	_control_status.flags.wind = control_status_wind_prev;
+
+	// reset the horizontal velocity variances to allow the velocity states to be pulled towards
+	// a solution that is aligned with the newly set wind estimates
+	static constexpr float hor_vel_var = 25.0f;
+	P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_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::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));
+}
+
+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();
+}

src/modules/ekf2/EKF2.cpp

@@ -534,6 +534,13 @@ void EKF2::Run()
 							accuracy, timestamp_observation);
 				}
 			}
+
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
+				if (_ekf.control_status_flags().wind_dead_reckoning || !_ekf.control_status_flags().in_air) {
+					_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
+									    vehicle_command.param4);
+				}
+			}
 		}
 	}