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

AP_NavEKF: Add parameter for max valid optical flow rate magnitude

This commit is contained in:
priseborough 2014-11-10 21:50:49 +11:00 committed by Andrew Tridgell
parent 6f8971d80a
commit 52c7e56a4a
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -344,6 +344,14 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @User: advanced
AP_GROUPINFO("RNG_GATE", 29, NavEKF, _rngInnovGate, 5),
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
// @Description: This parameter sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
// @Range: 1.0 - 4.0
// @Increment: 0.1
// @User: advanced
AP_GROUPINFO("MAX_FLOW", 30, NavEKF, _maxFlowRate, 2.5f),
// @Param: FALLBACK
// @DisplayName: Fallback strictness
// @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more.
@ -2779,7 +2787,7 @@ void NavEKF::RunAuxiliaryEKF()
// calculate the innovation consistency test ratio
auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(_flowInnovGate) * auxFlowObsInnovVar[obsIndex]);
if ((auxFlowTestRatio[obsIndex] < 1.0f) && (flowRadXY[obsIndex] < 2.0f)) {
if ((auxFlowTestRatio[obsIndex] < 1.0f) && (flowRadXY[obsIndex] < _maxFlowRate)) {
// correct the state
for (uint8_t i = 0; i < 2 ; i++) {
flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
@ -3020,7 +3028,7 @@ void NavEKF::FuseOptFlow()
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(_flowInnovGate) * varInnovOptFlow[obsIndex]);
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if ((flowTestRatio[obsIndex]) < 1.0f && (flowRadXY[obsIndex] < 2.0f)) {
if ((flowTestRatio[obsIndex]) < 1.0f && (flowRadXY[obsIndex] < _maxFlowRate)) {
// Reset the timer for velocity fusion timeout. This prevents a velocity timeout from being declared if we have to momentarily go into velocity hold mode.
velFailTime = imuSampleTime_ms;
// correct the state vector

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -395,6 +395,7 @@ private:
AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check
AP_Int8 _msecFLowDelay; // effective average delay of optical flow measurements rel to IMU (msec)
AP_Int8 _rngInnovGate; // Number of standard deviations applied to range finder innovation consistency check
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
// Tuning parameters