// select fusion of velocity, position and height measurements void NavEKF2_core::SelectVelPosFusion() { // 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 && !posVelFusionDelayed) { posVelFusionDelayed = true; return; } else { posVelFusionDelayed = false; } // read GPS data from the sensor and check for new data in the buffer readGpsData(); gpsDataToFuse = RecallGPS(); // Determine if we need to fuse position and velocity data on this time step if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // Don't fuse velocity data if GPS doesn't support it if (frontend->_fusionModeGPS <= 1) { fuseVelData = true; } else { fuseVelData = false; } fusePosData = true; } else { fuseVelData = false; fusePosData = false; } // Select height data to be fused from the available baro, range finder and GPS sources selectHeightForFusion(); // If we are operating without any aiding, fuse in the last known position and zero velocity // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { gpsDataDelayed.vel.zero(); gpsDataDelayed.pos.x = lastKnownPositionNE.x; gpsDataDelayed.pos.y = lastKnownPositionNE.y; // only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration if (accNavMag < 9.8f) { fusePosData = true; fuseVelData = true; } else { fusePosData = false; fuseVelData = false; } } // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { FuseVelPosNED(); // clear the flags to prevent repeated fusion of the same data fuseVelData = false; fuseHgtData = false; fusePosData = false; } }
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; } } }
// Processes incoming read events. They can come from the server or // from the GPS device. void GpsClient::processEvent( fd_set *fdMask ) { if( ipcPort ) { int sfd = clientData.getSock(); if( sfd != -1 && FD_ISSET( sfd, fdMask ) ) { int loops = 0; // Try to process several messages from the client in order. That // is more effective as to wait for a new select call. while( loops++ < 32 ) { readServerMsg(); if( shutdown == true ) { break; } // Check, if more bytes are available in the receiver buffer because we // use blocking IO. int bytes = 0; // Number of bytes currently in the socket receiver buffer. if( ioctl( sfd, FIONREAD, &bytes) == -1 ) { qWarning() << "GpsClient::processEvent():" << "ioctl() returns with Errno=" << errno << "," << strerror(errno); break; } if( bytes <= 0 ) { break; } } } } if( fd != -1 && FD_ISSET( fd, fdMask ) ) { if( readGpsData() == false ) { // problem occurred, likely buffer overrun. we do restart the GPS // receiving. int error = errno; // Save errno closeGps(); if( error == ECONNREFUSED ) { // BT devices can reject a connection try. If we don't return here // we run in an endless loop. setShutdownFlag(true); } else { sleep(3); // reopen connection openGps( device.data(), ioSpeedDevice ); } } } }
// select fusion of velocity, position and height measurements void NavEKF2_core::SelectVelPosFusion() { // 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 && !posVelFusionDelayed) { posVelFusionDelayed = true; return; } else { posVelFusionDelayed = false; } // read GPS data from the sensor and check for new data in the buffer readGpsData(); gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); // Determine if we need to fuse position and velocity data on this time step if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // Don't fuse velocity data if GPS doesn't support it if (frontend->_fusionModeGPS <= 1) { fuseVelData = true; } else { fuseVelData = false; } fusePosData = true; // correct GPS data for position offset of antenna phase centre relative to the IMU Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; if (!posOffsetBody.is_zero()) { if (fuseVelData) { // TODO use a filtered angular rate with a group delay that matches the GPS delay Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); Vector3f velOffsetBody = angRate % posOffsetBody; Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); gpsDataDelayed.vel -= velOffsetEarth; } Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); gpsDataDelayed.pos.x -= posOffsetEarth.x; gpsDataDelayed.pos.y -= posOffsetEarth.y; gpsDataDelayed.hgt += posOffsetEarth.z; } } else { fuseVelData = false; fusePosData = false; } // we have GPS data to fuse and a request to align the yaw using the GPS course if (gpsYawResetRequest) { realignYawGPS(); } // Select height data to be fused from the available baro, range finder and GPS sources selectHeightForFusion(); // If we are operating without any aiding, fuse in the last known position // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { gpsDataDelayed.vel.zero(); gpsDataDelayed.pos.x = lastKnownPositionNE.x; gpsDataDelayed.pos.y = lastKnownPositionNE.y; fusePosData = true; fuseVelData = false; } // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { FuseVelPosNED(); // clear the flags to prevent repeated fusion of the same data fuseVelData = false; fuseHgtData = false; fusePosData = false; } }
// select fusion of velocity, position and height measurements void NavEKF2_core::SelectVelPosFusion() { // 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 && !posVelFusionDelayed) { posVelFusionDelayed = true; return; } else { posVelFusionDelayed = false; } // Check for data at the fusion time horizon extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms); // read GPS data from the sensor and check for new data in the buffer readGpsData(); gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); // Determine if we need to fuse position and velocity data on this time step if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // set fusion request flags if (frontend->_fusionModeGPS <= 1) { fuseVelData = true; } else { fuseVelData = false; } fusePosData = true; extNavUsedForPos = false; // correct GPS data for position offset of antenna phase centre relative to the IMU Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; if (!posOffsetBody.is_zero()) { // Don't fuse velocity data if GPS doesn't support it if (fuseVelData) { // TODO use a filtered angular rate with a group delay that matches the GPS delay Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); Vector3f velOffsetBody = angRate % posOffsetBody; Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); gpsDataDelayed.vel.x -= velOffsetEarth.x; gpsDataDelayed.vel.y -= velOffsetEarth.y; gpsDataDelayed.vel.z -= velOffsetEarth.z; } Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); gpsDataDelayed.pos.x -= posOffsetEarth.x; gpsDataDelayed.pos.y -= posOffsetEarth.y; gpsDataDelayed.hgt += posOffsetEarth.z; } // copy corrected GPS data to observation vector if (fuseVelData) { velPosObs[0] = gpsDataDelayed.vel.x; velPosObs[1] = gpsDataDelayed.vel.y; velPosObs[2] = gpsDataDelayed.vel.z; } velPosObs[3] = gpsDataDelayed.pos.x; velPosObs[4] = gpsDataDelayed.pos.y; } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // This is a special case that uses and external nav system for position extNavUsedForPos = true; activeHgtSource = HGT_SOURCE_EV; fuseVelData = false; fuseHgtData = true; fusePosData = true; velPosObs[3] = extNavDataDelayed.pos.x; velPosObs[4] = extNavDataDelayed.pos.y; velPosObs[5] = extNavDataDelayed.pos.z; // if compass is disabled, also use it for yaw if (!use_compass()) { extNavUsedForYaw = true; if (!yawAlignComplete) { extNavYawResetRequest = true; magYawResetRequest = false; gpsYawResetRequest = false; controlMagYawReset(); finalInflightYawInit = true; } else { fuseEulerYaw(); } } else { extNavUsedForYaw = false; } } else { fuseVelData = false; fusePosData = false; } // we have GPS data to fuse and a request to align the yaw using the GPS course if (gpsYawResetRequest) { realignYawGPS(); } // Select height data to be fused from the available baro, range finder and GPS sources selectHeightForFusion(); // if we are using GPS, check for a change in receiver and reset position and height if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) { // record the ID of the GPS that we are using for the reset last_gps_idx = gpsDataDelayed.sensor_idx; // Store the position before the reset so that we can record the reset delta posResetNE.x = stateStruct.position.x; posResetNE.y = stateStruct.position.y; // Set the position states to the position from the new GPS stateStruct.position.x = gpsDataNew.pos.x; stateStruct.position.y = gpsDataNew.pos.y; // Calculate the position offset due to the reset posResetNE.x = stateStruct.position.x - posResetNE.x; posResetNE.y = stateStruct.position.y - posResetNE.y; // Add the offset to the output observer states for (uint8_t i=0; i<imu_buffer_length; i++) { storedOutput[i].position.x += posResetNE.x; storedOutput[i].position.y += posResetNE.y; } outputDataNew.position.x += posResetNE.x; outputDataNew.position.y += posResetNE.y; outputDataDelayed.position.x += posResetNE.x; outputDataDelayed.position.y += posResetNE.y; // store the time of the reset lastPosReset_ms = imuSampleTime_ms; // If we are alseo using GPS as the height reference, reset the height if (activeHgtSource == HGT_SOURCE_GPS) { // Store the position before the reset so that we can record the reset delta posResetD = stateStruct.position.z; // write to the state vector stateStruct.position.z = -hgtMea; // Calculate the position jump due to the reset posResetD = stateStruct.position.z - posResetD; // Add the offset to the output observer states outputDataNew.position.z += posResetD; outputDataDelayed.position.z += posResetD; for (uint8_t i=0; i<imu_buffer_length; i++) { storedOutput[i].position.z += posResetD; } // store the time of the reset lastPosResetD_ms = imuSampleTime_ms; } } // If we are operating without any aiding, fuse in the last known position // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { velPosObs[3] = lastKnownPositionNE.x; velPosObs[4] = lastKnownPositionNE.y; fusePosData = true; fuseVelData = false; } // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { FuseVelPosNED(); // clear the flags to prevent repeated fusion of the same data fuseVelData = false; fuseHgtData = false; fusePosData = false; } }