ekf2: refactor wind reset functions

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -96,8 +96,7 @@ void Ekf::fuseAirspeed()
 		const char *action_string = nullptr;
 
 		if (update_wind_only) {
-			resetWindStates();
+			resetWindUsingAirspeed();
-			resetWindCovariance();
 			action_string = "wind";
 
 		} else {
@@ -173,22 +172,38 @@ float Ekf::getTrueAirspeed() const
 	return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
 }
 
+void Ekf::resetWind()
+{
+	if (_control_status.flags.fuse_aspd) {
+		resetWindUsingAirspeed();
+
+	} else {
+		resetWindToZero();
+	}
+}
+
 /*
  * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 */
-void Ekf::resetWindStates()
+void Ekf::resetWindUsingAirspeed()
 {
 	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
 				? getEuler321Yaw(_state.quat_nominal)
 				: getEuler312Yaw(_state.quat_nominal);
 
-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
+	// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
-		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
+	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
-		_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
+	_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
-		_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
 
-	} else {
+	resetWindCovarianceUsingAirspeed();
-		// If we don't have an airspeed measurement, then assume the wind is zero
+
-		_state.wind_vel.setZero();
+	_time_last_arsp_fuse = _time_last_imu;
-	}
+}
+
+void Ekf::resetWindToZero()
+{
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
 }

src/modules/ekf2/EKF/control.cpp

@@ -1333,17 +1333,7 @@ void Ekf::controlAirDataFusion()
 			}
 
 		} else if (starting_conditions_passing) {
-			// 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 wind speed states and corresponding covariances
-				resetWindStates();
-				resetWindCovariance();
-			}
-
 			startAirspeedFusion();
-			_time_last_arsp_fuse = _time_last_imu;
 		}
 
 	} else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) {
@@ -1370,9 +1360,7 @@ void Ekf::controlBetaFusion()
 			_control_status.flags.wind = true;
 			// reset the timeout timers to prevent repeated resets
 			_time_last_beta_fuse = _time_last_imu;
-			// reset the wind speed states and corresponding covariances
+			resetWind();
-			resetWindStates();
-			resetWindCovariance();
 		}
 
 		fuseSideslip();
@@ -1389,8 +1377,7 @@ void Ekf::controlDragFusion()
 		if (!_control_status.flags.wind) {
 			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
-			resetWindStates();
+			resetWind();
-			resetWindCovariance();
 
 		} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
 			fuseDrag();

src/modules/ekf2/EKF/covariance.cpp

@@ -1140,39 +1140,33 @@ void Ekf::resetZDeltaAngBiasCov()
 	P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
 }
 
-void Ekf::resetWindCovariance()
+void Ekf::resetWindCovarianceUsingAirspeed()
 {
-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
+	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
-		// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
+	// TODO: explicitly include the sideslip angle in the derivation
-		// TODO: explicitly include the sideslip angle in the derivation
+	const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
-		const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
+	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
-		const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
-		constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
+	const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
-		const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
+	constexpr float R_yaw = sq(math::radians(10.0f));
-		constexpr float R_yaw = sq(math::radians(10.0f));
 
-		const float cos_yaw = cosf(euler_yaw);
+	const float cos_yaw = cosf(euler_yaw);
-		const float sin_yaw = sinf(euler_yaw);
+	const float sin_yaw = sinf(euler_yaw);
 
-		// rotate wind velocity into earth frame aligned with vehicle yaw
+	// rotate wind velocity into earth frame aligned with vehicle yaw
-		const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
+	const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
-		const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
+	const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
 
-		// it is safer to remove all existing correlations to other states at this time
+	// it is safer to remove all existing correlations to other states at this time
-		P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
+	P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
 
-		P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
+	P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
-		P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
+	P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
-			    initial_wind_var_body_y * sin_yaw * cos_yaw;
+		    initial_wind_var_body_y * sin_yaw * cos_yaw;
-		P(23, 22) = P(22, 23);
+	P(23, 22) = P(22, 23);
-		P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
+	P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
 
-		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
-		P(22, 22) += P(4, 4);
+	P(22, 22) += P(4, 4);
-		P(23, 23) += P(5, 5);
+	P(23, 23) += P(5, 5);
-
-	} else {
-		// without airspeed, start with a small initial uncertainty to improve the initial estimate
-		P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
-	}
 }

src/modules/ekf2/EKF/ekf.h

@@ -917,10 +917,12 @@ private:
 	void resetMagCov();
 
 	// perform a limited reset of the wind state covariances
-	void resetWindCovariance();
+	void resetWindCovarianceUsingAirspeed();
 
-	// perform a reset of the wind states
+	// perform a reset of the wind states and related covariances
-	void resetWindStates();
+	void resetWind();
+	void resetWindUsingAirspeed();
+	void resetWindToZero();
 
 	// check that the range finder data is continuous
 	void updateRangeDataContinuity();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1480,11 +1480,21 @@ void Ekf::loadMagCovData()
 	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
 }
 
-void Ekf::startAirspeedFusion() {
+void Ekf::startAirspeedFusion()
+{
+	// 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 wind speed states and corresponding covariances
+		resetWindUsingAirspeed();
+	}
+
 	_control_status.flags.fuse_aspd = true;
 }
 
-void Ekf::stopAirspeedFusion() {
+void Ekf::stopAirspeedFusion()
+{
 	_control_status.flags.fuse_aspd = false;
 }
 

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -140,8 +140,7 @@ void Ekf::fuseSideslip()
 			const char *action_string = nullptr;
 
 			if (update_wind_only) {
-				resetWindStates();
+				resetWind();
-				resetWindCovariance();
 				action_string = "wind";
 
 			} else {