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

AP_NavEKF3: Use sensor variance when checking for bad IMU

Browse Source
This commit is contained in:
Paul Riseborough 2021-07-16 16:27:35 +10:00 committed by Randy Mackay
parent 1cc7dc59a1
commit 7abecb4e0e
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -688,7 +688,7 @@ void NavEKF3_core::FuseVelPosNED()
const ftype hgtErr = stateStruct.position.z - velPosObs[5];
const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
// check if they are the same sign and both more than 3-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * R_OBS[5]) && (sq(velDErr) > 9.0f * R_OBS[2])) {
badIMUdata_ms = imuSampleTime_ms;
} else {
goodIMUdata_ms = imuSampleTime_ms;