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

AP_NavEKF : Add div0 protection to the IMU1_weighting calc

Browse Source
This commit is contained in:
Paul Riseborough 2014-03-02 08:07:25 +11:00 committed by Andrew Tridgell
parent bd152d332c
commit 573b3210dd
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1545,9 +1545,9 @@ void NavEKF::FuseVelPosNED()
// observation error to normalise // observation error to normalise
float R_hgt; float R_hgt;
if (i == 2) { if (i == 2) {
R_hgt = sq(_gpsVertVelNoise); R_hgt = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f));
} else { } else {
R_hgt = sq(_gpsHorizVelNoise); R_hgt = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f));
} }
K1 += R_hgt / (R_hgt + sq(velInnov1[i])); K1 += R_hgt / (R_hgt + sq(velInnov1[i]));
K2 += R_hgt / (R_hgt + sq(velInnov2[i])); K2 += R_hgt / (R_hgt + sq(velInnov2[i]));