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

AP_NavEKF2: Fix bug in flow fusion for terrain estimation 

Also remove unused class variables
This commit is contained in:
priseborough 2017-06-19 09:26:49 +10:00 committed by Francisco Ferreira
parent 0b26908359
commit b7e6e6f95f
2 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -188,7 +188,7 @@ void NavEKF2_core::EstimateTerrainOffset()
losPred = relVelSensor.length()/flowRngPred;
// calculate innovations
auxFlowObsInnov = losPred - sqrtf(sq(flowRadXYcomp[0]) + sq(flowRadXYcomp[1]));
auxFlowObsInnov = losPred - sqrtf(sq(ofDataDelayed.flowRadXYcomp[0]) + sq(ofDataDelayed.flowRadXYcomp[1]));
// calculate observation jacobian
float t3 = sq(q0);
@ -234,7 +234,7 @@ void NavEKF2_core::EstimateTerrainOffset()
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar);
// don't fuse if optical flow data is outside valid range
if (MAX(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) {
if (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) < frontend->_maxFlowRate) {
// correct the state
terrainState -= K_OPT * auxFlowObsInnov;

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -926,8 +926,6 @@ private:
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
float auxFlowObsInnovVar; // innovation variance for optical flow observations from 1-state terrain offset estimator
Vector2 flowRadXYcomp; // motion compensated optical flow angular rates(rad/sec)
Vector2 flowRadXY; // raw (non motion compensated) optical flow angular rates (rad/sec)
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)