Merge pull request #291 from PX4/pr-ekfWindEstBugFix

EKF: Fix bug affecting wind estimation for planes

EKF/common.h

@@ -219,6 +219,7 @@ struct parameters {
 	float switch_on_gyro_bias{0.1f};	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
 	float switch_on_accel_bias{0.2f};	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
 	float initial_tilt_err{0.1f};		// 1-sigma tilt error after initial alignment using gravity vector (rad)
+	float initial_wind_uncertainty{1.0f};	// 1-sigma initial uncertainty in wind velocity (m/s)
 
 	// position and velocity fusion
 	float gps_vel_noise{5.0e-1f};		// observation noise for gps velocity fusion (m/sec)

EKF/control.cpp

@@ -894,11 +894,10 @@ void Ekf::controlAirDataFusion()
 {
         // control activation and initialisation/reset of wind states required for airspeed fusion
 
-        // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
+	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
         bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
         bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
-                // if the airspeed or sideslip measurements have timed out for 10 seconds we declare the wind estimate to be invalid
 		_control_status.flags.wind = false;
 
 	}
@@ -928,7 +927,7 @@ void Ekf::controlBetaFusion()
 {
 	// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
 
-	// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
+	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
 	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
 	bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
 	if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
@@ -965,19 +964,22 @@ void Ekf::controlBetaFusion()
 
 void Ekf::controlDragFusion()
 {
-	if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) {
-		if (!_control_status.flags.wind) {
-			// reset the wind states and covariances when starting drag accel fusion
-			_control_status.flags.wind = true;
-			resetWindStates();
-			resetWindCovariance();
+	if (_params.fusion_mode & MASK_USE_DRAG ) {
+		if (_control_status.flags.in_air) {
+			if (!_control_status.flags.wind) {
+				// reset the wind states and covariances when starting drag accel fusion
+				_control_status.flags.wind = true;
+				resetWindStates();
+				resetWindCovariance();
 
-		} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
-			fuseDrag();
+			} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
+				fuseDrag();
+
+			}
+		} else {
+			_control_status.flags.wind = false;
 
 		}
-	} else {
-		_control_status.flags.wind = false;
 	}
 }
 

EKF/covariance.cpp

@@ -100,8 +100,8 @@ void Ekf::initialiseCovariance()
 	}
 
 	// wind
-	P[22][22] = 1.0f;
-	P[23][23] = 1.0f;
+	P[22][22] = sq(_params.initial_wind_uncertainty);
+	P[23][23] = sq(_params.initial_wind_uncertainty);
 
 }
 
@@ -204,7 +204,7 @@ void Ekf::predictCovariance()
 	float wind_vel_sig;
 
 	// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
-	if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f) {
+	if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f * sq(_params.initial_wind_uncertainty)) {
 		wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
 
 	} else {