fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 11:49:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: refactor wind reset functions

Browse Source
This commit is contained in:
bresch 2021-10-25 11:20:40 +02:00 committed by Mathieu Bresciani
parent 456dfcb4b9
commit d0f89f7fff
6 changed files with 69 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -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);
}

17
src/modules/ekf2/EKF/control.cpp
View File

@ -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();

52
src/modules/ekf2/EKF/covariance.cpp
View File

@ -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);
}

8
src/modules/ekf2/EKF/ekf.h
View File

@ -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();

14
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -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;
}

3
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

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