/* FILENAME : TerrainNav.cpp * AUTHOR : Debbie Meduna * DATE : 04/27/09 * * LAST MODIFIED : 11/30/10 * MODIFIED BY : Debbie Meduna * ----------------------------------------------------------------------------- * Modification History * ----------------------------------------------------------------------------- ******************************************************************************/ #include "TerrainNav.h" /****************************************************************************** TRANSITION MATRIX State 0: Well localized State 1: Localizing - medium uncertainty State 2: High uncertainty Trigger 0: North/East Variance < MIN_FILTER_VAR Trigger 1: MIN_FILTER_VAR+VAR_MARGIN < North/East Variance < MAX_FILTER_VAR- VAR_MARGIN and filter is either in state 0 or 2 Trigger 2: North/East Variance > MAX_FILTER_VAR Trigger 3: Missing valid range measurements for > MAX_MEAS_OUTAGE OR Vehicle does not have bottom lock for > MAX_VEL_OUTAGE Trigger 4: Vehicle has been over flat terrain for too long ******************************************************************************/ //normal case static int transitionMatrix[5][3] = {{0,0,1},{1,1,1},{1,2,2},{2,2,2},{2,2,2}}; //force reinit with well-converged //static int transitionMatrix[5][3] = {{2,0,1},{1,1,1},{1,2,2},{2,2,2},{2,2,2}}; TerrainNav::TerrainNav(char *mapName) { //initialize pointers this->mapFile = mapName; this->vehicleSpecFile = (char*)"mappingAUV_specs.cfg"; this->saveDirectory = NULL; this->tNavFilter = NULL; this->filterType = 1; this->octreeMap = NULL; //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs) { //initialize pointers this->mapFile = mapName; this->vehicleSpecFile = vehicleSpecs; this->saveDirectory = NULL; this->tNavFilter = NULL; this->filterType = 1; this->octreeMap = NULL; //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs, const int &filterType) { //initialize pointers this->mapFile = mapName; this->vehicleSpecFile = vehicleSpecs; this->saveDirectory = NULL; this->tNavFilter = NULL; this->filterType = filterType; this->octreeMap = NULL; //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs, const int &filterType, char *directory) { //initialize pointers this->mapFile = mapName; this->vehicleSpecFile = vehicleSpecs; this->saveDirectory = directory; this->tNavFilter = NULL; this->filterType = filterType; this->octreeMap = NULL; //initialize terrainNav private variables initVariables(); } TerrainNav::~TerrainNav() { if(tNavFilter != NULL) delete tNavFilter; tNavFilter = NULL; if(octreeMap != NULL) delete octreeMap; octreeMap = NULL; output("TerrainNav::Number of reinitializations: %i\n", numReinits); } void TerrainNav::estimatePose(poseT* estimate, const int &type) { //Cannot compute pose estimates if the filter motion has not been initialized if(tNavFilter->lastNavPose == NULL) { output("TerrainNav::Cannot compute pose estimate; motion has not been initialized.\n"); return; } switch(type) { case 1: tNavFilter->computeMLE(estimate); break; case 2: tNavFilter->computeMMSE(estimate); //If using a PMF, add on prior estNavOffset for attitude if(this->filterType == 1 && ALLOW_ATTITUDE_SEARCH) { estimate->phi = tNavFilter->lastNavPose->phi + estNavOffset.phi; estimate->theta = tNavFilter->lastNavPose->theta + estNavOffset.theta; estimate->psi = tNavFilter->lastNavPose->psi + estNavOffset.psi; estimate->wy = tNavFilter->lastNavPose->wy + estNavOffset.wz; estimate->wz = tNavFilter->lastNavPose->wz + estNavOffset.wz; } //Update current filter North/East variance tNavFilter->currVar[0] = estimate->covariance[0]; tNavFilter->currVar[1] = estimate->covariance[2]; //If estimate is confident, save INS predicted offset for reinit if(tNavFilter->currVar[0] < 100.0 && tNavFilter->currVar[1] < 100.0) { this->estNavOffset = *estimate; this->estNavOffset -= *this->tNavFilter->lastNavPose; } break; default: tNavFilter->computeMLE(estimate); } //TODO:***************NEED TO FIGURE THIS OUT!!!!! ********************* //*estimate = *tNavFilter->lastNavPose; //*estimate += estNavOffset; return; } void TerrainNav::measUpdate(measT* incomingMeas, const int &type) { //copy incoming measurement to current meas structure; measT currMeas; int i; currMeas = *incomingMeas; currMeas.dataType = type; //check validity of range data checkRangeValidity(currMeas); //If no motion updates have been performed (no navigation estimates included) //the measurement can not be added. if(tNavFilter->lastNavPose == NULL) { //check if our current measurements are valid this->lastMeasValid = false; for(i = 0; i < currMeas.numMeas; i++) { if(currMeas.measStatus[i]) { this->lastMeasValid = true; break; } } output("TerrainNav::Measurement type %i from time = %.2f sec. not included; " "vehicle motion has not been initialized.\n", currMeas.dataType, currMeas.time); this->lastMeasSuccess = false; return; } //check if vehicle is within correlation map before including measurement if(!USE_OCTREE){ if(!this->tNavFilter->withinRefMap()) { output("TerrainNav::Measurement type %i from time = %.2f sec. not included; " "vehicle is operating outside the given reference maps.\n", currMeas.dataType, currMeas.time); this->lastMeasSuccess = false; return; } } //Fill in the measurement variance based on range percent error computeMeasVariance(currMeas); //If the current measurement time is ahead of the latest navigation time, //then we need to wait for more recent navigation data before adding the //measurement. if((tNavFilter->lastNavPose->time < currMeas.time)) { //add current measurement to the measurement buffer this->waitingMeas[this->numWaitingMeas] = currMeas; this->numWaitingMeas++; output("TerrainNav::Delayed incorporating measurement type %i from time = %.2f" " sec.; waiting for more recent INS data...\n", currMeas.dataType, currMeas.time); return; } //If the current navigation time matches the measurement time, add the //measurement. Otherwise, ignore the measurement. if(tNavFilter->lastNavPose->time == currMeas.time) { this->lastMeasSuccess = tNavFilter->measUpdate(currMeas); if(this->lastMeasSuccess) { output("TerrainNav:: Measurement type %i successfully incorporated from" " time = %.2f sec.\n", currMeas.dataType, currMeas.time); lastMeasSuccessTime = currMeas.time; } return; } else output("TerrainNav::Did not incorporate measurement type %i from time" "= %.2f sec.; no INS pose data available. \n", currMeas.dataType, currMeas.time); return; } void TerrainNav::motionUpdate(poseT* incomingNav) { poseT currEstimate; double dt; currEstimate = *incomingNav; //try to initialize the filter if not already initialized if(tNavFilter->lastNavPose == NULL) { attemptInitFilter(currEstimate); return; } //check filter health before applying next motion update if(this->allowFilterReinits && !checkFilterHealth()) return; //estimate current acceleration based on delta v dt = currEstimate.time- tNavFilter->timeLastDvlValid; if(dt > 0) { currEstimate.ax = (currEstimate.vx - lastValidVel[0])/dt; currEstimate.ay = (currEstimate.vy - lastValidVel[1])/dt; currEstimate.az = (currEstimate.vz - lastValidVel[2])/dt; } //check validity of velocity data checkVelocityValidity(currEstimate); if(currEstimate.bottomLock && currEstimate.dvlValid) lastBottomLockTime = currEstimate.time; //if using a compass bias correction function, apply here: if(tNavFilter->compassBias != NULL) currEstimate.psi += -tNavFilter->compassBias->evalCompassBias(currEstimate.psi); //if dvl velocity data is bad, use last good velocity info if(!currEstimate.dvlValid) { currEstimate.vx = lastValidVel[0]; currEstimate.vy = lastValidVel[1]; currEstimate.vz = lastValidVel[2]; currEstimate.bottomLock = lastVelBotLock; } else { //if we just lost bottom lock, reset current velocity estimate if(lastVelBotLock && !currEstimate.bottomLock) { poseT currEst; tNavFilter->computeMMSE(&currEst); double attitude[3] = {currEst.phi, currEst.theta, currEst.psi}; //assuming attitude is ~constant over two time steps, can first //compute estimated current velocity in the body frame and then rotate //into inertial. (v_c = v_w - v_b, where v_c is current velocity, //v_w is water-relative velocity, v_b is bottom-relative velocity) double estWatVel[3] = {currEstimate.vx-lastValidVel[0], currEstimate.vy-lastValidVel[1], currEstimate.vz-lastValidVel[2]}; tNavFilter->applyVecRotation(attitude, estWatVel, tNavFilter->currentVel); //reinitialize water velocity estimate if searching over water velocity if(SEARCH_WAT_VEL) ((TNavParticleFilter *)tNavFilter)->reinitWatVelDist(estWatVel); } lastValidVel[0] = currEstimate.vx; lastValidVel[1] = currEstimate.vy; lastValidVel[2] = currEstimate.vz; lastVelBotLock = currEstimate.bottomLock; tNavFilter->timeLastDvlValid = currEstimate.time; } /* //convert measured velocity to vehicle frame (account for vehicle rotation //rate) int sensorIndx = 0; //look for the dvl sensor in the vehicle info if(tNavFilter->findMeasSensorIndex(1, sensorIndx)) { double rx, ry, rz; rx = tNavFilter->vehicle->T_sv[sensorIndx].dr[0]; ry = tNavFilter->vehicle->T_sv[sensorIndx].dr[1]; rz = tNavFilter->vehicle->T_sv[sensorIndx].dr[2]; currEstimate->vx += currEstimate->wy*rz - currEstimate->wz*ry; currEstimate->vy += currEstimate->wz*rx - currEstimate->wx*rz; currEstimate->vz += currEstimate->wx*ry - currEstimate->wy*rx; }*/ //if measurement is waiting to be added, update motion and add measurement if(outstandingMeas()) { //interpolate to find navigation corresponding to measurement poseT measPose; if(interpolatePoses(*tNavFilter->lastNavPose, currEstimate, measPose, waitingMeas[numWaitingMeas-1].time)) { for(int i = 0; i < this->numWaitingMeas; i++) { interpolatePoses(*tNavFilter->lastNavPose, currEstimate, measPose, waitingMeas[i].time); //check that we are not interpolating over a large time difference if(measPose.time - tNavFilter->lastNavPose->time > MAX_INTERP_TIME || currEstimate.time - measPose.time > MAX_INTERP_TIME) { this->lastMeasSuccess = false; output("TerrainNav::Measurement type %i not incorporated from time = " "%.2f sec.; No relevant navigation data available\n", waitingMeas[i].dataType, measPose.time); } else { //perform motion update in navigation filter tNavFilter->motionUpdate(measPose); //update lastNavPose variable *tNavFilter->lastNavPose = measPose; //incoporate measurement this->lastMeasSuccess = tNavFilter->measUpdate(waitingMeas[i]); if(this->lastMeasSuccess) { output("TerrainNav::Measurement type %i successfully incorporated " "from time = %.2f sec.\n", waitingMeas[i].dataType, measPose.time); lastMeasSuccessTime = measPose.time; } } } //set numWaitingMeas to zero because all measurements are included this->numWaitingMeas = 0; } } //perform motion update in navigation filter tNavFilter->motionUpdate(currEstimate); //update lastNavPose variable *tNavFilter->lastNavPose = currEstimate; return; } void TerrainNav::createFilter(const int filterType, const double *windowVar) { //ensure that the filter object is empty before creating. if(tNavFilter != NULL) delete tNavFilter; tNavFilter = NULL; //create new filter based on given filter type switch(filterType) { case 1: tNavFilter = new TNavPointMassFilter(this->mapFile, this->vehicleSpecFile, this->saveDirectory, windowVar); break; case 2: tNavFilter = new TNavParticleFilter(this->mapFile, this->vehicleSpecFile, this->saveDirectory, windowVar); break; case 3: tNavFilter = new TNavExtendKalmanFilter(this->mapFile, this->vehicleSpecFile, this->saveDirectory, windowVar); break; case 4: tNavFilter = new TNavSigmaPointFilter(this->mapFile, this->vehicleSpecFile, this->saveDirectory, windowVar); break; default: tNavFilter = new TNavPointMassFilter(this->mapFile, this->vehicleSpecFile, this->saveDirectory, windowVar); } this->filterType = filterType; output("TerrainNav::TNavFilter initialized with type %i\n", filterType); if(USE_OCTREE){ setOctreeMap(this->octreeMap); } } void TerrainNav::initVariables() { //initialize default variance for window size double windowVar[36] = {X_STDDEV_INIT*X_STDDEV_INIT, 0.0, Y_STDDEV_INIT*Y_STDDEV_INIT, 0.0, 0.0, Z_STDDEV_INIT*Z_STDDEV_INIT, 0.0, 0.0, 0.0, PHI_STDDEV_INIT*PHI_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, THETA_STDDEV_INIT*THETA_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, PSI_STDDEV_INIT*PSI_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT}; if(USE_OCTREE) { // TODO:******* Make separate function ***** pcl::PointCloud::Ptr cloud (new pcl::PointCloud); ifstream inputFile (this->mapFile); string line; vector xVec, yVec, zVec; float x, y, z; //float xmin, ymin, zmin; // Read input data if (inputFile.is_open()){ while ( inputFile.good() ){ getline (inputFile,line); stringstream sline; sline<> x >> y >> z; //Input file needs to be NED xVec.push_back(x); yVec.push_back(y); zVec.push_back(z); } } int NumPoints = xVec.size(); // Number of points in map // Set cloud properties cloud->is_dense = false; cloud->height = 1; cloud->width = NumPoints; cloud->points.resize (cloud->width * cloud->height); for (int ii=0; iipoints[ii].x = xVec[ii]; cloud->points[ii].y = yVec[ii]; cloud->points[ii].z = zVec[ii]; } this->octreeMap = (new pcl::octree::OctreePointCloudSearch(OCTREE_RESOLUTION)); this->octreeMap->setInputCloud(cloud); this->octreeMap->addPointsFromInputCloud(); } //create filter object createFilter(this->filterType, windowVar); filterState = 1; int i; lastMeasSuccess = false; numWaitingMeas = 0; lastValidVel[0] = 0.0; lastValidVel[1] = 0.0; lastValidVel[2] = 0.0; lastVelBotLock = false; lastMeasValid = false; lastMeasSuccessTime = -1.0; lastInitAttemptTime = -1.0; lastBottomLockTime = -1.0; for(i = 0; i < 4; i++) { lastValidRange[i] = 0; lastValidRangeTime[i] = 0; noValidRange[i] = true; } numReinits = 0; } void TerrainNav::attemptInitFilter(poseT& initEstimate) { bool withinMap; int i; static double windowVarInc[36] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; if(!USE_OCTREE){ withinMap = tNavFilter->withinValidMapRegion(initEstimate.x, initEstimate.y); } else{ withinMap = true; // TODO:*********** Need to actually check within map ***************** } checkVelocityValidity(initEstimate); //initialize the filter if not already initialized AND if within valid //region of the map AND vehicle has bottom lock AND vehicle has good //measurements AND vehicle is not on the surface if(withinMap && initEstimate.bottomLock && lastMeasValid && initEstimate.dvlValid && !initEstimate.gpsValid && initEstimate.z > 1) { //Incorporate increased search window to account for large initialization //waiting times for(i=0;i<36;i++) windowVarInc[i] *= windowVarInc[i]; tNavFilter->increaseInitSearchWin(windowVarInc); output("TerrainNav::attemptInitFilter is increasing Init Search Window by %f m\n",windowVarInc[0]); for(i=0;i<36;i++) windowVarInc[i] = 0.0; //lastInitAttemptTime = -1; //Initialize vehicle motion initMotion(initEstimate); } else { //increase search region for filter initialization if(lastInitAttemptTime > 0) { double dt = initEstimate.time - lastInitAttemptTime; tNavFilter->totalAttemptTime += dt; double dx = double (INCREASE_WINDOW)*(0.01*1.5*dt); windowVarInc[0] += dx; windowVarInc[2] += dx; } lastInitAttemptTime = initEstimate.time; if(!withinMap) { output("TerrainNav::Filter not initialized - vehicle is currently " "within a non-valid region of the reference map\n"); return; } if(initEstimate.gpsValid || initEstimate.z <= 1) { output("TerrainNav::Filter not initialized - vehicle is currently " "on the surface\n"); return; } if(!lastMeasValid) { output("TerrainNav::Filter not initialized - vehicle currently " "does not have good range measurements\n"); return; } output("TerrainNav::Filter not initialized - vehicle currently " "does not have bottom lock or good velocity data\n"); } } void TerrainNav::initMotion(poseT& initEstimate) { tNavFilter->lastNavPose = new poseT; //initialize velocity information lastValidVel[0] = initEstimate.vx; lastValidVel[1] = initEstimate.vy; lastValidVel[2] = initEstimate.vz; lastVelBotLock = initEstimate.bottomLock; tNavFilter->timeLastDvlValid = initEstimate.time; //set lastNavEstimate to initEstimate *tNavFilter->lastNavPose = initEstimate; //add on prior knowledge of navigation offset, if available initEstimate += estNavOffset; initEstimate.time = tNavFilter->lastNavPose->time; initEstimate.dvlValid = tNavFilter->lastNavPose->dvlValid; initEstimate.gpsValid = tNavFilter->lastNavPose->gpsValid; initEstimate.bottomLock = tNavFilter->lastNavPose->bottomLock; //initialize filter with initial pose estimate tNavFilter->initFilter(initEstimate); output("TerrainNav:: vehicle motion has been initialized\n"); return; } bool TerrainNav::interpolatePoses(const poseT& pose1, const poseT& pose2, poseT& newPose, const double newTime) { double deltaT, newDeltaT; deltaT = pose2.time - pose1.time; newPose.time = newTime; newDeltaT = newTime - pose1.time; if(newTime > pose2.time || newTime < pose1.time) return false; newPose.x = pose1.x + (pose2.x - pose1.x)*newDeltaT/deltaT; newPose.y = pose1.y + (pose2.y - pose1.y)*newDeltaT/deltaT; newPose.z = pose1.z + (pose2.z - pose1.z)*newDeltaT/deltaT; newPose.phi = pose1.phi + (pose2.phi - pose1.phi)*newDeltaT/deltaT; newPose.theta = pose1.theta+(pose2.theta - pose1.theta)*newDeltaT/deltaT; newPose.psi = pose1.psi + (pose2.psi - pose1.psi)*newDeltaT/deltaT; newPose.vx = pose1.vx + (pose2.vx - pose1.vx)*newDeltaT/deltaT; newPose.vy = pose1.vy + (pose2.vy - pose1.vy)*newDeltaT/deltaT; newPose.vz = pose1.vz + (pose2.vz - pose1.vz)*newDeltaT/deltaT; newPose.wx = pose1.wx + (pose2.wx - pose1.wx)*newDeltaT/deltaT; newPose.wy = pose1.wy + (pose2.wy - pose1.wy)*newDeltaT/deltaT; newPose.wz = pose1.wz + (pose2.wz - pose1.wz)*newDeltaT/deltaT; newPose.dvlValid = (pose1.dvlValid && pose2.dvlValid); newPose.gpsValid = (pose1.gpsValid && pose2.gpsValid); newPose.bottomLock = (pose1.bottomLock && pose2.bottomLock); return true; } void TerrainNav::computeMeasVariance(measT& currMeas) { int sensorIdx = 0; double perError; double rangeSq; int i; //find index of current measurement sensor. If none match, //return. if(!tNavFilter->findMeasSensorIndex(currMeas.dataType, sensorIdx)) return; perError = tNavFilter->vehicle->sensors[sensorIdx].percentRangeError; //if covariance vector not already intialized, intialize it if(currMeas.covariance == NULL) currMeas.covariance = new double[currMeas.numMeas]; //compute variance based on sensor's percent range error if(currMeas.dataType == 2) // mb measurement { for(i = 0; i < currMeas.numMeas; i++) { rangeSq = pow(currMeas.crossTrack[i],2)+pow(currMeas.alongTrack[i],2) +pow(currMeas.altitudes[i],2); currMeas.covariance[i] = rangeSq*pow(perError/100.0,2); } } else //dvl or altimeter measurement { for(i = 0; i < currMeas.numMeas; i++) currMeas.covariance[i] = pow(currMeas.ranges[i]*perError/100.0,2); } return; } void TerrainNav::checkVelocityValidity(poseT& currPose) { //check for out of range velocity data - if above max or equal to zero, // set dvlValid flag to false if((fabs(currPose.vx) > MAX_VEL) || (fabs(currPose.vx) <= 1e-4) || (fabs(currPose.vy) > MAX_VEL) || (fabs(currPose.vz) > MAX_VEL)) currPose.dvlValid = false; //check if predicted ground-based acceleration is too large //If this is the first velocity measurement, this check won't be performed //as lastVelBotLock = false initially; if(currPose.bottomLock && lastVelBotLock && currPose.z > 5) { if((fabs(currPose.ax) > MAX_ACCEL) || (fabs(currPose.ay) > MAX_ACCEL) || (fabs(currPose.az) > MAX_ACCEL)) { currPose.dvlValid = false; //update acceleration based on this new information currPose.ax = 0; currPose.ay = 0; currPose.az = 0; } } } void TerrainNav::checkRangeValidity(measT& currMeas) { int i,j; int numEqual = 0; double alpha, dr, dt; //this range check is only valid for DVL measurements if(currMeas.dataType != 1) return; for(i = 0; i < currMeas.numMeas; i++) { numEqual = 0; alpha = currMeas.ranges[i]; //check if more than two beams are equal if(i < 2) { for(j = i+1; j < currMeas.numMeas; j++) { if(fabs(alpha-currMeas.ranges[j]) < 0.1) numEqual++; } if(numEqual >= 2) { //if more than two beams are equal, throw out all beams for(j = 0; j < currMeas.numMeas; j++) currMeas.measStatus[j] = false; return; } } //check validity of each beam based on NaN or range value if(isnan(currMeas.ranges[i]) | (currMeas.ranges[i] >= MAX_RANGE) | (currMeas.ranges[i] <= MIN_RANGE)) currMeas.measStatus[i] = false; //check dr/dt for each beam if(currMeas.measStatus[i]) { if(noValidRange[i]) { noValidRange[i] = false; lastValidRange[i] = currMeas.ranges[i]; lastValidRangeTime[i] = currMeas.time; } else { dr = currMeas.ranges[i]-lastValidRange[i]; dt = currMeas.time - lastValidRangeTime[i]; if((dt > 0) && (fabs(dr/dt) > MAX_DRDT)) currMeas.measStatus[i] = false; else { lastValidRange[i] = currMeas.ranges[i]; lastValidRangeTime[i] = currMeas.time; } } } } } void TerrainNav::reinitFilter(int newState, bool lowInfoTransition) { int interpMapMethod = 1; bool interpMeasAttitude = true; double driftRate = 1; double windowVar[36] = {X_STDDEV_INIT*X_STDDEV_INIT, 0.0, Y_STDDEV_INIT*Y_STDDEV_INIT, 0.0, 0.0, Z_STDDEV_INIT*Z_STDDEV_INIT, 0.0, 0.0, 0.0, PHI_STDDEV_INIT*PHI_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, THETA_STDDEV_INIT*THETA_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, PSI_STDDEV_INIT*PSI_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT}; int i; poseT* temp = new poseT; //ensure that tNavFilter is non-empty before accessing and deleting if(tNavFilter != NULL) { //copy relevant data from current filter interpMapMethod = tNavFilter->interpMapMethod; interpMeasAttitude = tNavFilter->interpMeasAttitude; driftRate = tNavFilter->vehicle->driftRate; if(tNavFilter->lastNavPose != NULL && !lowInfoTransition) //trying to { tNavFilter->computeMMSE(temp); for (i=0; i<36; i++) { windowVar[i] = 1.0*temp->covariance[i]; } } //delete filter object delete tNavFilter; tNavFilter = NULL; } //create new filter switch(newState) { case 0: createFilter(2,windowVar); break; case 1: createFilter(2,windowVar); break; case 2: //If low information transition, reinitialize with uniform distribution //and initial search window (larger search window because using PMF) if(lowInfoTransition) { for(i=0;i<36;i++) { windowVar[i] *= 1.0; // want to use smaller value if starting w/ init window, but bigger if taking current var } } createFilter(1,windowVar); tNavFilter->setInitDistribType(0); //0 is uniform, 1 is gaussian output("TerrainNav::reinitializing filter from lowInfoTransition with uniform distribution \n"); break; default: createFilter(2,windowVar); } filterState = newState; //if not transitioning due to low information, initialize the filter //with a Gaussian distribution if(!lowInfoTransition) tNavFilter->setInitDistribType(1); //reset filter and terrainNav parameters setMapInterpMethod(interpMapMethod); setVehicleDriftRate(driftRate); setInterpMeasAttitude(interpMeasAttitude); numWaitingMeas = 0; lastMeasSuccessTime = -1.0; lastInitAttemptTime = -1.0; lastBottomLockTime = -1.0; numReinits++; delete temp; } bool TerrainNav::checkFilterHealth() { bool healthy = true; //1 is healthy, 0 is not healthy and //needs to be reinitialized bool lowInfoTransition = false; double currVarArea; currVarArea = (tNavFilter->currVar[0]+tNavFilter->currVar[1]); //double larger; //larger = max(tNavFilter->currVar[0], tNavFilter->currVar[1]); //initialize the state to change to as the current state int newState = filterState; //check if the length of time since last successful measurement exceeds //set maximum. Ensure that the filter has been initialized and that there //has been at least one successful measurement incorporated if (this->lastMeasSuccessTime > 0 && tNavFilter->lastNavPose != NULL && tNavFilter->lastNavPose->time - this->lastMeasSuccessTime > MAX_MEAS_OUTAGE) { newState = transitionMatrix[3][filterState]; healthy = false; output("TerrainNav::No valid range measurements for the past %.1f " "seconds. Re-initializing the filter.\n", MAX_MEAS_OUTAGE); lowInfoTransition = true; } //check if the length of time since last successful bottom velocity meas //exceeds set maximum. Ensure that the filter has been initialized and that //there has been at least one successful measurement incorporated if (this->lastBottomLockTime > 0 && tNavFilter->lastNavPose != NULL && tNavFilter->lastNavPose->time - this->lastBottomLockTime > MAX_VEL_OUTAGE) { newState = transitionMatrix[3][filterState]; healthy = false; lowInfoTransition = true; output("TerrainNav::No valid bottom velocity measurements for the past %.1f " "seconds. Re-initializing the filter.\n", MAX_VEL_OUTAGE); } //check if x/y uncertainty of the filter is below a set minimum. //potentially for switching filters in the future. // force reinit is for testing the fitler reinitialization only if(healthy && currVarArea < MIN_FILTER_VAR) //if(healthy && larger < MIN_FILTER_VAR) { newState = transitionMatrix[0][filterState]; output("CHECK TerrainNav:: TransitionMatrix is [%i %i %i]",transitionMatrix[0][0],transitionMatrix[0][1],transitionMatrix[0][2]); if(newState != filterState) { healthy = false; output("TerrainNav::North/East uncertainty has fallen below the " "minimum of %.1f m^2. Re-initializing the filter.\n", MIN_FILTER_VAR); lowInfoTransition = true; //only use for force reinit } } //check if measurement variance is below a set minimum /*if(tNavFilter->measVariance > 0 && tNavFilter->measVariance < MIN_MEAS_VAR) { healthy = false; output("TerrainNav::Measurement variance has fallen below the minimum " "of %f . Re-initializing the filter.\n", MIN_MEAS_VAR); } */ //check if the x/y uncertainty of the filter exceeds a set maximum if(healthy && currVarArea > MAX_FILTER_VAR) //if(healthy && larger > MAX_FILTER_VAR) { newState = transitionMatrix[2][filterState]; if(newState != filterState) { healthy = false; output("TerrainNav::North/East uncertainty has exceeded the " "maximum of %.1f m^2. Re-initializing the filter.\n", MAX_FILTER_VAR); output("North: %f East: %f Comb: %f. \n", tNavFilter->currVar[0], tNavFilter->currVar[1], currVarArea); } } //check if the uncertainty of the filter has changed enough to warrant //changing states if(healthy && currVarArea < MAX_FILTER_VAR-1*VAR_MARGIN && currVarArea > MIN_FILTER_VAR+0.1*VAR_MARGIN) /*if(healthy && larger < MAX_FILTER_VAR-1*VAR_MARGIN && larger > MIN_FILTER_VAR+0.1*VAR_MARGIN)*/ { newState = transitionMatrix[1][filterState]; if(newState != filterState) { healthy = false; output("TerrainNav::North/East uncertainty is within the margins " "of %.1f m^2. Re-initializing the filter.\n", VAR_MARGIN); output("North: %f East: %f Comb: %f. \n", tNavFilter->currVar[0], tNavFilter->currVar[1],currVarArea); } } //if filter is not healthy reinitialize if(!healthy) { reinitFilter(newState, lowInfoTransition); } return healthy; }