fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:43: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;
}
}