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

ekf2: robustify wind init using airspeed

Browse Source 
Tests showed that sideslip fusion often starts just before airspeed
fusion, resetting the wind states to 0 instead of using the airspeed
data. A quick look at the wind uncertainty allows to know if the current
wind estimate is meaningful or if we should rather reset it using the
airspeed data.
This commit is contained in:
bresch
2023-04-08 14:12:21 +02:00
committed by Mathieu Bresciani
parent 8f520ab019
commit 17806a9608
4 changed files with 58 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+4 -1
View File
@@ -114,7 +114,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
ECL_INFO("starting airspeed fusion");
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
const Vector2f wind_var_xy = getWindVelocityVariance();
if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) {
// activate the wind states
_control_status.flags.wind = true;
// reset the wind speed states and corresponding covariances