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

AP_NavEKF3: always calculate optical flow innovations and variances

Browse Source
This commit is contained in:
Randy Mackay 2021-08-16 21:41:54 +09:00
parent 914a8dca49
commit 11847cfcf5
3 changed files with 27 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

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

12
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

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

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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