fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 21:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Improve wind state reset logic

Browse Source 
Enables wind estimation without an airspeed sensor and enables synthetic sideslip to be used with an airspeed sensor for improved wind state estimation.
Wind states and covariances are reset differently depending on whether airspeed is available.
This commit is contained in:
Paul Riseborough
2016-11-05 12:19:01 +01:00
committed by Lorenz Meier
parent 2fedbc8a18
commit 951c8b77af
3 changed files with 82 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+11 -3
View File
@@ -234,7 +234,15 @@ void Ekf::resetWindStates()
matrix::Euler<float> 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;
}
}
EKF/control.cpp
+39 -14
View File
@@ -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;
}
}
EKF/covariance.cpp
+32 -21
View File
@@ -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];
}