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 : improvements to range finder fusion

Browse Source
This commit is contained in:
priseborough 2014-10-16 19:15:11 +11:00 committed by Andrew Tridgell
parent 1222559da0
commit 79698f7742
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2536,7 +2536,7 @@ void NavEKF::RunAuxiliaryEKF()
}
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(1.0f * timeLapsed);
Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(2.0f * timeLapsed);
timeAtLastAuxEKF_ms = imuSampleTime_ms;
}
@ -3676,8 +3676,8 @@ void NavEKF::SetFlightAndFusionModes()
} else {
inhibitGndState = false;
}
// Don't update focal length offset state if there is no range finder or flying at low velocity
if (!useRngFinder() || !highGndSpdStage2) {
// Don't update focal length offset state if there is no range finder
if (!useRngFinder()) {
fScaleInhibit = true;
} else {
fScaleInhibit = false;