aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve optical flow height estimation

Browse Source
This commit is contained in:
Paul Riseborough 2019-02-23 08:11:36 +11:00 committed by Andrew Tridgell
parent b848e231c7
commit 35c82ef67f
5 changed files with 95 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

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

161
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

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

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

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

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

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

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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