From 11847cfcf52a0645e14056f3c1418b01cb4793d4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Aug 2021 21:41:54 +0900 Subject: [PATCH] AP_NavEKF3: always calculate optical flow innovations and variances --- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 19 +++++++++++-------- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 12 ++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 ++++- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 76f3be8387..9baea7c5f4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -54,12 +54,11 @@ void NavEKF3_core::SelectFlowFusion() // Fuse optical flow data into the main filter if (flowDataToFuse && tiltOK) { - if ((frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { - // 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(ofDataDelayed); - } + const bool fuse_optflow = (frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW); + // 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(ofDataDelayed, fuse_optflow); } } @@ -265,8 +264,10 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed) * The script file used to generate these and other equations in this filter can be found here: * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m * Requires a valid terrain height estimate. + * + * really_fuse should be true to actually fuse into the main filter, false to only calculate variances */ -void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) +void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse) { Vector24 H_LOS; Vector3F relVelSensor; @@ -426,6 +427,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) varInnovOptFlow[0] = t77; // calculate innovation for X axis observation + // flowInnovTime_ms will be updated when Y-axis innovations are calculated innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x; // calculate Kalman gains for X-axis observation @@ -603,6 +605,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) // calculate innovation for Y observation innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y; + flowInnovTime_ms = AP_HAL::millis(); // calculate Kalman gains for the Y-axis observation Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); @@ -664,7 +667,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed) flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * 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 && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) { + if (really_fuse && (flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) { // record the last time observations were accepted for fusion prevFlowFuseTime_ms = imuSampleTime_ms; // notify first time only diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index be21a92c1c..b71c792713 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -477,6 +477,18 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour variances = extNavVelVarInnov.tofloat(); return true; #endif // EK3_FEATURE_EXTERNAL_NAV + case AP_NavEKF_Source::SourceXY::OPTFLOW: + // check for timeouts + if (AP_HAL::millis() - flowInnovTime_ms > 500) { + return false; + } + innovations.x = innovOptFlow[0]; + innovations.y = innovOptFlow[1]; + innovations.z = 0; + variances.x = varInnovOptFlow[0]; + variances.y = varInnovOptFlow[1]; + variances.z = 0; + return true; default: // variances are not available for this source return false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 673d804840..39fad5a320 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -819,7 +819,8 @@ private: void EstimateTerrainOffset(const of_elements &ofDataDelayed); // fuse optical flow measurements into the main filter - void FuseOptFlow(const of_elements &ofDataDelayed); + // really_fuse should be true to actually fuse into the main filter, false to only calculate variances + void FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse); // Control filter mode changes void controlFilterModes(); @@ -1172,12 +1173,14 @@ private: uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec) Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2 Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) + uint32_t flowInnovTime_ms; // system time that optical flow innovations and variances were recorded (to detect timeouts) ftype Popt; // Optical flow terrain height state covariance (m^2) ftype terrainState; // terrain position state (m) ftype prevPosN; // north position at last measurement ftype prevPosE; // east position at last measurement ftype varInnovRng; // range finder observation innovation variance (m^2) ftype innovRng; // range finder observation innovation (m) + ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m) bool inhibitGndState; // true when the terrain position state is to remain constant uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks