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

ekf2: stricter GNSS vel rejected innovation override

Browse Source 
- in the special case of bad vertical acceleration detected
   (bad_acc_vertical) only allow overriding rejected GNSS 3D velocity
   if horizontal innovations are accepted
This commit is contained in:
Daniel Agar
2024-10-07 10:36:09 +02:00
committed by Mathieu Bresciani
parent 9238656a33
commit 9ddac762e3
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+7 -1
View File
@@ -204,7 +204,13 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
// but limit innovation to prevent spikes that could destabilise the filter
if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) {
bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical
&& (aid_src.test_ratio[2] > 1.f) // vz rejected
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
if (bad_acc_vz_rejected
&& (gnss_sample.sacc < _params.req_sacc)
) {
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
aid_src.innovation_rejected = false;