Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Add div0 protection to the IMU1_weighting calc

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
float R_hgt;
if (i == 2) {
R_hgt = sq(_gpsVertVelNoise);
R_hgt = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f));
} else {
R_hgt = sq(_gpsHorizVelNoise);
R_hgt = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f));
}
K1 += R_hgt / (R_hgt + sq(velInnov1[i]));
K2 += R_hgt / (R_hgt + sq(velInnov2[i]));