diff --git a/EKF/common.h b/EKF/common.h index 40332488ec..6948f71125 100644 --- a/EKF/common.h +++ b/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) diff --git a/EKF/control.cpp b/EKF/control.cpp index 498a78f387..4712714699 100644 --- a/EKF/control.cpp +++ b/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; } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index d6c8f0b9fb..e7f72a553c 100644 --- a/EKF/covariance.cpp +++ b/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 {