Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: fixed baro innovation gate when on ground with AIDING_NONE

Browse Source 
when on the ground without a position source we would disable the
innovation gate for the barometer. This meant that a single (or small
number of) really bad baro readings would be fused into the EKF,
causing it to destabilise

Fixes #11903
This commit is contained in:
Andrew Tridgell 2019-07-30 07:50:39 +10:00
parent 8e095d4a81
commit 098701ec79
1 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -602,10 +602,17 @@ void NavEKF2_core::FuseVelPosNED()
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio // calculate the innovation consistency test ratio
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); // when on ground we accept a larger test ratio to allow
// the filter to handle large switch on IMU bias errors
// without rejecting the height sensor
const float maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0;
// fail if the ratio is > maxTestRatio, but don't fail if bad IMU data
hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata;
// Fuse height data if healthy or timed out or in constant position mode // Fuse height data if healthy or timed out or in constant position mode
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { if (hgtHealth || hgtTimeout) {
// Calculate a filtered value to be used by pre-flight health checks // Calculate a filtered value to be used by pre-flight health checks
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
if (onGround) { if (onGround) {