// select fusion of optical flow measurements
void NavEKF2_core::SelectFlowFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
optFlowFusionDelayed = true;
return;
} else {
optFlowFusionDelayed = false;
}
// start performance timer
hal.util->perf_begin(_perf_FuseOptFlow);
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check
bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
// Constrain measurements to zero if we are using optical flow and are on the ground
if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
ofDataDelayed.flowRadXYcomp.zero();
ofDataDelayed.flowRadXY.zero();
flowDataValid = true;
}
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
PV_AidingMode = AID_NONE;
// reset the velocity
ResetVelocity();
// store the current position to be used to as a sythetic position measurement
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// reset the position
ResetPosition();
}
// 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 ((newDataFlow || newDataRng) && tiltOK) {
// fuse range data into the terrain estimator if available
fuseRngData = newDataRng;
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
fuseOptFlowData = (newDataFlow && !fuseRngData);
// Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset();
// Indicate we have used the range data
newDataRng = false;
}
// Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
{
// Set the flow noise used by the fusion processes
R_LOS = sq(max(frontend._flowNoise, 0.05f));
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();
// 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
newDataFlow = false;
}
// stop the performance timer
hal.util->perf_end(_perf_FuseOptFlow);
}
int main()
{
// open data files
pImuFile = fopen ("IMU.txt","r");
pMagFile = fopen ("MAG.txt","r");
pGpsFile = fopen ("GPS.txt","r");
pAhrsFile = fopen ("ATT.txt","r");
pAdsFile = fopen ("NTUN.txt","r");
pTimeFile = fopen ("timing.txt","r");
pStateOutFile = fopen ("StateDataOut.txt","w");
pEulOutFile = fopen ("EulDataOut.txt","w");
pCovOutFile = fopen ("CovDataOut.txt","w");
pRefPosVelOutFile = fopen ("RefVelPosDataOut.txt","w");
pVelPosFuseFile = fopen ("VelPosFuse.txt","w");
pMagFuseFile = fopen ("MagFuse.txt","w");
pTasFuseFile = fopen ("TasFuse.txt","w");
pGpsRawINFile = fopen ("GPSraw.txt","r");
pGpsRawOUTFile = fopen ("GPSrawOut.txt","w");
memset(gpsRaw, 0, sizeof(gpsRaw));
// read test data from files for first timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
readTimingData();
while (!endOfData)
{
if ((IMUmsec >= msecStartTime) && (IMUmsec <= msecEndTime))
{
// Initialise states, covariance and other data
if ((IMUmsec > msecAlignTime) && !statesInitialised && (GPSstatus == 3))
{
InitialiseFilter();
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised)
{
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j];
quat2eul(eulerEst, tempQuat);
for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
if (eulerDif[2] > pi) eulerDif[2] -= 2*pi;
if (eulerDif[2] < -pi) eulerDif[2] += 2*pi;
// store the predicted states for subsequent use by measurement fusion
StoreStates();
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))
{
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
// Fuse GPS Measurements
if (newDataGps && statesInitialised)
{
// Convert GPS measurements to Pos NE, hgt and Vel NED
calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD);
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
hgtMea = -posNED[2];
// set fusion flags
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
FuseVelposNED();
}
else
{
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised)
{
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data
}
else
{
fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f)
{
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
}
else
{
fuseVtasData = false;
}
// debug output
//printf("Euler Angle Difference = %3.1f , %3.1f , %3.1f deg\n", rad2deg*eulerDif[0],rad2deg*eulerDif[1],rad2deg*eulerDif[2]);
WriteFilterOutput();
}
// read test data from files for next timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
if (IMUmsec > msecEndTime)
{
CloseFiles();
endOfData = true;
}
}
}
