aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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); distanceTravelledSq = min(distanceTravelledSq, 100.0f);
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.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; timeAtLastAuxEKF_ms = imuSampleTime_ms;
} }
@ -3676,8 +3676,8 @@ void NavEKF::SetFlightAndFusionModes()
} else { } else {
inhibitGndState = false; inhibitGndState = false;
} }
// Don't update focal length offset state if there is no range finder or flying at low velocity // Don't update focal length offset state if there is no range finder
if (!useRngFinder() || !highGndSpdStage2) { if (!useRngFinder()) {
fScaleInhibit = true; fScaleInhibit = true;
} else { } else {
fScaleInhibit = false; fScaleInhibit = false;