From 35c82ef67f88be329276ba6a0a58163a7f5c22a9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 23 Feb 2019 08:11:36 +1100 Subject: [PATCH] AP_NavEKF3: Improve optical flow height estimation --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 5 +- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 161 ++++++++++-------- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 - libraries/AP_NavEKF3/AP_NavEKF3_core.h | 6 +- 5 files changed, 95 insertions(+), 80 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 0e6e425098..aabe0fb187 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -581,10 +581,11 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: FLOW_MASK // @DisplayName: Optical flow use bitmask - // @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator and/or the 1-state terrain height estimator. + // @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator. // @User: Advanced - // @Values: 0:None,1:Navgation,2:Terrain,3:Both + // @Values: 0:None,1:Navigation,2:Terrain // @Bitmask: 0:Navigation,1:Terrain + // @Range: 0 2 // @RebootRequired: True AP_GROUPINFO("FLOW_MASK", 54, NavEKF3, _flowUseMask, FLOW_USE_MASK_DEFAULT), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index b2cba6971f..3a9308a5c7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -51,24 +51,18 @@ void NavEKF3_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 (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 @@ -82,23 +76,25 @@ void NavEKF3_core::SelectFlowFusion() /* Estimation of terrain offset using a single state EKF The filter can fuse motion compensated optical flow rates and range finder measurements +Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator */ void NavEKF3_core::EstimateTerrainOffset() { // start performance timer hal.util->perf_begin(_perf_TerrainOffset); - // constrain height above ground to be above range measured on ground - float heightAboveGndEst = MAX((terrainState - stateStruct.position.z), rngOnGnd); - - // calculate a predicted LOS rate squared + // horizontal velocity squared float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); - float losRateSq = velHorizSq / sq(heightAboveGndEst); - // don't update terrain offset state if there is no range finder - // don't update terrain state if not generating enough LOS rate, or without GPS, as it is poorly observable - // 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 || losRateSq < 0.01f); + // 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 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; @@ -116,7 +112,7 @@ void NavEKF3_core::EstimateTerrainOffset() // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); - float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[6][6]; + float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5]; Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms; @@ -167,90 +163,111 @@ void NavEKF3_core::EstimateTerrainOffset() } } - if (fuseOptFlowData && !cantFuseFlowData) { + if (!cantFuseFlowData) { - Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes - float losPred; // predicted optical flow angular rate measurement + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + Vector2f losPred; // predicted optical flow angular rate measurement float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time float K_OPT; float H_OPT; + Vector2f auxFlowObsInnovVar; // predict range to centre of image - float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; + float flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z; // constrain terrain height to be below the vehicle - terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); // calculate relative velocity in sensor frame relVelSensor = prevTnb*stateStruct.velocity; // divide velocity by range, subtract body rates and apply scale factor to // get predicted sensed angular optical rates relative to X and Y sensor axes - losPred = norm(relVelSensor.x, relVelSensor.y)/flowRngPred; + losPred.x = relVelSensor.y / flowRngPred; + losPred.y = - relVelSensor.x / flowRngPred; // calculate innovations - auxFlowObsInnov = losPred - norm(ofDataDelayed.flowRadXYcomp.x, ofDataDelayed.flowRadXYcomp.y); + auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp; - // calculate observation jacobian - float t3 = sq(q0); - float t4 = sq(q1); - float t5 = sq(q2); - float t6 = sq(q3); - float t10 = q0*q3*2.0f; - float t11 = q1*q2*2.0f; - float t14 = t3+t4-t5-t6; - float t15 = t14*stateStruct.velocity.x; - float t16 = t10+t11; - float t17 = t16*stateStruct.velocity.y; - float t18 = q0*q2*2.0f; - float t19 = q1*q3*2.0f; - float t20 = t18-t19; - float t21 = t20*stateStruct.velocity.z; - float t2 = t15+t17-t21; - float t7 = t3-t4-t5+t6; - float t8 = stateStruct.position[2]-terrainState; - float t9 = 1.0f/sq(t8); - float t24 = t3-t4+t5-t6; - float t25 = t24*stateStruct.velocity.y; - float t26 = t10-t11; - float t27 = t26*stateStruct.velocity.x; - float t28 = q0*q1*2.0f; - float t29 = q2*q3*2.0f; - float t30 = t28+t29; - float t31 = t30*stateStruct.velocity.z; - float t12 = t25-t27+t31; - float t13 = sq(t7); - float t22 = sq(t2); - float t23 = 1.0f/(t8*t8*t8); - float t32 = sq(t12); - H_OPT = 0.5f*(t13*t22*t23*2.0f+t13*t23*t32*2.0f)/sqrtf(t9*t13*t22+t9*t13*t32); + // calculate observation jacobians + float t2 = q0*q0; + float t3 = q1*q1; + float t4 = q2*q2; + float t5 = q3*q3; + float t6 = stateStruct.position.z - terrainState; + float t7 = 1.0f / (t6*t6); + float t8 = q0*q3*2.0f; + float t9 = t2-t3-t4+t5; - // calculate innovation variances - auxFlowObsInnovVar = H_OPT*Popt*H_OPT + R_LOS; + // prevent the state variances from becoming badly conditioned + Popt = MAX(Popt,1E-6f); + + // calculate observation noise variance from parameter + float flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f)); + + // Fuse Y axis data + + // Calculate observation partial derivative + H_OPT = t7*t9*(-stateStruct.velocity.z*(q0*q2*2.0-q1*q3*2.0)+stateStruct.velocity.x*(t2+t3-t4-t5)+stateStruct.velocity.y*(t8+q1*q2*2.0)); + + // calculate innovation variance + auxFlowObsInnovVar.y = H_OPT * Popt * H_OPT + flow_noise_variance; // calculate Kalman gain - K_OPT = Popt*H_OPT/auxFlowObsInnovVar; + K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y; // calculate the innovation consistency test ratio - auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar); + auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y); // don't fuse if optical flow data is outside valid range - if (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) < frontend->_maxFlowRate) { + if (auxFlowTestRatio.y < 1.0f) { // correct the state - terrainState -= K_OPT * auxFlowObsInnov; + terrainState -= K_OPT * auxFlowObsInnov.y; // constrain the state - terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); + + // update intermediate variables used when fusing the X axis + t6 = stateStruct.position.z - terrainState; + t7 = 1.0f / (t6*t6); // correct the covariance Popt = Popt - K_OPT * H_OPT * Popt; - // prevent the state variances from becoming negative - Popt = MAX(Popt,0.0f); + // prevent the state variances from becoming badly conditioned + Popt = MAX(Popt,1E-6f); + } + + // fuse X axis data + H_OPT = -t7*t9*(stateStruct.velocity.z*(q0*q1*2.0+q2*q3*2.0)+stateStruct.velocity.y*(t2-t3+t4-t5)-stateStruct.velocity.x*(t8-q1*q2*2.0)); + + // calculate innovation variances + auxFlowObsInnovVar.x = H_OPT * Popt * H_OPT + flow_noise_variance; + + // calculate Kalman gain + K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x; + + // calculate the innovation consistency test ratio + auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x); + + // don't fuse if optical flow data is outside valid range + if (auxFlowTestRatio.x < 1.0f) { + + // correct the state + terrainState -= K_OPT * auxFlowObsInnov.x; + + // constrain the state + terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); + + // correct the covariance + Popt = Popt - K_OPT * H_OPT * Popt; + + // prevent the state variances from becoming badly conditioned + Popt = MAX(Popt,1E-6f); } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 63e9473fa5..8429408351 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -56,7 +56,7 @@ void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn gndOffset = terrainState; flowInnovX = innovOptFlow[0]; flowInnovY = innovOptFlow[1]; - auxInnov = auxFlowObsInnov; + auxInnov = norm(auxFlowObsInnov.x,auxFlowObsInnov.y); HAGL = terrainState - stateStruct.position.z; rngInnov = innovRng; range = rangeDataDelayed.rng; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 2adfa8991c..b6701d2b16 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -201,7 +201,6 @@ void NavEKF3_core::InitialiseVariables() memset(&nextP[0][0], 0, sizeof(nextP)); flowDataValid = false; rangeDataToFuse = false; - fuseOptFlowData = false; Popt = 0.0f; terrainState = 0.0f; prevPosN = stateStruct.position.x; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f7017a0fe7..ed2f42b00b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1016,9 +1016,7 @@ private: uint8_t ofStoreIndex; // OF data storage index bool flowDataToFuse; // true when optical flow data has 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 - 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 + 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) uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) @@ -1036,7 +1034,7 @@ private: 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 Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail - float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator + Vector2f auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail Vector2f flowGyroBias; // bias error of optical flow sensor gyro output