diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index bb733c630a..1f578501a0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -489,7 +489,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: TERR_GRAD // @DisplayName: Maximum terrain gradient - // @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference + // @Description: Specifies the maximum gradient of the terrain below the vehicle assumed when it is fusing range finder or optical flow to estimate terrain height. // @Range: 0 0.2 // @Increment: 0.01 // @User: Advanced diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index fd0c12342b..8017426d3e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -426,7 +426,6 @@ private: const uint8_t flowIntervalMax_ms = 100; // maximum allowable time between flow fusion events const uint16_t gndEffectTimeout_ms = 1000; // time in msec that ground effect mode is active after being activated const float gndEffectBaroScaler = 4.0f; // scaler applied to the barometer observation variance when ground effect mode is active - const uint8_t gndGradientSigma = 50; // RMS terrain gradient percentage assumed by the terrain height estimation const uint8_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec struct { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index b7b34eb715..3606ea6e61 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -78,23 +78,20 @@ void NavEKF2_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 NavEKF2_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 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 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 || losRateSq < 0.01f); + bool cantFuseFlowData = (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; @@ -112,7 +109,7 @@ void NavEKF2_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[5][5]; + float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5]; Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms; @@ -165,88 +162,109 @@ void NavEKF2_core::EstimateTerrainOffset() if (fuseOptFlowData && !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_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 048832c230..b81deba87e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -61,7 +61,7 @@ void NavEKF2_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_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 21a4821a28..764d8c5f11 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -966,8 +966,7 @@ private: 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 - 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) @@ -985,7 +984,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