/* FILENAME : TerrainNav.cpp * AUTHOR : Debbie Meduna * DATE : 04/27/09 * * LAST MODIFIED : 11/30/10 * MODIFIED BY : Debbie Meduna * ----------------------------------------------------------------------------- * Modification History * ----------------------------------------------------------------------------- ******************************************************************************/ #include #include #include "TerrainNav.h" #include "TNavConfig.h" /*TODO delete Transition Matrix, filterState, and newState trn_server.cpp calls getFilterState() which currently only returns 0 */ /****************************************************************************** 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 Trigger 5: Windowed NIS maximum has been exceeded ******************************************************************************/ //static int transitionMatrix[5][3] = {{0, 0, 1}, {1, 1, 1}, {1, 2, 2}, {2, 2, 2}, {2, 2, 2}}; TerrainNav::TerrainNav(char* mapName) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = (char*)"mappingAUV_specs.cfg"; this->particlesFile = NULL; this->saveDirectory = NULL; this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = 1; //this->octreeMap = NULL; this->mapType = 1; // defaults to DEM this->allowFilterReinits = true; if(this->mapType == 1) { terrainMap = new TerrainMapDEM(this->mapFile); } else { terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->particlesFile = NULL; this->saveDirectory = NULL; this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = 1; //this->octreeMap = NULL; this->mapType = 1; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs, const int& filterType) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->particlesFile = NULL; this->saveDirectory = NULL; this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = filterType; //this->octreeMap = NULL; this->mapType = 1; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs, const int& filterType, char* directory) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->saveDirectory = strdup(directory); this->particlesFile = NULL; this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = filterType; //this->octreeMap = NULL; this->mapType = 1; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs, const int& filterType, const int& mapType) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->saveDirectory = NULL; this->particlesFile = NULL; this->tNavFilter = NULL; this->filterType = filterType; //this->octreeMap = NULL; this->mapType = mapType; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs, const int& filterType, const int& mapType, char* directory) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->saveDirectory = strdup(directory); this->particlesFile = NULL; this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = filterType; //this->octreeMap = NULL; this->mapType = mapType; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::TerrainNav(char* mapName, char* vehicleSpecs, char* particles, const int& filterType, const int& mapType, char* directory) { //initialize pointers this->mapFile = strdup(mapName); this->vehicleSpecFile = strdup(vehicleSpecs); this->saveDirectory = strdup(directory); this->particlesFile = strdup(particles); this->tNavFilter = NULL; this->terrainMap = NULL; this->filterType = filterType; //this->octreeMap = NULL; this->mapType = mapType; this->allowFilterReinits = true; if(this->mapType == 1) { this->terrainMap = new TerrainMapDEM(this->mapFile); } else { this->terrainMap = new TerrainMapOctree(this->mapFile); } //initialize terrainNav private variables initVariables(); } TerrainNav::~TerrainNav() { if(tNavFilter != NULL) { delete tNavFilter; } tNavFilter = NULL; if(this->terrainMap != NULL){ delete this->terrainMap; } this->terrainMap = NULL; if(this->mapFile !=NULL)free(this->mapFile); this->mapFile=NULL; if(this->vehicleSpecFile != NULL)free(this->vehicleSpecFile); this->vehicleSpecFile=NULL; if(this->saveDirectory !=NULL)free(this->saveDirectory); this->saveDirectory=NULL; if(this->particlesFile !=NULL)free(this->particlesFile); this->particlesFile=NULL; output("TerrainNav::Number of reinitializations: %i\n", numReinits); delete TNavConfig::instance(); } 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!!!!! ********************* //Trying to solve problem of TRN estimate latching to a specific value and not changing with motion updates //*estimate = *tNavFilter->lastNavPose; //*estimate += this->estNavOffset; ////*estimate += estNavOffset; if(isnan(estimate->x) || isnan(estimate->y)){ estimate->covariance[0] = X_STDDEV_INIT*X_STDDEV_INIT; estimate->covariance[2] = Y_STDDEV_INIT*Y_STDDEV_INIT; } return; } void TerrainNav::measUpdate(measT* incomingMeas, const int& type) { //copy incoming measurement to current meas structure; measT currMeas; int i; // measT '=' operator performs copy operation, // allocating dynamic memory for variables... // this has to be released (note call to clean() below) 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. lastMeasValid = %d\n", currMeas.dataType, currMeas.time, lastMeasValid); this->lastMeasSuccess = false; // release dynamic memory resources currMeas.clean(); return; } //check if vehicle is within correlation map before including measurement if(this->mapType == 1) { 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 // measT '=' operator performs copy operation, // allocating dynamic memory for variables... // this has to be released (after pending measurements processed) 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); // release dynamic memory resources currMeas.clean(); 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::measUpdate - Measurement type %i successfully incorporated from" " time = %.2f sec.\n", currMeas.dataType, currMeas.time); lastMeasSuccessTime = currMeas.time; } // release dynamic memory resources currMeas.clean(); return; } else{ output("TerrainNav::Did not incorporate measurement type %i from time" "= %.2f sec.; no INS pose data available. \n", currMeas.dataType, currMeas.time); } // release dynamic memory resources currMeas.clean(); return; } ////////////////////////// // Process multibeam range update message // void TerrainNav::mbUpdate(mbT* mb) { // Process the reson ranges here output("TerrainNav::mbUpdate - recv mbT %d ranges from time = %.2f sec. \n", mb->numBeams, mb->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()) { output("TerrainNav::motionUpdate() - filter unhealthy, skipping this update\n"); 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; } else { output("TerrainNav::motionUpdate() - Invalid velocity\n"); } //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) { output("TerrainNav::motionUpdate() - Invalid velocity, using prior value\n"); currEstimate.vx = lastValidVel[0]; currEstimate.vy = lastValidVel[1]; currEstimate.vz = lastValidVel[2]; currEstimate.bottomLock = lastVelBotLock; } else { //if we just lost bottom lock, reset water 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) Matrix estWatVel(3, 1); Matrix tempCurrentVel(3, 1); estWatVel(1, 1) = currEstimate.vx - lastValidVel[0]; estWatVel(2, 1) = currEstimate.vy - lastValidVel[1]; estWatVel(3, 1) = currEstimate.vz - lastValidVel[2]; tempCurrentVel = tNavFilter->applyRotation(attitude, estWatVel); tNavFilter->currentVel[0] = tempCurrentVel(1, 1); tNavFilter->currentVel[1] = tempCurrentVel(2, 1); tNavFilter->currentVel[2] = tempCurrentVel(3, 1); } 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].translation[0]; ry = tNavFilter->vehicle->T_sv[sensorIndx].translation[1]; rz = tNavFilter->vehicle->T_sv[sensorIndx].translation[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::motionUpdate - Measurement type %i successfully incorporated " "from time = %.2f sec.\n", waitingMeas[i].dataType, measPose.time); lastMeasSuccessTime = measPose.time; } } } // release resources allocated earlier by copy operator for(int i = 0; i < this->numWaitingMeas; i++) { waitingMeas[i].clean(); } //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; printf("Loading vehicle config file... %s\n", this->vehicleSpecFile); //create new filter based on given filter type switch(filterType) { case 1: tNavFilter = new TNavPointMassFilter(this->terrainMap, this->vehicleSpecFile, this->saveDirectory, windowVar, this->mapType); break; case 2: tNavFilter = new TNavParticleFilter(this->terrainMap, this->vehicleSpecFile, this->saveDirectory, windowVar, this->mapType); break; default: tNavFilter = new TNavPointMassFilter(this->terrainMap, this->vehicleSpecFile, this->saveDirectory, windowVar, this->mapType); } this->filterType = filterType; output("TerrainNav::TNavFilter initialized with type %i\n", filterType); } void TerrainNav::initVariables() { _initialized = false; bool mapOk = true; //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 }; // Initialize TNavConfig TNavConfig::instance()->setMapFile(this->mapFile); TNavConfig::instance()->setVehicleSpecsFile(this->vehicleSpecFile); TNavConfig::instance()->setParticlesFile(this->particlesFile); TNavConfig::instance()->setLogDir(this->saveDirectory); copyToLogDir(); //output("TerrainNav::TNavFilter initialized with Z variance %.2f\n", windowVar[5]); //create filter object createFilter(this->filterType, windowVar); //old state machine version: filterState = 1; //filterState = 2;//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; _initialized = mapOk; } 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(this->mapType == 1) { withinMap = tNavFilter->withinValidMapRegion(initEstimate.x, initEstimate.y); } else { withinMap = true; // TODO:**** Need to actually check within map, if we care for octrees ******** } 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 and IDT measurements if(currMeas.dataType == 1) { 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; } output("TerrainNav:: Throwing out all beams beacause more " "than two are equal at t=%.2f.\n", currMeas.time); 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; } } } } } else if(currMeas.dataType == 5){ for(i = 0; i < currMeas.numMeas; i++) { //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) || ((i < 30) || (i >= 90)) ) { // TODO:HACK to use only the middle 60 of 120 beams currMeas.measStatus[i] = false; } else { // // measStatus is set back to false during trip from MVC? currMeas.measStatus[i] = true; } } output("TerrainNav::measUpdate() - IDT[45] = %.2f, IDT[75] = %.2f\n", currMeas.ranges[45], currMeas.ranges[75]); } else { return; } } //void TerrainNav::reinitFilter(int newState, bool lowInfoTransition) { //old state machine declaratino: void TerrainNav::reinitFilter(int newState, bool lowInfoTransition) { void TerrainNav::reinitFilter(bool lowInfoTransition) { int interpMapMethod = 1; bool interpMeasAttitude = true; double driftRate = 1; bool useModWeight = 0; //Default initialization window --> mainly used for broad area reinitializations 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->GetInterpMapMethod(); interpMeasAttitude = tNavFilter->interpMeasAttitude; driftRate = tNavFilter->vehicle->driftRate; useModWeight = tNavFilter->useModifiedWeighting; //If keeping mean and covariance if(tNavFilter->lastNavPose != NULL && !lowInfoTransition) { tNavFilter->computeMMSE(temp); //puts covariance and MMSE in temp for(i = 0; i < 36; i++) { windowVar[i] = 1.0 * temp->covariance[i]; } } //delete filter object delete tNavFilter; tNavFilter = NULL; } /*//old state machine version: //create new filter based on the newState switch(newState) { case 0: //Create new Sigma Point filter (NOMINALLY, CURRENTLY USING PARTICLE FILTER (state 2)) //Create new Sigma Point filter (NOMINALLY, CURRENTLY USING PARTICLE FILTER (filter type 2)) createFilter(2, windowVar); break; case 1: //Create new Particle filter 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 } } if(this->mapType == 2) { //Create Particle Fitler from windowVar TODO: don't only use PF with octree, but allow PMF too createFilter(2, windowVar); } else { //Create Point Mass Filter from windowVar TODO: fix PMF so that it can be used here for both cases createFilter(2, 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; */ createFilter(2, windowVar); //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); setModifiedWeighting(useModWeight); 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; //old state machine version: 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) { //old state machine version: 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) { //old state machine version: 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); } /*//old state machine version: //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); } }*/ //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); } */ /*//old state machine version: //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); } }*/ /*//old state machine version: //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)*/ /*//old state machine version: { 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); } }*/ //check the windowed average of the Normalized Innovations Squared if(healthy && tNavFilter->windowedNIS > MAX_NIS_VALUE) { //***TODO Figure out how this should work //old state machine version: newState = transitionMatrix[1][filterState]; healthy = false; lowInfoTransition = true; output("TerrainNav:: Windowed NIS average %.2f exceeds the maximum allowed " "of %.1f. Re-initializing the filter.\n", tNavFilter->windowedNIS, MAX_NIS_VALUE); } else { output("TerrainNav:: Windowed NIS is %.1f, in allowable region under " "%.1f. Keep on Trucking.\n", tNavFilter->windowedNIS, MAX_NIS_VALUE); } //if filter is not healthy reinitialize if(!healthy) { //old state machine version: reinitFilter(newState, lowInfoTransition); reinitFilter(lowInfoTransition); } return healthy; } /* Function: copyToLogDir() - copies configuration files to the log directory * for future reference * A log directory (saveDirectory) must be specified in the initialization * message, otherwise nothing is copied. */ void TerrainNav::copyToLogDir() { // Used the following line for testing // this->saveDirectory = "./logdir"; // Copy only if there is a place for the files to land // int sys_return; if (NULL != this->saveDirectory) { mkdir(this->saveDirectory, 0777); remove("latest"); sys_return = symlink(this->saveDirectory, "latest"); } else return; char copybuf[300]; if (NULL != this->vehicleSpecFile) { sprintf(copybuf, "cp %s %s/.", this->vehicleSpecFile, this->saveDirectory); sys_return = system(copybuf); } if (NULL != this->particlesFile) { sprintf(copybuf, "cp %s %s/.", this->particlesFile, this->saveDirectory); sys_return = system(copybuf); } // Create a vehicleT object just to get the sensor spec files to copy // vehicleT *v = new vehicleT(vehicleSpecFile); for (int i = 0; i < v->numSensors; i++) { sprintf(copybuf, "cp %s %s/.", v->sensors[i].filename, this->saveDirectory); sys_return = system(copybuf); } delete v; }