diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index cbacdb91cd..c7c44a186e 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -234,7 +234,15 @@ void Ekf::resetWindStates() matrix::Euler euler321(_state.quat_nominal); float euler_yaw = euler321(2); - // estimate wind assuming zero sideslip - _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); + if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) { + // 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(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw); + + } else { + // If we don't have an airspeed measurement, then assume the wind is zero + _state.wind_vel(0) = 0.0f; + _state.wind_vel(1) = 0.0f; + + } } diff --git a/EKF/control.cpp b/EKF/control.cpp index 63fee6c5e2..c77da70579 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -750,24 +750,30 @@ void Ekf::controlRangeFinderFusion() void Ekf::controlAirDataFusion() { - // control activation and initialisation/reset of wind states + // control activation and initialisation/reset of wind states required for airspeed fusion - bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; - - // wind states are required to do airspeed fusion - if ((airspeed_timed_out && !_control_status.flags.fuse_beta) || _time_last_arsp_fuse == 0) { - // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid + // If both airspeed and sideslip fusion have timed out 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) { + // 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; } - if (_tas_data_ready) { - // if we have airspeed data to fuse and winds states are inactive, then - // they need to be activated and the corresponding states and covariances reset + // Always try to fuse airspeed data if available and we are in flight + if (_tas_data_ready && _control_status.flags.in_air) { + // 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 timout timer to prevent repeated resets + _time_last_arsp_fuse = _time_last_imu; + _time_last_beta_fuse = _time_last_imu; + // reset the wind speed states and corresponding covariances resetWindStates(); resetWindCovariance(); + } fuseAirspeed(); @@ -777,15 +783,34 @@ void Ekf::controlAirDataFusion() 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 + 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){ + _control_status.flags.wind = false; + + } + + // Attempt synthetic sideslip fusion at the prescribed rate if we are in-flight and fusion has been enabled bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air){ - fuseSideslip(); + // 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 timout timers to prevent repeated resets + _time_last_beta_fuse = _time_last_imu; + _time_last_arsp_fuse = _time_last_imu; + // reset the wind speed states and corresponding covariances + resetWindStates(); + resetWindCovariance(); + } + + fuseSideslip(); } - bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; - if(sideslip_timed_out && _control_status.flags.fuse_beta && _time_last_beta_fuse > 0.0f){ - _control_status.flags.wind = false; - } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 73802b4220..552386843b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -757,28 +757,39 @@ void Ekf::resetWindCovariance() zeroRows(P,22,23); zeroCols(P,22,23); - // calculate the wind speed and bearing - float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1))); - float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0)); + if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) { + // Use airspeed and zer sideslip assumption to set initial covariance values for wind states - // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle - // used to calculate the initial wind speed - float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); - float R_yaw = sq(0.1745f); + // calculate the wind speed and bearing + float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1))); + float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0)); - // calculate the variance and covariance terms for the wind states - float cos_yaw = cosf(yaw); - float sin_yaw = sinf(yaw); - float cos_yaw_2 = sq(cos_yaw); - float sin_yaw_2 = sq(sin_yaw); - float sin_cos_yaw = sin_yaw*cos_yaw; - float spd_2 = sq(spd); - P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2; - P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw; - P[23][22] = P[22][23]; - P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2; + // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle + // used to calculate the initial wind speed + float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); + float R_yaw = sq(0.1745f); + + // calculate the variance and covariance terms for the wind states + float cos_yaw = cosf(yaw); + float sin_yaw = sinf(yaw); + float cos_yaw_2 = sq(cos_yaw); + float sin_yaw_2 = sq(sin_yaw); + float sin_cos_yaw = sin_yaw*cos_yaw; + float spd_2 = sq(spd); + P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2; + P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw; + P[23][22] = P[22][23]; + P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2; + + // 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[23][23] += P[5][5]; + + } else { + // without airspeed, start with a small initial uncertainty to improve the initial estimate + P[22][22] = sq(5.0f); + P[23][23] = sq(5.0f); + + } - // 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[23][23] += P[5][5]; }