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

AP_NavEKF : Adjust initial flow scale factor and limits

This commit is contained in:
priseborough 2014-11-01 11:00:56 +11:00 committed by Andrew Tridgell
parent 573633daf7
commit 84421e0a35
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

@ -2648,7 +2648,7 @@ void NavEKF::RunAuxiliaryEKF()
flowStates[i] -= K_RNG[i] * innovRng; flowStates[i] -= K_RNG[i] * innovRng;
} }
// constrain the states // constrain the states
flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f);
flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.1f); flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.1f);
// correct the covariance matrix // correct the covariance matrix
@ -2771,7 +2771,7 @@ void NavEKF::RunAuxiliaryEKF()
flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
} }
// constrain the states // constrain the states
flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f);
flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.1f); flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.1f);
// correct the covariance matrix // correct the covariance matrix