fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 19:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fix GNSS drift false alarm

Browse Source 
In SIH, the GNSS signal is zero-mean, but apparently not symmetric. The
issue is that saturating such a signal creates an artificial bias. This
made the check fail as the bias was above the threshold.
This commit is contained in:
bresch
2024-12-20 15:51:08 +01:00
committed by Daniel Agar
parent e9536cb30b
commit ab70ae3252
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+5 -3
View File
@@ -102,11 +102,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
// Apply a low pass filter
_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
// hdrift: calculate the horizontal drift speed and fail if too high
_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();