diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 77d3c277af..e6df5135f1 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -96,8 +96,7 @@ void Ekf::fuseAirspeed() const char *action_string = nullptr; if (update_wind_only) { - resetWindStates(); - resetWindCovariance(); + resetWindUsingAirspeed(); 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 - _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); + // 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.setZero(); - } + resetWindCovarianceUsingAirspeed(); + + _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); } diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c865d24d8e..0922c4d9cf 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/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 - resetWindStates(); - resetWindCovariance(); + resetWind(); } 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(); - resetWindCovariance(); + resetWind(); } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { fuseDrag(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 978db13cbe..405269614e 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/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 - // TODO: explicitly include the sideslip angle in the derivation - 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)); - 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)); - constexpr float R_yaw = sq(math::radians(10.0f)); + // Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py + // TODO: explicitly include the sideslip angle in the derivation + 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)); + 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)); + constexpr float R_yaw = sq(math::radians(10.0f)); - const float cos_yaw = cosf(euler_yaw); - const float sin_yaw = sinf(euler_yaw); + const float cos_yaw = cosf(euler_yaw); + const float sin_yaw = sinf(euler_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 Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_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 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 - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + // it is safer to remove all existing correlations to other states at this time + 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, 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; - 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(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) - + initial_wind_var_body_y * sin_yaw * cos_yaw; + 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); - // 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.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); - } + // 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); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9e2b655994..a7442fa097 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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 - void resetWindStates(); + // perform a reset of the wind states and related covariances + void resetWind(); + void resetWindUsingAirspeed(); + void resetWindToZero(); // check that the range finder data is continuous void updateRangeDataContinuity(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5f0f8962c3..a08c67498e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/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; } diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 3c5698b74b..8179413de0 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -140,8 +140,7 @@ void Ekf::fuseSideslip() const char *action_string = nullptr; if (update_wind_only) { - resetWindStates(); - resetWindCovariance(); + resetWind(); action_string = "wind"; } else {