Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Rework opt flow terrain height logic

Browse Source
This commit is contained in:
Paul Riseborough 2019-02-25 09:54:29 +11:00 committed by Andrew Tridgell
parent 67d20f3b3c
commit aa6eee82f4
3 changed files with 15 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -48,16 +48,11 @@ void NavEKF2_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();
}
}
// Fuse optical flow data into the main filter
if (flowDataToFuse && tiltOK) {
@ -85,13 +80,18 @@ void NavEKF2_core::EstimateTerrainOffset()
// start performance timer
hal.util->perf_begin(_perf_TerrainOffset);
// calculate a predicted LOS rate squared
// horizontal velocity squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
// 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 || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
// 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;
@ -160,7 +160,7 @@ void NavEKF2_core::EstimateTerrainOffset()
}
}
if (fuseOptFlowData && !cantFuseFlowData) {
if (!cantFuseFlowData) {
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2f losPred; // predicted optical flow angular rate measurement

1
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -156,7 +156,6 @@ void NavEKF2_core::InitialiseVariables()
memset(&processNoise[0], 0, sizeof(processNoise));
flowDataValid = false;
rangeDataToFuse = false;
fuseOptFlowData = false;
Popt = 0.0f;
terrainState = 0.0f;
prevPosN = stateStruct.position.x;

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -965,7 +965,6 @@ private:
uint8_t ofStoreIndex; // OF data storage index
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
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)