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

AP_NavEKF3: 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 committed by Randy Mackay
parent cf1d3eac35
commit 89b6026cf9
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -564,10 +564,17 @@ void NavEKF3_core::FuseVelPosNED()
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; varInnovVelPos[5] = P[9][9] + 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]);
// 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 > 1, but don't fail if bad IMU data // fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); 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) {