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 : improve criteria used to inhibit scale factor estimation

Browse Source
This commit is contained in:
priseborough 2014-10-25 08:03:59 +11:00 committed by Andrew Tridgell
parent e09ff84218
commit 2ee1c549be
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3689,8 +3689,9 @@ void NavEKF::SetFlightAndFusionModes()
} else {
inhibitGndState = false;
}
// Don't update focal length offset state if there is no range finder or GPS, or we are flying slowly
if (!useRngFinder() || _fusionModeGPS == 3 || gndSpdSq < 5.0f) {
// Don't update focal length offset state if there is no range finder or GPS, or we are not flying fas enough to generate a useful LOS rate
float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
if (!useRngFinder() || _fusionModeGPS == 3 || losRateSq < 0.09f || gndSpdSq < 9.0f) {
fScaleInhibit = true;
} else {
fScaleInhibit = false;