From aa6eee82f4d641b7b765bccf01f584dcb707f31e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 25 Feb 2019 09:54:29 +1100 Subject: [PATCH] AP_NavEKF2: Rework opt flow terrain height logic --- .../AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 30 +++++++++---------- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 1 - libraries/AP_NavEKF2/AP_NavEKF2_core.h | 1 - 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 3606ea6e61..1e6cd4fb6d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -48,23 +48,18 @@ void NavEKF2_core::SelectFlowFusion() flowDataValid = true; } - // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height - // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference - if ((flowDataToFuse || rangeDataToFuse) && tiltOK) { - // fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better) - fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse); + // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height + if (((flowDataToFuse && (frontend->_flowUseMask & (1<<1))) || rangeDataToFuse) && tiltOK) { // Estimate the terrain offset (runs a one state EKF) - if (frontend->_flowUseMask & (1<<1)) { - EstimateTerrainOffset(); - } + EstimateTerrainOffset(); } // Fuse optical flow data into the main filter if (flowDataToFuse && tiltOK) { if (frontend->_flowUseMask & (1<<0)) { - // Set the flow noise used by the fusion processes - R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); - // Fuse the optical flow X and Y axis data into the main filter sequentially + // Set the flow noise used by the fusion processes + R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); + // Fuse the optical flow X and Y axis data into the main filter sequentially FuseOptFlow(); } // reset flag to indicate that no new flow data is available for fusion @@ -85,13 +80,18 @@ void NavEKF2_core::EstimateTerrainOffset() // start performance timer hal.util->perf_begin(_perf_TerrainOffset); - // calculate a predicted LOS rate squared + // horizontal velocity squared float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); // don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable // don't fuse flow data if it exceeds validity limits - // don't update terrain state if we are using it as a height reference in the main filter - bool cantFuseFlowData = (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate)); + // don't update terrain offset if grpund is being used as the zero height datum in the main filter + bool cantFuseFlowData = (!(frontend->_flowUseMask & (1<<1)) + || gpsNotAvailable + || PV_AidingMode == AID_RELATIVE + || velHorizSq < 25.0f + || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate)); + if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) { // skip update inhibitGndState = true; @@ -160,7 +160,7 @@ void NavEKF2_core::EstimateTerrainOffset() } } - if (fuseOptFlowData && !cantFuseFlowData) { + if (!cantFuseFlowData) { Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes Vector2f losPred; // predicted optical flow angular rate measurement diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 2364905448..887a86a61e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -156,7 +156,6 @@ void NavEKF2_core::InitialiseVariables() memset(&processNoise[0], 0, sizeof(processNoise)); flowDataValid = false; rangeDataToFuse = false; - fuseOptFlowData = false; Popt = 0.0f; terrainState = 0.0f; prevPosN = stateStruct.position.x; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 764d8c5f11..c90f475d76 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -965,7 +965,6 @@ private: uint8_t ofStoreIndex; // OF data storage index bool flowDataToFuse; // true when optical flow data is ready for fusion bool flowDataValid; // true while optical flow data is still fresh - bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)