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

AP_NavEKF2: Improve optical flow height estimation 

Updated derivation using sequential fusion of Y and X axis data.
This commit is contained in:
Paul Riseborough 2019-02-13 11:55:41 +11:00 committed by Andrew Tridgell
parent 2b8b53d6b2
commit a40208ebd4
5 changed files with 78 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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 {

130
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -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);
}
}
}

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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;

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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