Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-13 11:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix terrain estimator innovation consistency check

Browse Source
This commit is contained in:
priseborough 2017-06-20 09:40:17 +10:00 committed by Randy Mackay
parent 647fb51688
commit fc36bd7874
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -145,9 +145,8 @@ void NavEKF2_core::EstimateTerrainOffset()
// calculate the innovation consistency test ratio // calculate the innovation consistency test ratio
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng); auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
// Check the innovation for consistency and don't fuse if > 5Sigma // Check the innovation test ratio and don't fuse if too large
if ((sq(innovRng)*SK_RNG) < 25.0f) if (auxRngTestRatio < 1.0f) {
{
// correct the state // correct the state
terrainState -= K_RNG * innovRng; terrainState -= K_RNG * innovRng;