/* FILENAME      : TNavParticleFilter.cpp
 * AUTHOR        : Debbie Meduna and Peter Kimball
 * DATE          : 05/06/09
 * 
 * LAST MODIFIED : 07/15/10
 * MODIFIED BY   : Debbie Meduna
 * -----------------------------------------------------------------------------
 * Modification History
 * -----------------------------------------------------------------------------
 ******************************************************************************/

#include "TNavParticleFilter.h"

TNavParticleFilter::TNavParticleFilter(char *mapName, char *vehicleSpecs, char *directory, const double *windowVar) : TNavFilter(mapName, vehicleSpecs, directory, windowVar)
{
   initVariables();
}


TNavParticleFilter::~TNavParticleFilter()
{
  if(saveDirectory != NULL)
   {
      //close data files
      allParticlesFile.close();
      resampParticlesFile.close();
      homerParticlesFile.close();
      homerMmseFile.close();
      measWeightsFile.close();
   }
}

void TNavParticleFilter::initFilter(poseT& initNavPose)
{
   particleT initGuess;
   int i;

   //Fill in particle pose information with poseT struct
   initGuess.position[0] = initNavPose.x;
   initGuess.position[1] = initNavPose.y;
   initGuess.position[2] = initNavPose.z;
   initGuess.attitude[0] = initNavPose.phi;
   initGuess.attitude[1] = initNavPose.theta;
   initGuess.attitude[2] = initNavPose.psi;
   initGuess.compassBias = 0;
   initGuess.gyroBias[0] = 0;
   initGuess.gyroBias[1] = 0;
   if(INTEG_PHI_THETA)
      initGuess.gyroBias[2] = 0;
  
   for(i = 0; i < 3; i++)
   {
      initGuess.terrainState[i] = 0;   
      initGuess.alignState[i] = 0;
      initGuess.watVel[i] = 0;
      initGuess.dvlBias[i] = 0;
   }
   initGuess.dvlScaleFactor = 0;

   //initialize particle distribution using initial navigation pose
   initParticleDist(initGuess);
   
}

bool TNavParticleFilter::measUpdate(measT& currMeas)
{
   Matrix beamsVF(3,currMeas.numMeas);
   double sumSquaresWeights = 0;
   double sumWeights = 0;
   double sumMeasWeights = 0;
   double effSampSize;
   int i;
   double attitude[3] ={lastNavPose->phi, lastNavPose->theta, lastNavPose->psi};
   int mapStatus;
   double avgWeights;
   bool successfulMeas = false;

   //if the current measurement is a homer measurement, use homerMeasUpdate()
   if(currMeas.dataType == 4)      
      return homerMeasUpdate(currMeas);
   
   //if searching over DVL alignment, only project into the sensor frame
   if(SEARCH_ALIGN_STATE)
      successfulMeas = projectMeasSF(beamsVF, currMeas);
   else
      successfulMeas = projectMeasVF(beamsVF, currMeas);

   //if projection was successful, load relevant submap and compute 
   //measurement weights for each particle
   if(successfulMeas)
   {
      //Load a sub-map for use in measurement correlation
      //Find the bounds of the North/East rectangle which circumscribes the 
      //particle distribution
      if(!USE_OCTREE){
      mapStatus = defineAndLoadSubMap(beamsVF);
      }
      else{
      mapStatus = MAPBOUNDS_OK;  //TODO:**** Have to correct this later ****
      }
      
      // check that the map extraction worked correctly
      if(mapStatus == MAPBOUNDS_OUT_OF_BOUNDS)
      {
         output("TerrainNav::Measurement from time = %.2f sec. not included; "
                "unable to successfully extract a map segment for correlation"
                , currMeas.time);
         successfulMeas = false;       
      }
      else
      {
         successfulMeas = true;
        
        //If we are not searching over attitude, we can perform rotation first 
        if(!ALLOW_ATTITUDE_SEARCH)
           beamsVF = applyRotation(attitude, beamsVF);

		//Here we trigger betweeen using the modified weighting scheme concocted by Shandor
		//and the standard TRN weighting
		if(!this->useModifiedWeighting)
		{
			//Loop through & compute measurement update weights for all particles
			for(i = 0; i < nParticles; i++)
			{
				//TODO: Include USE_OCTREE and perform this step with octreeMap
			   if(measUpdateParticle(allParticles[i], beamsVF, currMeas.covariance, 
									 currMeas.ranges, currMeasWeights[i]))
			   {
				  sumWeights += allParticles[i].weight*currMeasWeights[i];
				  sumMeasWeights += currMeasWeights[i];
			   }
			   else
			   {
				  output("TerrainNav::Measurement from time = %.2f sec. not included; "
						 "encountered NaN values in the correlation map segment\n", 
						 currMeas.time);
				  return false;
			   }
			}
		}
		else
		{
			//Implement modified algorithm
			double mapSquared[4]  = {0.0, 0.0, 0.0, 0.0};   //
			double mapMean[4]  = {0.0, 0.0, 0.0, 0.0};		//mean value of expected measurement
			double mapVariance[4]= {0.0, 0.0, 0.0, 0.0};	//variance of expected measurements
			//First, compute map information for each beam, iterating through all the particles
			for(i = 0; i < nParticles; i++)
			{
				//Checks that the map return for the beam is valid and updates the mapSquared
				//and mapMean parameters with the quantities for that particle

			   if(!getMapInfoParticle(allParticles[i], beamsVF, currMeas.ranges, mapSquared, mapMean))
			   {

				  //If the map returns are invalid, exit the loop
				  output("TerrainNav::Measurement from time = %.2f sec. not included; "
						 "encountered NaN values in the correlation map segment\n", 
						 currMeas.time);
				  return false;
			   }
			}
			output("TNavParticleFilter::mapSquared = {%.2f,%.2f,%.2f,%.2f}\n",mapSquared[0],
				mapSquared[1],mapSquared[2],mapSquared[3]);
			output("TNavParticleFilter::mapMean = {%.2f,%.2f,%.2f,%.2f}\n",mapMean[0],
				mapMean[1],mapMean[2],mapMean[3]);


			for(i = 0; i< beamsVF.Ncols(); i++)
			{
				//Compute variance of expected measurements
				mapVariance[i] = mapSquared[i]-pow(mapMean[i],2);
			}
			output("TNavParticleFilter::mapVariance = {%.2f,%.2f,%.2f,%.2f}\n",mapVariance[0],
				mapVariance[1],mapVariance[2],mapVariance[3]);
			//Now, update the particle distribution
			for(i = 0; i < nParticles; i++)
			{
			   if(modifiedMeasUpdateParticle(allParticles[i], beamsVF, currMeas.covariance, 
									 currMeas.ranges, currMeasWeights[i],mapVariance))
			   {
				  sumWeights += allParticles[i].weight*currMeasWeights[i];
				  sumMeasWeights += currMeasWeights[i];
			   }
			   else
			   {
				  output("TerrainNav::Measurement from time = %.2f sec. not included; "
						 "encountered NaN values in the correlation map segment\n", 
						 currMeas.time);
				  return false;
			   }
			}


		}
        
        //Keep track of the number of soundings used since the last resampling
        nSoundings += beamsVF.Ncols();
        
        measVariance = 0;

        //Apply measurement weights and normalize the distribution
        for(i = 0; i < nParticles; i++)
        {
           allParticles[i].weight *= currMeasWeights[i]/sumWeights;
           sumSquaresWeights += pow(allParticles[i].weight,2);
           //compute variance of measurement weights
           currMeasWeights[i] /= sumMeasWeights;
           measVariance += pow(currMeasWeights[i]-1.0/nParticles,2)/nParticles;
           if(saveDirectory!=NULL)
           {
              //output("%.2f",currMeasWeights[i]);
              measWeightsFile << currMeasWeights[i] << "\t";
           }
        }
        effSampSize = 1.0/sumSquaresWeights;
        
        if(saveDirectory!=NULL)
       	{
           measWeightsFile << endl;
        }

        if(USE_AUG_MCL)
        {
           //Update Augmented MCL average weight parameters
           avgWeights = sumWeights/nParticles;
           if(w_slow == 0 && w_fast == 0)
           {
              w_slow = avgWeights;
              w_fast = avgWeights;
           }
           w_slow += a_slow*(avgWeights - w_slow);
           w_fast += a_fast*(avgWeights - w_fast);
        }

         //Resample the distribution if appropriate
        if(effSampSize < MIN_EFF_SAMP_SIZE*nParticles && nSoundings >= MIN_NUM_SOUNDINGS)
        {
           //plot current particle distribution if Matlab is set to be used
#ifdef USE_MATLAB
           plotMapMatlab(this->terrainMap->map.depths, 
                         this->terrainMap->map.xpts, 
                         this->terrainMap->map.ypts, 
                         "title('Sub-Map and Particle Distribution: Prior to Resampling');", 
                         "figure(2)");
              
           plotParticleDistMatlab(allParticles, "figure(2)");
#endif              

           //resample particle distribution
           resampParticleDist(); 
           resampled = true;

           //reset nSoundings counter
           nSoundings = 0;
        }
        else
           resampled = false;

      }
   }
  
#ifdef USE_MATLAB
  plotMapMatlab(this->terrainMap->map.depths, this->terrainMap->map.xpts,
                this->terrainMap->map.ypts, "title('Sub-Map and Post-Resampling Particle Distribution');", "figure(1)");
  mapPlotted = 1;
  plotParticleDistMatlab(allParticles, "figure(1)");
#endif 

  //if measurement successfully added, recheck estimator convergence
  //if(successfulMeas)
  //   checkConvergence();

  return successfulMeas;
}


void TNavParticleFilter::motionUpdate(poseT& currNavPose)
{
   int i;
   double velocity_sf_sigma[3];
   poseT diffPose;
   double gyroStddev;

   //if no valid gps data, extract relevant velocity measurement for 
   //dead-reckoning
   if(!currNavPose.gpsValid && DEAD_RECKON)
   {
      velocity_sf_sigma[0] = 0.0;
      velocity_sf_sigma[1] = 0.0;
      velocity_sf_sigma[2] = 0.0;

      //Extract PREVIOUS sensor frame velocity plus guassian noise based on 
      //bottom lock:
      if(lastNavPose->bottomLock)
      {
         velocity_sf_sigma[0] += fabs(VEL_PER_ERROR*lastNavPose->vx/100.0);
         velocity_sf_sigma[1] += fabs(VEL_PER_ERROR*lastNavPose->vy/100.0);
         velocity_sf_sigma[2] += fabs(VEL_PER_ERROR*lastNavPose->vz/100.0);
      }
      else
      {
         velocity_sf_sigma[0] += fabs(WATER_VEL_PER_ERROR*lastNavPose->vx/100.0);
         velocity_sf_sigma[1] += fabs(WATER_VEL_PER_ERROR*lastNavPose->vy/100.0);
         velocity_sf_sigma[2] += fabs(WATER_VEL_PER_ERROR*lastNavPose->vz/100.0);
      }
   
      //check if velocity data is old and add noise according to a 
      //0.01m/s^2 acceleration
      if(this->timeLastDvlValid != lastNavPose->time)
      {
         velocity_sf_sigma[0] += 0.01*fabs(lastNavPose->time - this->timeLastDvlValid);
         velocity_sf_sigma[1] += 0.01*fabs(lastNavPose->time - this->timeLastDvlValid);
         velocity_sf_sigma[2] += 0.01*fabs(lastNavPose->time - this->timeLastDvlValid);
      }

      //add on constant velocity sigma
      velocity_sf_sigma[0] += VEL_STDDEV;
      velocity_sf_sigma[1] += VEL_STDDEV;
      velocity_sf_sigma[2] += VEL_STDDEV;
   }
   

   //Compute delta pose of the vehicle since the last update
   diffPose = currNavPose;
   diffPose -= *lastNavPose;  

   if(diffPose.time > 0)
      gyroStddev = DGBIAS_ERROR/sqrt(diffPose.time);
   else
      gyroStddev = 0.0;

   //Update each particle's position individually
   for(i = 0; i < nParticles; i++){
      motionUpdateParticle(allParticles[i], diffPose, velocity_sf_sigma, gyroStddev);
   }

   //Apply attitude measurement update if integrating for phi/theta states
   if(INTEG_PHI_THETA)
      attitudeMeasUpdate(currNavPose);

   //Apply water velocity measurement update if we have both bottom lock and good dvl data
   if(SEARCH_WAT_VEL && currNavPose.bottomLock && currNavPose.dvlValid)
      watVelMeasUpdate(currNavPose);

   //if(saveDirectory != NULL)
   //   writeParticlesToFile(allParticles, allParticlesFile);

#ifdef USE_MATLAB
   plotMapMatlab(this->terrainMap->map.depths, 
                 this->terrainMap->map.xpts, 
                 this->terrainMap->map.ypts, 
                 "title('Sub-Map and Particle Distribution: After motion update');", 
                 "figure(3)");
   
   plotParticleDistMatlab(allParticles, "figure(3)");
#endif      

   return;
}

void TNavParticleFilter::computeMLE(poseT* mlePose)
{
  int i;
  particleT mleParticle;
  double maxWeight = 0;
  
  mleParticle = allParticles[0];

  //Find the particle with the highest weight
  for (i = 0; i < nParticles; i++){
    if (allParticles[i].weight > maxWeight){
      maxWeight = allParticles[i].weight;
      mleParticle = allParticles[i];
    }
  }
  
  //Set the mlePose to the pose of the particle with the highest weight
  getParticlePose(mleParticle, mlePose);

  return;
}


void TNavParticleFilter::computeMMSE(poseT* mmsePose)
{
  int i;
  double weight, alpha, temp1,temp2;
  double sumWeights = 0;
  poseT tempPose;
  
  for (i = 0; i < nParticles; i++)
  {
     weight = allParticles[i].weight;
     sumWeights += weight;

     tempPose.x += weight*allParticles[i].position[0];
     tempPose.y += weight*allParticles[i].position[1];
     tempPose.z += weight*allParticles[i].position[2];
     tempPose.phi += weight*allParticles[i].attitude[0];
     tempPose.theta += weight*allParticles[i].attitude[1];
     if(SEARCH_COMPASS_BIAS)
        tempPose.psi += weight*(allParticles[i].attitude[2]+allParticles[i].compassBias);
     else
        tempPose.psi += weight*allParticles[i].attitude[2];
     if(SEARCH_GYRO_BIAS)
     {
        tempPose.wy += weight*allParticles[i].gyroBias[0];
        tempPose.wz += weight*allParticles[i].gyroBias[1];
     }
  }

  if(sumWeights != 1)
  {
     tempPose.x /= sumWeights;
     tempPose.y /= sumWeights;
     tempPose.z /= sumWeights;
     tempPose.phi /= sumWeights;
     tempPose.theta /= sumWeights;
     tempPose.psi /= sumWeights;
     if(SEARCH_GYRO_BIAS)
     {
        tempPose.wy /= sumWeights;
        tempPose.wz /= sumWeights;
     }
  }

  for (i = 0; i < nParticles; i++)
  {
     weight = allParticles[i].weight;
     alpha = weight/sumWeights;
     temp1 = allParticles[i].position[0]-tempPose.x;
     tempPose.covariance[0]+=temp1*temp1*alpha;
     temp2 = allParticles[i].position[1]-tempPose.y;
     tempPose.covariance[2]+=temp2*temp2*alpha; 
     tempPose.covariance[1]+=temp1*temp2*alpha;
     temp1 = allParticles[i].position[2]-tempPose.z;
     tempPose.covariance[5]+=temp1*temp1*alpha;
     temp1 = allParticles[i].attitude[0]-tempPose.phi;
     tempPose.covariance[9]+=temp1*temp1*alpha;
     temp1 = allParticles[i].attitude[1]-tempPose.theta;
     tempPose.covariance[14]+=temp1*temp1*alpha;
     if(SEARCH_COMPASS_BIAS)
        temp1 = allParticles[i].attitude[2]+allParticles[i].compassBias-tempPose.psi;
     else
        temp1 = allParticles[i].attitude[2]-tempPose.psi;
     tempPose.covariance[20]+=temp1*temp1*alpha;
     if(SEARCH_GYRO_BIAS)
     {
        temp1 = allParticles[i].gyroBias[0]-tempPose.wy;
        tempPose.covariance[27]+=temp1*temp1*alpha;
        temp1 = allParticles[i].gyroBias[1]-tempPose.wz;
        tempPose.covariance[35]+=temp1*temp1*alpha;
     }   
  }

  tempPose.time = this->lastNavPose->time;
  *mmsePose = tempPose;

  return;
}

void TNavParticleFilter::checkConvergence()
{
   double kl;

   kl = computeKLdiv_gaussian_particles();

   //output("Current KL: %.10f\n", kl);
   if(kl < 1)
      converged = true;
   else
      converged = false;
}

void TNavParticleFilter::saveCurrDistrib(ofstream &outputFile)
{
   //check that the desired file exists and is open for writing
   if(!outputFile.is_open())
   {
      output("Error:Tried to write to an unopened file."
             " Ignoring write command.");
      return;
   }

   if(SAVE_PARTICLES)
      writeParticlesToFile(allParticles, outputFile);
   else
      writeHistDistribToFile(allParticles, outputFile);
}


particleT* TNavParticleFilter::getParticles()
{
   return this->allParticles;
}


void TNavParticleFilter::saveCurrParticles(ofstream &outputFile)
{
   //check that the desired file exists and is open for writing
   if(!outputFile.is_open())
   {
      output("Error:Tried to write to an unopened file."
             " Ignoring write command.");
      return;
   }

   writeParticlesToFile(allParticles, outputFile);
}


void TNavParticleFilter::reinitWatVelDist(double* initWatVel)
{
   int i;
   double noisyWatVel[3];

   if(SEARCH_WAT_VEL)
   {
      for(i = 0; i < nParticles; i++)
      {
         //Add uncertainty to initWatVel:
         noisyWatVel[0] = initWatVel[0] + 2.0*randn_zeroMean(VEL_STDDEV) + 
            randn_zeroMean(WATER_VEL_PER_ERROR*initWatVel[0]/100.0);
         noisyWatVel[1] = initWatVel[1] + 2.0*randn_zeroMean(VEL_STDDEV) + 
            randn_zeroMean(WATER_VEL_PER_ERROR*initWatVel[1]/100.0);
         noisyWatVel[2] = initWatVel[2] + 2.0*randn_zeroMean(VEL_STDDEV) + 
            randn_zeroMean(WATER_VEL_PER_ERROR*initWatVel[2]/100.0);

         //if searching over DVL error factors, factor these in
         if(SEARCH_DVL_ERRORS)
         {
            noisyWatVel[0] += allParticles[i].dvlScaleFactor*initWatVel[0] + 
               allParticles[i].dvlBias[0];
            noisyWatVel[1] += allParticles[i].dvlScaleFactor*initWatVel[1] + 
               allParticles[i].dvlBias[1];
            noisyWatVel[2] += allParticles[i].dvlScaleFactor*initWatVel[2] + 
               allParticles[i].dvlBias[2];
         }
              
         //rotate wat velocity into inertial frame
         applyVecRotation(allParticles[i].attitude, noisyWatVel, allParticles[i].watVel);
      }
   }
}


void TNavParticleFilter::initVariables()
{ 
   //initialize particle array pointers
   allParticles = particleArray1;
   resampParticles = particleArray2;

   //The particle filter will start out with the maximum number of particles
   nParticles = MAX_PARTICLES;
   
   //Initialize random seed to desired location
   srand(time(NULL));
   
   //Initialize counter for soundings used in correlation
   nSoundings = 0;
   
   //Initialize the mapPlotted flag to zero
   mapPlotted = 0;

   //Set resampled flag to false
   resampled = false;

   //Initialize Augmented MCL parameters
   if(USE_AUG_MCL)
   {
      a_slow = 0.03;
      a_fast = 0.2;
      w_slow = 0.0;
      w_fast = 0.0;
   }

   //Initialize save directories
   if(saveDirectory != NULL)
   {
      //load data files for storing results
      char fileName[2048];
      allParticlesFile.open(charCat(fileName,saveDirectory,"randParticles.txt"));
      resampParticlesFile.open(charCat(fileName,saveDirectory,
                                       "resampParticles.txt"));
      homerParticlesFile.open(charCat(fileName,saveDirectory,"homerParticles.txt"));
      homerMmseFile.open(charCat(fileName,saveDirectory,"homerMmse.txt"));
      measWeightsFile.open(charCat(fileName,saveDirectory,"measWeights.txt"));
   }
}

void TNavParticleFilter::initParticleDist(particleT& initialGuess)
{
   int i,j;
   SymmetricMatrix tempCov(8);
   SymmetricMatrix tempCovSqrt(8);
   tempCovSqrt = 0.0;
   double tempX,tempY;
   
   tempCov.Row(1)<<initWindowVar[0];
   tempCov.Row(2)<<initWindowVar[1]<<initWindowVar[2];
   tempCov.Row(3)<<0.0<<0.0<<initWindowVar[5];
   tempCov.Row(4)<<0.0<<0.0<<0.0<<initWindowVar[9];
   tempCov.Row(5)<<0.0<<0.0<<0.0<<0.0<<initWindowVar[14];
   tempCov.Row(6)<<0.0<<0.0<<0.0<<0.0<<0.0<<initWindowVar[20];
   tempCov.Row(7)<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<initWindowVar[27];
   tempCov.Row(8)<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<initWindowVar[35];
   tempCovSqrt.SymSubMatrix(1,2) << computeMatrixSqrt(tempCov.SymSubMatrix(1,2));

   tempCovSqrt(3,3) = fabs(sqrt(initWindowVar[5]));
   tempCovSqrt(4,4) = fabs(sqrt(initWindowVar[9]));
   tempCovSqrt(5,5) = fabs(sqrt(initWindowVar[14]));
   tempCovSqrt(6,6) = fabs(sqrt(initWindowVar[20]));
   tempCovSqrt(7,7) = fabs(sqrt(initWindowVar[27]));
   tempCovSqrt(8,8) = fabs(sqrt(initWindowVar[35]));

   //define pointer to randon number generator function
   double (*pt2randFunction)(const double&) = NULL;

   //define random number generator function based on value of initDistribType
   switch(this->initDistribType)
   {
   case 0:
      pt2randFunction = &unif_zeroMean;
      break;

   case 1:
      pt2randFunction = &randn_zeroMean;
      break;

   default:
      pt2randFunction = &unif_zeroMean;
   }

  //Initialize the particle distribution to a gaussian around the intial guess
  //Variances defined in particleFilterDefs.h
  for(i = 0; i < nParticles; i++){
     allParticles[i] = initialGuess;
     allParticles[i].weight = 1.0/nParticles;

     //initialize positions with uniform distribution (or reinitialize with gaussian)
     tempX = (*pt2randFunction)(1.0);
     tempY = (*pt2randFunction)(1.0);
     
     this->allParticles[i].position[0] += tempX*tempCovSqrt(1,1) + 
        tempY*tempCovSqrt(1,2);
     this->allParticles[i].position[1] += tempX*tempCovSqrt(2,1) + 
        tempY*tempCovSqrt(2,2);
     if(!USE_CONTOUR_MATCHING)
        this->allParticles[i].position[2] += (*pt2randFunction)(tempCovSqrt(3,3));

     //initialize angles with gaussian distribution
     if(ALLOW_ATTITUDE_SEARCH)
     {
        this->allParticles[i].attitude[0]+= (*pt2randFunction)(tempCovSqrt(4,4));
        this->allParticles[i].attitude[1]+= (*pt2randFunction)(tempCovSqrt(5,5));
        this->allParticles[i].attitude[2]+= (*pt2randFunction)(tempCovSqrt(6,6));
     }
    
     //initialize moving terrain states if used
     if(MOVING_TERRAIN)
     {
        this->allParticles[i].terrainState[0] += randn_zeroMean(TERRAIN_DXDT_STDDEV_INIT);
        this->allParticles[i].terrainState[1] += randn_zeroMean(TERRAIN_DYDT_STDDEV_INIT);
        this->allParticles[i].terrainState[2] += randn_zeroMean(TERRAIN_DHDT_STDDEV_INIT);
     }

     //initialize compass bias if used
     if(SEARCH_COMPASS_BIAS)
        this->allParticles[i].compassBias += unif_zeroMean(COMPASS_BIAS_STDDEV_INIT);

     //initialize alignment states if used
     if(SEARCH_ALIGN_STATE)
     {
        this->allParticles[i].alignState[0] += unif_zeroMean(PHI_ALIGN_ERROR_STDDEV_INIT);
        this->allParticles[i].alignState[1] += unif_zeroMean(THETA_ALIGN_ERROR_STDDEV_INIT);
        this->allParticles[i].alignState[2] += unif_zeroMean(PSI_ALIGN_ERROR_STDDEV_INIT);
     }

     //initialize gyro bias states if used
     if(SEARCH_GYRO_BIAS)
     {
        if(SEARCH_GYRO_Y)
           this->allParticles[i].gyroBias[0]+= (*pt2randFunction)(tempCovSqrt(7,7));
        this->allParticles[i].gyroBias[1]+= (*pt2randFunction)(tempCovSqrt(8,8));
        if(INTEG_PHI_THETA)
           this->allParticles[i].gyroBias[2] += (*pt2randFunction)(tempCovSqrt(8,8));
     }

     //initialize water velocity states if used
     if(SEARCH_WAT_VEL)
     {
        this->allParticles[i].watVel[0] += unif_zeroMean(WAT_VEL_STDDEV_INIT);
        this->allParticles[i].watVel[1] += unif_zeroMean(WAT_VEL_STDDEV_INIT);
        this->allParticles[i].watVel[2] += unif_zeroMean(WAT_VEL_STDDEV_INIT);
     }

     //initialize DVL scale factor and bias if used
     if(SEARCH_DVL_ERRORS)
     {
        this->allParticles[i].dvlScaleFactor += unif_zeroMean(DVL_SF_STDDEV_INIT);
        for(j = 0; j < 3; j++)
           this->allParticles[i].dvlBias[j] += unif_zeroMean(DVL_BIAS_STDDEV_INIT);
     }
  }

  return;
}

void TNavParticleFilter::attitudeMeasUpdate(poseT& currPose)
{
   double sumWeights = 0;
   double sumSquaresWeights = 0;
   double effSampSize;
   double avgWeights;
   double phiVar = DPHI_STDDEV*DPHI_STDDEV;
   double thetaVar = DTHETA_STDDEV*DTHETA_STDDEV;
   double sumSquaredError = 0;
   double measProb;
   int i;

   //Loop through & apply measurement update to all particles
   for(i = 0; i < nParticles; i++)
   {
      sumSquaredError = (1.0/phiVar)*pow(allParticles[i].attitude[0] - currPose.phi,2)
         +(1.0/thetaVar)*pow(allParticles[i].attitude[1] - currPose.theta,2);
      measProb = pow(2*PI,-1.0)*pow(thetaVar*phiVar,-0.5)*
         exp(-0.5*sumSquaredError);
      allParticles[i].weight *= measProb;
      sumWeights += allParticles[i].weight;
   }
   //Normalize the distribution
   for(i = 0; i < nParticles; i++)
   {
      allParticles[i].weight /= sumWeights;
      sumSquaresWeights += pow(allParticles[i].weight,2);
   }
   effSampSize = 1.0/sumSquaresWeights;

   if(USE_AUG_MCL)
   {
      //Update Augmented MCL average weight parameters
      avgWeights = sumWeights/nParticles;
      if(w_slow == 0 && w_fast == 0)
      {
         w_slow = avgWeights;
         w_fast = avgWeights;
      }
      w_slow += a_slow*(avgWeights - w_slow);
      w_fast += a_fast*(avgWeights - w_fast);
   }

   //Resample the distribution if appropriate
   if(effSampSize < MIN_EFF_SAMP_SIZE*nParticles)
   {
      //plot current particle distribution if Matlab is set to be used
#ifdef USE_MATLAB
      plotMapMatlab(this->terrainMap->map.depths, 
                    this->terrainMap->map.xpts, 
                    this->terrainMap->map.ypts, 
                         "title('Sub-Map and Particle Distribution: Prior to Resampling');", 
                    "figure(2)");
      
      plotParticleDistMatlab(allParticles, "figure(2)");
#endif              
      
      //resample particle distribution
      resampParticleDist(); 
      resampled = true;
   }
   else
      resampled = false;
   
}

void TNavParticleFilter::watVelMeasUpdate(poseT& currPose)
{
   double sumWeights = 0;
   double sumSquaresWeights = 0;
   double effSampSize;
   double avgWeights;
   double velVar[3];
   double velCurrVF[3];
   double sumSquaredError = 0;
   double measProb;
   double velCurrEst[3];
   int i,j;
   output("in wat vel meas update\n");

   //If water velocity data is bad, don't apply measurement update
   if(currPose.vw_x == 0 || currPose.vw_y == 0 || currPose.vw_z == 0)
      return;

   //Define water velocity sigma
   velVar[0] = pow(VEL_STDDEV + fabs(WATER_VEL_PER_ERROR*currPose.vw_x/100.0),2);
   velVar[1] = pow(VEL_STDDEV + fabs(WATER_VEL_PER_ERROR*currPose.vw_y/100.0),2);
   velVar[2] = pow(VEL_STDDEV + fabs(WATER_VEL_PER_ERROR*currPose.vw_z/100.0),2);

   //Define the current velocity estimate for the current time:
   velCurrEst[0] = currPose.vw_x - currPose.vx;
   velCurrEst[1] = currPose.vw_y - currPose.vy;
   velCurrEst[2] = currPose.vw_z - currPose.vz;
   output("current velocity estimate: %f, %f, %f\n", velCurrEst[0], velCurrEst[1], velCurrEst[2]);
   output("water velocity: %f, %f, %f\n", currPose.vw_x, currPose.vw_y, currPose.vw_z);
   output("velvar: %f, %f, %f\n", 1.0/velVar[0], 1.0/velVar[1], 1.0/velVar[2]);

   //Loop through & apply measurement update to all particles
   for(i = 0; i < nParticles; i++)
   {
      sumSquaredError = 0;
      //rotate estimated current velocity into vehicle frame
      //output("pre rotation: %f, %f, %f\n", allParticles[i].watVel[0], allParticles[i].watVel[1], allParticles[i].watVel[2]);
      //output("post rotation: %f, %f, %f\n", velCurrVF[0], velCurrVF[1], velCurrVF[2]);
      applyVecRotationIFtoVF(allParticles[i].attitude, velCurrVF, allParticles[i].watVel);

      //output("vel diff: %f, %f, %f\n", velCurrVF[0]-velCurrEst[0], velCurrVF[1]-velCurrEst[1], velCurrVF[2]-velCurrEst[2]);
      //output("velvar: %f, %f, %f\n", 1.0/velVar[0], 1.0/velVar[1], 1.0/velVar[2]);

      for(j = 0; j < 3; j++)
         sumSquaredError += (1.0/velVar[j])*pow(velCurrVF[j] -velCurrEst[j],2);

      // output("sumSquaredError: %f\n", sumSquaredError);
      measProb = pow(2*PI,-1.0)*pow(velVar[0]*velVar[1]*velVar[2],-0.5)*
         exp(-0.5*sumSquaredError);
      //output("measProb: %f\n", measProb);
      allParticles[i].weight *= measProb;
      sumWeights += allParticles[i].weight;
   }
   //Normalize the distribution
   for(i = 0; i < nParticles; i++)
   {
      allParticles[i].weight /= sumWeights;
      sumSquaresWeights += pow(allParticles[i].weight,2);
   }
   effSampSize = 1.0/sumSquaresWeights;

   if(USE_AUG_MCL)
   {
      //Update Augmented MCL average weight parameters
      avgWeights = sumWeights/nParticles;
      if(w_slow == 0 && w_fast == 0)
      {
         w_slow = avgWeights;
         w_fast = avgWeights;
      }
      w_slow += a_slow*(avgWeights - w_slow);
      w_fast += a_fast*(avgWeights - w_fast);
   }

   //Resample the distribution if appropriate
   if(effSampSize < MIN_EFF_SAMP_SIZE*nParticles)
   {
      //plot current particle distribution if Matlab is set to be used
#ifdef USE_MATLAB
      plotMapMatlab(this->terrainMap->map.depths, 
                    this->terrainMap->map.xpts, 
                    this->terrainMap->map.ypts, 
                         "title('Sub-Map and Particle Distribution: Prior to Resampling');", 
                    "figure(2)");
      
      plotParticleDistMatlab(allParticles, "figure(2)");
#endif              
      
      //resample particle distribution
      resampParticleDist(); 
      resampled = true;
   }
   else
      resampled = false;
   
}

bool TNavParticleFilter::homerMeasUpdate(measT& currMeas)
{
   double homerPoseN[MAX_PARTICLES];
   double homerPoseE[MAX_PARTICLES];
   double homerPoseMu[2] = {0,0};
   double homerPoseCov[3] = {0,0,0};
   double homerRelPose[3] = {currMeas.alongTrack[0],currMeas.crossTrack[0],
                             currMeas.altitudes[0]};
   double homerInertPose[3] = {0,0,0};
   int i;
   double sumWeights = 0;
   double weight = 0;
   double temp1, temp2, alpha;
   //double totalRange = sqrt(homerRelPose[0]*homerRelPose[0] + homerRelPose[1]*homerRelPose[1] +
   //                         homerRelPose[2]*homerRelPose[2]);
   double range_stddev[3] = {fabs(homerRelPose[0])*HOMER_RANGE_PER_ERROR/100.0,fabs(homerRelPose[1])*HOMER_RANGE_PER_ERROR/100.0,
                             fabs(homerRelPose[2])*HOMER_RANGE_PER_ERROR/100.0};
   double currHomerPose[3];

   //Compute homer locations in inertial space from particles
   for(i = 0; i < nParticles; i++)
   {
      //add uncertainty to homer measurement based on 2.75% range error
      currHomerPose[0] = homerRelPose[0] + randn_zeroMean(range_stddev[0]);
      currHomerPose[1] = homerRelPose[1] + randn_zeroMean(range_stddev[1]);
      currHomerPose[2] = homerRelPose[2] + randn_zeroMean(range_stddev[2]);

      //rotate rel pose into inertial coordinates
      applyVecRotation(allParticles[i].attitude, currHomerPose, homerInertPose);

      //fill in homer pose based on particle vehicle pose estimate
      homerPoseN[i] = allParticles[i].position[0] + homerInertPose[0];
      homerPoseE[i] = allParticles[i].position[1] + homerInertPose[1];
   }

   //Compute mean and variance of current homer pose estimate
   for(i = 0; i < nParticles; i++)
   {
      weight = allParticles[i].weight;
      sumWeights += weight;
      
      homerPoseMu[0] += weight*homerPoseN[i];
      homerPoseMu[1] += weight*homerPoseE[i];
   }

   if(sumWeights != 1)
   {
      homerPoseMu[0] /= sumWeights;
      homerPoseMu[1] /= sumWeights;
   }
   for (i = 0; i < nParticles; i++)
   {
      weight = allParticles[i].weight;
      alpha = weight/sumWeights;
      temp1 = homerPoseN[i]-homerPoseMu[0];
      homerPoseCov[0]+=temp1*temp1*alpha;
      temp2 = homerPoseE[i]-homerPoseMu[1];
      homerPoseCov[1]+=temp2*temp2*alpha; 
      homerPoseCov[2]+=temp1*temp2*alpha;
   }
  
   //Write homer estimates to file
   
   //check that the desired file exists and is open for writing
   if(!homerParticlesFile.is_open())
   {
      output("Error:Tried to write homer location particle data to an unopened file."
             " Ignoring write command.");
      return false;
   }
   else
   {
      if(SAVE_PARTICLES)
      {
         for(i = 0; i < nParticles; i++)
            homerParticlesFile << setprecision(15) <<i<<"\t"<<allParticles[i].weight<<"\t"
                               <<homerPoseN[i]<<"\t"<<homerPoseE[i]<<endl;
      }
   }

   //check that the desired file exists and is open for writing
   if(!homerMmseFile.is_open())
   {
      output("Error:Tried to write homer location mmse data to an unopened file."
             " Ignoring write command.");
      return false;
   }
   else
   {
      homerMmseFile << setprecision(15) <<currMeas.time<<"\t"<<homerPoseMu[0]
                    << "\t"<<homerPoseMu[1]<<"\t"<<homerPoseCov[0]<<"\t"
                    << homerPoseCov[1] << "\t" << homerPoseCov[2] << endl;
   }


   return true;
}


bool TNavParticleFilter::measUpdateParticle(particleT& particle, 
                                            const Matrix& beamsSF, double* beamVar,
                                            double* beamRanges, double& newWeight)
{
	int i;
	double beamN, beamE, beamZ, mapZ;
	double sumSquaredError = 0;
	newWeight = 1;
	double mapVar = 1;
	double totalVar;
	double detSigma = 1;
	int numBeamsCorrelated = 0;
	Matrix beamsMF;
	Matrix beamsVF;
	double sumWeightedError = 0;
	double sumInvVar = 0;
	double r_pred;
	double beamU[3];
	double currDepthBias = 0;
	double currDvlAttitude[3] = {dvlAttitude[0], dvlAttitude[1],
			dvlAttitude[2]};
	double currAttitude[3] = {particle.attitude[0], particle.attitude[1],
			particle.attitude[2]};
	Eigen::Vector3f sensorOrigin, direction; 		// startPoint=sensor origin, direction=beam direction
	float estRange;

	//If searching over alignment state, first bring beams into vehicle frame
	if(SEARCH_ALIGN_STATE)
	{
		currDvlAttitude[0] += particle.alignState[0];
		currDvlAttitude[1] += particle.alignState[1];
		currDvlAttitude[2] += particle.alignState[2];
		beamsVF = applyRotation(currDvlAttitude, beamsSF);
	}
	else
		beamsVF = beamsSF;

	//Rotate the beams from the vehicle frame to the map frame
	if(ALLOW_ATTITUDE_SEARCH)
	{
		if(SEARCH_COMPASS_BIAS)
			currAttitude[2] += particle.compassBias;

		beamsMF = applyRotation(currAttitude, beamsVF);
	}
	else
		beamsMF = beamsVF;

	if(USE_OCTREE){
		sensorOrigin << static_cast<float>(particle.position[0]), 
				static_cast<float>(particle.position[1]),
				static_cast<float>(particle.position[2]);

		for(i = 0; i < beamsMF.Ncols(); i++)
		{
			beamU[0] = beamsMF(1,i+1)/beamRanges[i];
			beamU[1] = beamsMF(2,i+1)/beamRanges[i];
			beamU[2] = beamsMF(3,i+1)/beamRanges[i];
			direction << static_cast<float>(beamU[0]), 
					static_cast<float>(beamU[1]),
					static_cast<float>(beamU[2]);

			pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::AlignedPointTVector alignedPoints;


			int ret = this->octreeMap->getIntersectedVoxelCenters(sensorOrigin, direction, alignedPoints);

			if(ret>0) 
			{
				estRange = pow(pow(alignedPoints[0].x - sensorOrigin[0],2)
						+ pow(alignedPoints[0].y - sensorOrigin[1],2)
						+ pow(alignedPoints[0].z - sensorOrigin[2],2),0.5);

				totalVar = mapVar + beamVar[i];
				sumSquaredError += (1.0/(totalVar))*pow(static_cast<double>(estRange) - beamRanges[i],2);
				detSigma *= totalVar;
				numBeamsCorrelated += 1;
				sumWeightedError += (1.0/(totalVar))*(beamRanges[i] - static_cast<double>(estRange));
				sumInvVar += (1.0/(totalVar));
			}
			else
			{
				//if we don't want to use nan values, don't incorporate measurement
				if(!USE_MAP_NAN)
					return false;
				//ADD IN CODE TO HANDLE NAN VALUES
			}
		}
	}
	else{

		//Modify beam measurement vector to correspond with closest intersection point
		//of the beam's measurement cone with the terrain
		//modifyBeamDir(beamsMF, particle.position);

		//Translate each beam to the appropriate position in the map and compare its
		//depth with the interpolated map depth.

		//RANGE METHOD
		if(USE_RANGE_CORR)
		{
			for(i = 0; i < beamsMF.Ncols(); i++)
			{
				beamU[0] = beamsMF(1,i+1)/beamRanges[i];
				beamU[1] = beamsMF(2,i+1)/beamRanges[i];
				beamU[2] = beamsMF(3,i+1)/beamRanges[i];
				//compute predicted range given particle position and beam direction
				if(computeMapRayIntersection(particle.position, beamU, r_pred, mapVar))
				{
					//If the predicted range is a valid number (the beam intersects a valid map
					//region), use it in correlation
					totalVar = MAP_NOISE_MULTIPLIER*(mapVar) + beamVar[i];
					sumSquaredError += (1.0/(totalVar))*pow(fabs(r_pred) - beamRanges[i],2);
					detSigma *= totalVar;
					numBeamsCorrelated += 1;
				}
				else
				{
					//if we don't want to use nan values, don't incorporate measurement
					if(!USE_MAP_NAN)
						return false;
					//ADD IN CODE TO HANDLE NAN VALUES
				}
			}
		}
		//PROJECTION METHOD
		else
		{
			for(i = 0; i< beamsMF.Ncols(); i++)
			{
				beamN = particle.position[0] + beamsMF(1,i+1);
				beamE = particle.position[1] + beamsMF(2,i+1);
				beamZ = particle.position[2] + beamsMF(3,i+1);

				//find the terrain depth value at the current beam location
				interpolateDepth(beamN, beamE, mapZ, mapVar);

				//If the interpolated map depth is a valid number, use it in correlation
				if(!isnan(mapZ))
				{
					totalVar = mapVar + beamVar[i];
					sumSquaredError += (1.0/(totalVar))*pow(fabs(mapZ) - beamZ,2);
					detSigma *= totalVar;
					numBeamsCorrelated += 1;
					sumWeightedError += (1.0/(totalVar))*(beamZ - fabs(mapZ));
					sumInvVar += (1.0/(totalVar));
				}
				else
				{
					//if we don't want to use nan values, don't incorporate measurement
					if(!USE_MAP_NAN)
						return false;
					//ADD IN CODE TO HANDLE NAN VALUES
				}
			}
		}
	}

  //Define measurement weight of the particle.
  if(numBeamsCorrelated == 0)
     newWeight = 1.0/nParticles;
  else
  {
     //If using a contour-based TRN algorithm, use the following probability 
     //distribution:
     if(USE_CONTOUR_MATCHING && !USE_RANGE_CORR)
     {
        currDepthBias = (1.0/sumInvVar)*sumWeightedError;
        particle.position[2] -= currDepthBias;
        
        //calculate likelihood equation.
        newWeight = exp(-0.5*(sumSquaredError-2*currDepthBias*sumWeightedError+ 
                              pow(currDepthBias,2)*sumInvVar));
        //newWeight *= exp(-0.5*(currDepthBias*currDepthBias));
     }
     else
        newWeight = pow(2*PI,-0.5*numBeamsCorrelated)*pow(detSigma,-0.5)*
           exp(-0.5*sumSquaredError);
  }

  //update particle weight
  //particle.weight *= newWeight;

  return true;
}

bool TNavParticleFilter::modifiedMeasUpdateParticle(particleT& particle, 
                                            const Matrix& beamsSF, double* beamVar,
                                            double* beamRanges, double& newWeight,double* mapVariance)
{
  int i;
  double beamN, beamE, beamZ, mapZ; 
  double sumSquaredError = 0;
  newWeight = 1;
  //mapVar is passed into the interpolation to get the expected map variance
  double mapVar = 1; 
  //As Shandor is not sure where Debbie got her numbers from, he's going to assume
  //Debbie is being conservative.  So the map variance from mapVar is split into 
  //two parts by the ratio below
  double mapVarRatio = .25;
  double baseSensorVarRatio = .75;

  double modMapVar = 0;
  double baseSensorVar = 0;

  double totalVar;
  double detSigma = 1;
  int numBeamsCorrelated = 0;
  Matrix beamsMF;
  Matrix beamsVF; 
  double sumWeightedError = 0; 
  double sumInvVar = 0;
  double currDepthBias = 0;
  double currDvlAttitude[3] = {dvlAttitude[0], dvlAttitude[1], 
                               dvlAttitude[2]};
  double currAttitude[3] = {particle.attitude[0], particle.attitude[1],
                           particle.attitude[2]};
  Eigen::Vector3f sensorOrigin, direction; 		// startPoint=sensor origin, direction=beam direction
  float estRange;
  double beamU[3];

  //If searching over alignment state, first bring beams into vehicle frame
  if(SEARCH_ALIGN_STATE)
  {
     currDvlAttitude[0] += particle.alignState[0];
     currDvlAttitude[1] += particle.alignState[1];
     currDvlAttitude[2] += particle.alignState[2];
     beamsVF = applyRotation(currDvlAttitude, beamsSF);
  }
  else
     beamsVF = beamsSF;

  //Rotate the beams from the vehicle frame to the map frame
  if(ALLOW_ATTITUDE_SEARCH)
  {
     if(SEARCH_COMPASS_BIAS)
        currAttitude[2] += particle.compassBias;

     beamsMF = applyRotation(currAttitude, beamsVF);
  }
  else
     beamsMF = beamsVF;

//TODO: add octreeMap and USE_OCTREE to modified weighting
// this piece is in, but the map is checked earlier in measUpdate()
  if(USE_OCTREE){
  		sensorOrigin << static_cast<float>(particle.position[0]),
  				static_cast<float>(particle.position[1]),
  				static_cast<float>(particle.position[2]);

  		for(i = 0; i < beamsMF.Ncols(); i++)
  		{
  			beamU[0] = beamsMF(1,i+1)/beamRanges[i];
  			beamU[1] = beamsMF(2,i+1)/beamRanges[i];
  			beamU[2] = beamsMF(3,i+1)/beamRanges[i];
  			direction << static_cast<float>(beamU[0]),
  					static_cast<float>(beamU[1]),
  					static_cast<float>(beamU[2]);

  			pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::AlignedPointTVector alignedPoints;


  			int ret = this->octreeMap->getIntersectedVoxelCenters(sensorOrigin, direction, alignedPoints);
  			double mapInfoCov = 0.0;

  			if(ret>0)
  			{
  				estRange = pow(pow(alignedPoints[0].x - sensorOrigin[0],2)
  						+ pow(alignedPoints[0].y - sensorOrigin[1],2)
  						+ pow(alignedPoints[0].z - sensorOrigin[2],2),0.5);

  				modMapVar = mapVar*mapVarRatio;
  				baseSensorVar = mapVar*baseSensorVarRatio;

  			   if(mapVariance[i] > modMapVar)
  			   {
  				   mapInfoCov = mapVariance[i]-modMapVar;
  			   }
  	           else
  			   {
  				   mapInfoCov = 0.0000001;
  			   }

  			   totalVar = ((beamVar[i] + baseSensorVar + modMapVar)*mapVariance[i]+(baseSensorVar+beamVar[i])*modMapVar)/mapInfoCov;

  	           sumSquaredError += (1.0/(totalVar))*pow(static_cast<double>(estRange) - beamRanges[i],2);
  	           detSigma *= totalVar;
  	           numBeamsCorrelated += 1;
  	           sumWeightedError += (1.0/(totalVar))*(beamRanges[i] - static_cast<double>(estRange));
  	           sumInvVar += (1.0/(totalVar));
  			}
  			else
  			{
  				//if we don't want to use nan values, don't incorporate measurement
  				if(!USE_MAP_NAN)
  					return false;
  				//ADD IN CODE TO HANDLE NAN VALUES
  			}
  		}
  	}
  	else{

  //RANGE METHOD
  if(USE_RANGE_CORR)
  {
	 return false;  //Modified Algorithm currently only set up for projection method
  }
  //PROJECTION METHOD
  else
  {
     for(i = 0; i< beamsMF.Ncols(); i++)
     {
        beamN = particle.position[0] + beamsMF(1,i+1);
        beamE = particle.position[1] + beamsMF(2,i+1);
        beamZ = particle.position[2] + beamsMF(3,i+1);

        //find the terrain depth value at the current beam location
        interpolateDepth(beamN, beamE, mapZ, mapVar);
		double mapInfoCov = 0.0;

        //If the interpolated map depth is a valid number, use it in correlation
        if(!isnan(mapZ) && !isnan(beamZ))
        {
		   //totalSigma = 2.*((beamErrorCovs + mapErrorCovs).*(mapMeasCovs)+beamErrorCovs.*mapErrorCovs)./mapInfoCovs;
		   //totalVar = mapVar + beamVar[i];

			modMapVar = mapVar*mapVarRatio;
			baseSensorVar = mapVar*baseSensorVarRatio;

		   if(mapVariance[i] > modMapVar)
		   {
			   mapInfoCov = mapVariance[i]-modMapVar;
		   }
           else
		   {
			   mapInfoCov = 0.0000001;
		   }
	   
		   totalVar = ((beamVar[i] + baseSensorVar + modMapVar)*mapVariance[i]+(baseSensorVar+beamVar[i])*modMapVar)/mapInfoCov;

           sumSquaredError += (1.0/(totalVar))*pow(fabs(mapZ) - beamZ,2);
           detSigma *= totalVar;
           numBeamsCorrelated += 1;
           sumWeightedError += (1.0/(totalVar))*(beamZ - fabs(mapZ));
           sumInvVar += (1.0/(totalVar));
        }
        else
        {
           //if we don't want to use nan values, don't incorporate measurement
           if(!USE_MAP_NAN)
              return false;
           //ADD IN CODE TO HANDLE NAN VALUES 
        }
     }
  }
  	}
  //Define measurement weight of the particle.
  if(numBeamsCorrelated == 0)
     newWeight = 1.0/nParticles;
  else
  {
     //If using a contour-based TRN algorithm, use the following probability 
     //distribution:
     if(USE_CONTOUR_MATCHING && !USE_RANGE_CORR)
     {
        currDepthBias = (1.0/sumInvVar)*sumWeightedError;
        particle.position[2] -= currDepthBias;
        
        //calculate likelihood equation.
        newWeight = exp(-0.5*(sumSquaredError-2*currDepthBias*sumWeightedError+ 
                              pow(currDepthBias,2)*sumInvVar));
        //newWeight *= exp(-0.5*(currDepthBias*currDepthBias));
     }
     else
        newWeight = pow(2*PI,-0.5*numBeamsCorrelated)*pow(detSigma,-0.5)*
           exp(-0.5*sumSquaredError);
  }

  //update particle weight
  //particle.weight *= newWeight;

  return true;
}

//Update Expected Map Information
bool TNavParticleFilter::getMapInfoParticle(particleT& particle, const Matrix& beamsSF, 
							double* beamRanges, double* mapSquared, double* mapMean)
{
  int i;
  double beamN, beamE, beamZ, mapZ; 
  double mapVar = 1;
  Matrix beamsMF;
  Matrix beamsVF; 
  double currDvlAttitude[3] = {dvlAttitude[0], dvlAttitude[1], 
                               dvlAttitude[2]};
  double currAttitude[3] = {particle.attitude[0], particle.attitude[1],
                           particle.attitude[2]};

  //If searching over alignment state, first bring beams into vehicle frame
  if(SEARCH_ALIGN_STATE)
  {
     currDvlAttitude[0] += particle.alignState[0];
     currDvlAttitude[1] += particle.alignState[1];
     currDvlAttitude[2] += particle.alignState[2];
     beamsVF = applyRotation(currDvlAttitude, beamsSF);
  }
  else
     beamsVF = beamsSF;

  //Rotate the beams from the vehicle frame to the map frame
  if(ALLOW_ATTITUDE_SEARCH)
  {
     if(SEARCH_COMPASS_BIAS)
        currAttitude[2] += particle.compassBias;

     beamsMF = applyRotation(currAttitude, beamsVF);
  }
  else
     beamsMF = beamsVF;


  //RANGE METHOD
  if(USE_RANGE_CORR)
  {
	 return false;  //Modified Algorithm currently only set up for projection method
  }
  //PROJECTION METHOD
  else
  {
     for(i = 0; i< beamsMF.Ncols(); i++)
     {
        beamN = particle.position[0] + beamsMF(1,i+1);
        beamE = particle.position[1] + beamsMF(2,i+1);
        beamZ = particle.position[2] + beamsMF(3,i+1);

        //find the terrain depth value at the current beam location
        interpolateDepth(beamN, beamE, mapZ, mapVar);

        //If the interpolated map depth is a valid number, use it in correlation
        if(!isnan(mapZ) && !isnan(beamZ))
        {
			//UPDATE MAP INFORMATION PARAMETERS
			mapSquared[i] += pow(beamZ-mapZ,2)*particle.weight;
			mapMean[i] += (beamZ-mapZ)*particle.weight;
        }
        else
        {
           //if we don't want to use nan values, don't incorporate measurement
           if(!USE_MAP_NAN)
              return false;
           //ADD IN CODE TO HANDLE NAN VALUES 
        }
     }
  }
  return true;
}


  
void TNavParticleFilter::motionUpdateParticle(particleT& particle, 
                                              poseT& diffPose,
                                              double* velocity_sf_sigma,
                                              const double& gyroStddev)
{
  double currDvlAttitude[3] = {dvlAttitude[0], dvlAttitude[1], 
                               dvlAttitude[2]};
  double currAttitude[3] = {particle.attitude[0], particle.attitude[1],
                            particle.attitude[2]};
  double vehicleDisp[3];
  double psiDot, phiDot, thetaDot;
  double velocity_sf[3];
  double velocity_vf[3];
  double velocity_if[3];
  double accel_sf[3];
  double accel_vf[3];
  double accel_if[3];
  double driftStddev,cep; 
  double watVelStddev = DWAT_VEL_STDDEV*diffPose.time;
  double cosTheta = cos(particle.attitude[1]);
  double sinPhi = sin(particle.attitude[0]);
  double cosPhi = cos(particle.attitude[0]);
  double tanTheta = tan(particle.attitude[1]);
  int i;

  //Depth update is given by INS delta z
  vehicleDisp[2] = diffPose.z;
  if(!USE_CONTOUR_MATCHING)
     vehicleDisp[2] += randn_zeroMean(DZ_STDDEV);

  //If there is valid GPS data, use the stored INS pose information to perform 
  // the motion update.  Otherwise, use the stored velocity data (from DVL) and
  // vehicle attitude information from the previous time step to perform the
  // motion update.
  if(diffPose.gpsValid || !DEAD_RECKON || !lastNavPose->dvlValid)
  {
     //Add Gaussian noise to account for uncertainty in the inertial disp. 
     //measurement
     cep = (this->vehicle->driftRate/100.0)*(sqrt(diffPose.x*diffPose.x + diffPose.y*diffPose.y));
     driftStddev = sqrt(cep/sqrt(-2*(log(1-0.5))));
     vehicleDisp[0] = diffPose.x + randn_zeroMean(driftStddev);
     vehicleDisp[1] = diffPose.y + randn_zeroMean(driftStddev);
     
     //output("standard navigation update...\n");
  }
  else
  {   
     //Apply bias and scale factor corrections IFF DVL is returning ground 
     //velocity and we are searching over dvl bias/scale factor 
     if(SEARCH_DVL_ERRORS && lastNavPose->bottomLock)
     {        
        velocity_sf[0] = (1.0+particle.dvlScaleFactor)*lastNavPose->vx + 
           particle.dvlBias[0]; 
        velocity_sf[1] = (1.0+particle.dvlScaleFactor)*lastNavPose->vy + 
           particle.dvlBias[1]; 
        velocity_sf[2] = (1.0+particle.dvlScaleFactor)*lastNavPose->vz + 
           particle.dvlBias[2]; 
     }
     else
     {
        velocity_sf[0] = lastNavPose->vx;
        velocity_sf[1] = lastNavPose->vy;
        velocity_sf[2] = lastNavPose->vz;
     }
     
     //Add uniform noise if velocity is water based and gaussian otherwise
     if(!lastNavPose->bottomLock)
     {
        velocity_sf[0] += unif_zeroMean(velocity_sf_sigma[0]);
        velocity_sf[1] += unif_zeroMean(velocity_sf_sigma[1]);
        velocity_sf[2] += unif_zeroMean(velocity_sf_sigma[2]);
     }
     else
     {
        velocity_sf[0] += randn_zeroMean(velocity_sf_sigma[0]);
        velocity_sf[1] += randn_zeroMean(velocity_sf_sigma[1]);
        velocity_sf[2] += randn_zeroMean(velocity_sf_sigma[2]);
     }

     //Transform sensor frame velocities to vehicle frame:
     if(SEARCH_ALIGN_STATE)
     {
        currDvlAttitude[0] += particle.alignState[0];
        currDvlAttitude[1] += particle.alignState[1];
        currDvlAttitude[2] += particle.alignState[2];
     }
     applyVecRotation(currDvlAttitude, velocity_sf, velocity_vf);

     //Transform vehicle frame velocities to inertial frame
     if(SEARCH_COMPASS_BIAS)
        currAttitude[2] += particle.compassBias;
     
     applyVecRotation(currAttitude, velocity_sf, velocity_if);
    
     //Account for water velocity if DVL does not have bottom lock
     if(!lastNavPose->bottomLock)
     {
        if(SEARCH_WAT_VEL)
        {
           velocity_if[0] -= particle.watVel[0];
           velocity_if[1] -= particle.watVel[1];
           velocity_if[2] -= particle.watVel[2];
           
           //only update water velocity states if we don't have bottom lock
           particle.watVel[0] += randn_zeroMean(watVelStddev);
           particle.watVel[1] += randn_zeroMean(watVelStddev);
           particle.watVel[2] += randn_zeroMean(watVelStddev);
        }
        else
        {
           velocity_if[0] -= currentVel[0];
           velocity_if[1] -= currentVel[1];
           velocity_if[2] -= currentVel[2];
        }
     }
     if(SEARCH_WAT_VEL)
     {
        particle.watVel[0] += randn_zeroMean(watVelStddev);
        particle.watVel[1] += randn_zeroMean(watVelStddev);
        particle.watVel[2] += randn_zeroMean(watVelStddev);
     }
  
     //Compute vehicle displacement based on inertial velocity
     vehicleDisp[0] = velocity_if[0]*diffPose.time;
     vehicleDisp[1] = velocity_if[1]*diffPose.time;
     
     if(USE_ACCEL)
     {
        //estimate current constant acceleration
        accel_sf[0] = lastNavPose->ax + 
           randn_zeroMean(2.0*velocity_sf_sigma[0]*diffPose.time*diffPose.time);
        accel_sf[1] = lastNavPose->ay + 
           randn_zeroMean(2.0*velocity_sf_sigma[1]*diffPose.time*diffPose.time);
        accel_sf[2] = lastNavPose->az + 
           randn_zeroMean(2.0*velocity_sf_sigma[2]*diffPose.time*diffPose.time);
        
        applyVecRotation(currDvlAttitude, accel_sf, accel_vf);
        applyVecRotation(currAttitude, accel_vf, accel_if);

        //Compute vehicle displacement based on inertial velocity
        vehicleDisp[0] += 0.5*accel_if[0]*diffPose.time*diffPose.time;
        vehicleDisp[1] += 0.5*accel_if[1]*diffPose.time*diffPose.time;
     }
  }      

  //Compute the terrain displacement since the last update
  if(MOVING_TERRAIN)
  {
     Matrix particlePos(3,1);
     Matrix tempPos(3,1);
     Matrix finalPos(3,1);
     Matrix terrainDisp(3,1);
     Matrix Rmi;
     double mapAttitude[3] = {0,0,0};

     terrainDisp(1,1) = diffPose.time*particle.terrainState[0];
     terrainDisp(2,1) = diffPose.time*particle.terrainState[1];
     //this algorithm does not allow for vertical terrain motion:
     terrainDisp(3,1) = 0; 

     //Compute the new terrain-relative position in a N-E-D inertial frame
     //centered at the new position of the Map frame.
     //Map Frame psi = Vehicle Inertial Psi - Vehicle Terrainrelative Psi
     mapAttitude[2] = lastNavPose->psi - particle.attitude[2];
     Rmi = getRotMatrix(mapAttitude);
     particlePos << particle.position;
     tempPos = Rmi.t()*particlePos- terrainDisp;
     tempPos(1,1) += vehicleDisp[0];
     tempPos(2,1) += vehicleDisp[1];
     tempPos(3,1) += vehicleDisp[2];

     //Rotate the vehicle's position into the current terrain frame.
     mapAttitude[2] += diffPose.time*particle.terrainState[2];
     Rmi = getRotMatrix(mapAttitude);
     finalPos = Rmi*tempPos; 
     
     particle.position[0] = finalPos(1,1);
     particle.position[1] = finalPos(2,1);
     particle.position[2] = finalPos(3,1);
     particle.attitude[2] += diffPose.psi - 
        diffPose.time*particle.terrainState[2] + randn_zeroMean(DPSI_STDDEV);
  }
  else
  {
     particle.position[0] += vehicleDisp[0];
     particle.position[1] += vehicleDisp[1];
     particle.position[2] += vehicleDisp[2];
  }

  //Compute new heading of the particle
  if(SEARCH_GYRO_BIAS)
  { 
     if(INTEG_PHI_THETA)
     {
        //Compute psi by integrating the gyro readings
        psiDot = (sinPhi/cosTheta)*(lastNavPose->wy - particle.gyroBias[1]) + 
           (cosPhi/cosTheta)*(lastNavPose->wz - particle.gyroBias[2]);
        particle.attitude[2] += psiDot*diffPose.time;
        
        //Compute theta by integrating gyro readings
        thetaDot = cosPhi*(lastNavPose->wy - particle.gyroBias[1])
           - sinPhi*(lastNavPose->wz - particle.gyroBias[2]);
        particle.attitude[1] += thetaDot*diffPose.time;
        
        //Compute phi by integrating gyro readings
        phiDot = (lastNavPose->wx - particle.gyroBias[0]) +
           sinPhi*tanTheta*(lastNavPose->wy - particle.gyroBias[1])
           + cosPhi*tanTheta*(lastNavPose->wz - particle.gyroBias[2]);
        particle.attitude[0] += phiDot*diffPose.time;
     }
     else
     {        
        //Compute psi by integrating the gyro readings
        if(SEARCH_GYRO_Y)
           psiDot = (sinPhi/cosTheta)*(lastNavPose->wy - particle.gyroBias[0])  
              +(cosPhi/cosTheta)*(lastNavPose->wz - particle.gyroBias[1]);
        else
           psiDot = (cosPhi/cosTheta)*(lastNavPose->wz - particle.gyroBias[1]);
        particle.attitude[2] += psiDot*diffPose.time;  
     }

     //Update gyro bias terms with random noise to allow slight drift
     if(diffPose.time > 0)
     {
        if(SEARCH_GYRO_Y)
           particle.gyroBias[0] += randn_zeroMean(gyroStddev);
        particle.gyroBias[1] += randn_zeroMean(gyroStddev);
        if(INTEG_PHI_THETA)
           particle.gyroBias[2] += randn_zeroMean(gyroStddev);
     }
  }
  else
  {
     particle.attitude[2] += diffPose.psi;
  }

  //Compute the new roll/pitch of the particle
  if(!INTEG_PHI_THETA)
  {
     particle.attitude[0] += diffPose.phi;
     particle.attitude[1] += diffPose.theta;
  }

  //Add Gaussian noise to account for measurement uncertainty
  //if searching over attitude states
  if(ALLOW_ATTITUDE_SEARCH)
  {
     if(!INTEG_PHI_THETA)
     {
        particle.attitude[0] += randn_zeroMean(DPHI_STDDEV);
        particle.attitude[1] += randn_zeroMean(DTHETA_STDDEV);
     }

     if(!SEARCH_GYRO_BIAS)
        particle.attitude[2] += randn_zeroMean(DPSI_STDDEV);
  }

  //Add randomness to alignState
  if(SEARCH_ALIGN_STATE)
  {
     particle.alignState[0] += randn_zeroMean(DALIGN_STDDEV);
     particle.alignState[1] += randn_zeroMean(DALIGN_STDDEV);
     particle.alignState[2] += randn_zeroMean(DALIGN_STDDEV);
  }

  //Add randomness to DVL error states
  if(SEARCH_DVL_ERRORS)
  {
     particle.dvlScaleFactor += randn_zeroMean(DDVLSF_STDDEV);
     for(i = 0; i < 3; i++)
        particle.dvlBias[i] += randn_zeroMean(DDVLBIAS_STDDEV);
  }

  return;
}


void TNavParticleFilter::resampParticleDist()
{
  double step;
  double r;
  double c;
  double U;
  int m, N, M = 0;
  int i = 0;
  poseT mmseEst;
  
  output("TerrainNav::Resampling particle filter...\n");
  
  //If using Augmented MCL, add some purely random samples to the resampled 
  //distribution
  if(USE_AUG_MCL)
  {
     //For a fraction of the particles based on average weights, add random 
     //samples
     M = int(0.1*(1.0 - w_fast/w_slow)*nParticles);
     if(M < 0)
        M = 0;
     
     //output("number of random samples to generate: %i\n", M);
     computeMMSE(&mmseEst);
     for (m = 0; m < M; m++)
     {
        resampParticles[m] = allParticles[0];
        resampParticles[m].weight = 1.0/nParticles;
     }
  }
  N = nParticles - M;

  //Use the low-variance sampling algorithm as outlined in
  //"Probabilistic Robotics" by Thrun, Burgard, Fox - pg. 110
  step = 1.0/N;
  r = (rand()+1)*(1.0/(RAND_MAX+1.0)) * 1.0/nParticles;
  c = allParticles[0].weight;
  for (m = 0; m < N; m++){
    U = r + m*step;

    while (c < U){
      i++;
      c += allParticles[i].weight;
    }
    
    //copy state of current particles to resampled particles
    resampParticles[m+M] = allParticles[i];
    resampParticles[m+M].weight = 1.0/nParticles;  
  }	    

  //if(saveDirectory != NULL)
  //   writeParticlesToFile(resampParticles, resampParticlesFile);

  //swap in resampled distribution
  updateParticleDist();
}

void TNavParticleFilter::getDistBounds(double& Nmin, double& Nmax, double& Emin, double& Emax)
{
  int i;

  Nmin = allParticles[0].position[0];
  Nmax = Nmin;
  Emin = allParticles[0].position[1];
  Emax = Emin;
    
  for(i=0;i<nParticles;i++){
    Nmin = min(Nmin, allParticles[i].position[0]);
    Nmax = max(Nmax, allParticles[i].position[0]);
    Emin = min(Emin, allParticles[i].position[1]);
    Emax = max(Emax, allParticles[i].position[1]);
  }
  return;
}


int TNavParticleFilter::defineAndLoadSubMap(const Matrix &beamsVF)
{
   poseT mmseEst;
   double maxAttitude[3];
   Matrix beamsMF;
   double max_dx, max_dy, dx, dy;
   double widthPhi, widthTheta;
   int i;
   double Nmin, Nmax, Emin, Emax;
   double mapSearch[2] = {0, 0};
   double numXdesired, numYdesired;
   int mapStatus;

   //determine current particle spread
   computeMMSE(&mmseEst);

   //check that variances are non-zero
   if(mmseEst.covariance[4] > 0.001*PI/180.0)
      widthPhi = sqrt(mmseEst.covariance[4]);
   else
      widthPhi = 0.0;

   if(mmseEst.covariance[5] > 0.001*PI/180.0)
      widthTheta = sqrt(mmseEst.covariance[5]);
   else
      widthTheta = 0.0;

   //determine max attitude angles from current particle spread
   maxAttitude[0] = max(fabs(mmseEst.phi + 3.0*widthPhi),
                        fabs(mmseEst.phi - 3.0*widthPhi));
   maxAttitude[1] = max(fabs(mmseEst.theta + 3.0*widthTheta),
                        fabs(mmseEst.theta - 3.0*widthTheta));
   maxAttitude[2] = mmseEst.psi;

   //determine beam projection for maximum attitude
   beamsMF = applyRotation(maxAttitude, beamsVF);
   
   //find maximum projection of beams
   max_dx = 0;
   max_dy = 0;

   for(i = 0; i < beamsMF.Ncols(); i++)
   {
      dx = fabs(beamsMF(1,i+1));
      if(dx > max_dx)
         max_dx = dx;
           
      dy = fabs(beamsMF(2,i+1));
      if(dy > max_dy)
         max_dy = dy;
   }
   
   //find maximum particle North/East locations
   getDistBounds(Nmin, Nmax, Emin, Emax);

   //load submap based on max particle locations and projections.
   numXdesired = (Nmax-Nmin)/2.0 + 1.5*max_dx + 
      2*fabs(terrainMap->refMap->bounds->dx);
   numYdesired = (Emax-Emin)/2.0 + 1.5*max_dy + 
      2*fabs(terrainMap->refMap->bounds->dy);  

   mapSearch[0] = 2.0*numXdesired; 
   mapSearch[1] = 2.0*numYdesired; 
   
   //ask to extract a map based on vehicle location and search bounds
   mapStatus = loadSubMap((Nmax-Nmin)/2.0+Nmin,(Emax-Emin)/2.0+Emin, mapSearch);

   //return MAPBOUNDS_OUT_OF_BOUNDS;
   return mapStatus;
}


void TNavParticleFilter::getParticlePose(const particleT& particle, 
                                         poseT* particlePose)
{
  particlePose->x = particle.position[0];
  particlePose->y = particle.position[1];
  particlePose->z = particle.position[2];

  particlePose->phi = particle.attitude[0];
  particlePose->theta = particle.attitude[1];
  particlePose->psi = particle.attitude[2];

  if(SEARCH_COMPASS_BIAS)
     particlePose->psi += particle.compassBias;

  particlePose->time = this->lastNavPose->time;

  return;
}

void TNavParticleFilter::updateParticleDist()
{
   //temporary pointer
   particleT* tempPointer;

   tempPointer = resampParticles;
   
   //swap pointers
   resampParticles = allParticles;
   allParticles = tempPointer;
}

double TNavParticleFilter::computeKLdiv_gaussian_particles()
{
   Matrix Value(1,1);
   ColumnVector dx(2); 
   SymmetricMatrix Cov(2);
   Matrix A;  
   double q;
   int i;
   double eta;
   double kl = 0;
   poseT mmseEst;
   double mu[2];

   //compute mean estimate
   computeMMSE(&mmseEst);
   
   mu[0] = mmseEst.x;
   mu[1] = mmseEst.y;
   Cov(1,1) = mmseEst.covariance[0];
   Cov(2,2) = mmseEst.covariance[1];
   Cov(2,1) = mmseEst.covariance[2];

   //compute gaussian normalization factor
   A = 2.0*PI*Cov;  
   eta = pow(A.Determinant(),-0.5);

   //compute inverse of covariance for gaussian calculation
   A = Cov.i();

   //sum KL over all entries
   for(i = 0; i < nParticles; i++)
   {
      dx(1) = allParticles[i].position[0] - mu[0];
      dx(2) = allParticles[i].position[1] - mu[1];
      
      //compute current guassian probability
      Value = dx.t()*A*dx;
      q = eta*exp(Value.AsScalar()*-0.5);

      //add current kl entry
      if(allParticles[i].weight/q > 1e-50 && allParticles[i].weight/q < 1e50)
         kl += allParticles[i].weight*log(allParticles[i].weight/q);
   }

   return kl;
}

void TNavParticleFilter::plotMapMatlab(const Matrix &Surf, double* xpts, 
                                       double* ypts, const char* plotTitle, 
                                       const char* figureNum)
{
#ifdef USE_MATLAB
   mxArray *Map = NULL;
   mxArray *MATLABxpts = NULL;
   mxArray *MATLABypts = NULL;
   mxArray *NavPoseX = NULL;
   mxArray *NavPoseY = NULL;

   //copy contents of Surf into Matlab variables Map, MATLABxpts, MATLABypts
   Map = mxCreateDoubleMatrix(Surf.Ncols(), Surf.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(Map), (void *) Surf.Store(), 
          Surf.Storage()*sizeof(double));
   engPutVariable(matlabEng, "Map", Map);
   
   MATLABxpts = mxCreateDoubleMatrix(1, Surf.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(MATLABxpts),(void *)xpts,Surf.Nrows()*sizeof(double));
   engPutVariable(matlabEng, "xpts", MATLABxpts);

   MATLABypts = mxCreateDoubleMatrix(1, Surf.Ncols(), mxREAL);
   memcpy((void *)mxGetPr(MATLABypts),(void *)ypts,Surf.Ncols()*sizeof(double));
   engPutVariable(matlabEng, "ypts", MATLABypts);

   //copy nav pose information into Matlab variables navPoseX and navPoseY
   NavPoseX = mxCreateDoubleMatrix(1, 1, mxREAL);
   memcpy((void *)mxGetPr(NavPoseX), (void *) &lastNavPose->x, sizeof(double));
   engPutVariable(matlabEng, "navPoseX", NavPoseX);

   NavPoseY = mxCreateDoubleMatrix(1, 1, mxREAL);
   memcpy((void *)mxGetPr(NavPoseY), (void *) &lastNavPose->y, sizeof(double));
   engPutVariable(matlabEng, "navPoseY", NavPoseY);


   //plot the loaded submap surface
   engEvalString(matlabEng, figureNum);
   engEvalString(matlabEng, "hold off;");
   engEvalString(matlabEng, "imagesc(ypts-navPoseY,xpts-navPoseX, Map')");
   engEvalString(matlabEng, "ylabel('North (m)'); xlabel('East (m)')");
   engEvalString(matlabEng, "axis equal;");
   engEvalString(matlabEng, "set(gcf,'color','w');");
   engEvalString(matlabEng, "colorbar;");
   engEvalString(matlabEng, plotTitle);  
   engEvalString(matlabEng, "hold on;");
   
   //remove memory in Matlab
   mxDestroyArray(Map);
   mxDestroyArray(MATLABxpts);
   mxDestroyArray(MATLABypts);
   mxDestroyArray(NavPoseX);
   mxDestroyArray(NavPoseY);

#else
      output("Could not generate plot; Matlab is not set to be used");
#endif
}

void TNavParticleFilter::plotParticleDistMatlab(particleT* particles, 
                                                const char* figureNum)
{
#ifdef USE_MATLAB
   mxArray *matlabPart = NULL;
   int i;
   Matrix allPart(nParticles,3);

   //fill Matrix with particle pose and weight information
   for(i = 0; i < nParticles; i++)
   {
      allPart(i+1,1) = particles[i].position[0] - lastNavPose->x;
      allPart(i+1,2) = particles[i].position[1] - lastNavPose->y;
      allPart(i+1,3) = particles[i].weight;
   }

   //put matrix into matlab
   matlabPart = mxCreateDoubleMatrix(3, nParticles, mxREAL);
   memcpy((void *)mxGetPr(matlabPart), (void *) allPart.Store(), 
          allPart.Storage()*sizeof(double));
   engPutVariable(matlabEng, "A", matlabPart);

   //plot particle positions
   engEvalString(matlabEng, figureNum);
   engEvalString(matlabEng, "if(exist('h','var')); delete(h); end;");
   engEvalString(matlabEng, "scatter(A(2,:), A(1,:), A(3,:)*50000, 'w')");
   
   //remove memory in Matlab
   mxDestroyArray(matlabPart);
#else
   output("Could not generate plot; Matlab is not set to be used");
#endif

}

void TNavParticleFilter::plotBeamMatlab(const double& npos, const double& epos,
                                        const double& zpos, const double& beamN,
                                        const double& beamE, const double& beamZ
                                        , const char* figureNum)
{
#ifdef USE_MATLAB
   mxArray *beamParams = NULL;
   Matrix beamInfo(1,6);

   beamInfo(1,1) = npos;
   beamInfo(1,2) = epos;
   beamInfo(1,3) = zpos;
   beamInfo(1,4) = beamN;
   beamInfo(1,5) = beamE;
   beamInfo(1,6) = beamZ;

   //put matrix into matlab
   beamParams = mxCreateDoubleMatrix(1, 6, mxREAL);
   memcpy((void *)mxGetPr(beamParams), (void *) beamInfo.Store(), 
          beamInfo.Storage()*sizeof(double));
   engPutVariable(matlabEng, "beamParams", beamParams);

   //plot particle positions
   engEvalString(matlabEng, figureNum);
   engEvalString(matlabEng, "line([beamParams(1) beamParams(4)], [beamParams(2) beamParams(5)], [beamParams(3) beamParams(6)])");
   //engEvalString(matlabEng, "save /home/pkimball/latestRunWorkspace");      
   
   //remove memory in Matlab
   mxDestroyArray(beamParams);
#else
   output("Could not generate plot; Matlab is not set to be used");
#endif
}

void TNavParticleFilter::writeParticlesToFile(particleT* particles, 
                                              ofstream &particlesFile)
{
   int i;

   //write particle information to file for all particles
   for(i = 0; i < nParticles; i++)
   {
      particlesFile << setprecision(15) <<i<<"\t"<<particles[i].weight<<"\t"
                    <<particles[i].position[0]<<"\t"<<particles[i].position[1]
                    <<"\t"<<particles[i].position[2];
      if(ALLOW_ATTITUDE_SEARCH)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].attitude[0]
                       <<"\t"<<particles[i].attitude[1]
                       <<"\t"<<particles[i].attitude[2];
      if(MOVING_TERRAIN)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].terrainState[0]
                       <<"\t"<<particles[i].terrainState[1]
                       <<"\t"<<particles[i].terrainState[2];
      if(SEARCH_COMPASS_BIAS)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].compassBias;

      if(SEARCH_ALIGN_STATE)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].alignState[0]
                       <<"\t"<<particles[i].alignState[1]
                       <<"\t"<<particles[i].alignState[2];
      if(SEARCH_GYRO_BIAS)
      {
         particlesFile <<setprecision(15)<<"\t"<<particles[i].gyroBias[0]
                       <<"\t"<<particles[i].gyroBias[1];
         if(INTEG_PHI_THETA)
            particlesFile <<setprecision(15)<<"\t"<<particles[i].gyroBias[2];
      }
      if(SEARCH_WAT_VEL)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].watVel[0]
                       <<"\t"<<particles[i].watVel[1]
                       <<"\t"<<particles[i].watVel[2];
      if(SEARCH_DVL_ERRORS)
         particlesFile <<setprecision(15)<<"\t"<<particles[i].dvlScaleFactor
                       <<"\t"<<particles[i].dvlBias[0]
                       <<"\t"<<particles[i].dvlBias[1]
                       <<"\t"<<particles[i].dvlBias[2];

      particlesFile <<endl;
    
   }
}


void TNavParticleFilter::writeHistDistribToFile(particleT* particles, 
                                                ofstream &particlesFile)
{
   int i, idx;
   double Nmin, Nmax, Emin, Emax, Zmin, Zmax, Phmin, Phmax, Tmin, Tmax, Pmin, Pmax;
   double dN, dE, dZ, dPh, dT, dP;
   RowVector likeN, likeE, likeZ, likePh, likeT, likeP;
   int numN, numE, numZ, numPh, numT, numP;

   //compute histogram distributions for each filter state
   //find minimum/maximum particle North/East locations
   getDistBounds(Nmin, Nmax, Emin, Emax);

   //find minimum/maximum particle Depth and Heading
   Zmin = allParticles[0].position[2];
   Zmax = Zmin;
   Pmin = allParticles[0].attitude[2];
   if(SEARCH_COMPASS_BIAS)
     Pmin += allParticles[0].compassBias;
   Pmax = Pmin;
   Phmin = allParticles[0].attitude[0];
   Phmax = Phmin;
   Tmin = allParticles[0].attitude[1];
   Tmax = Tmin;
  
   for(i=0;i<nParticles;i++){
      Zmin = min(Zmin, allParticles[i].position[2]);
      Zmax = max(Zmax, allParticles[i].position[2]);
      Phmin = min(Phmin, allParticles[i].attitude[0]);
      Phmax = max(Phmax, allParticles[i].attitude[0]);
      Tmin = min(Tmin, allParticles[i].attitude[1]);
      Tmax = max(Tmax, allParticles[i].attitude[1]);
      if(SEARCH_COMPASS_BIAS)
      {
         Pmin = min(Pmin, allParticles[i].attitude[2]+allParticles[i].compassBias);
         Pmax = max(Pmax, allParticles[i].attitude[2]+allParticles[i].compassBias);
      }
      else
      {
         Pmin = min(Pmin, allParticles[i].attitude[2]);
         Pmax = max(Pmax, allParticles[i].attitude[2]);
      }
   }

   dN = 0.1;
   dE = 0.1;
   dZ = 0.01;
   dP = 0.001;
   dPh = 0.001;
   dT = 0.001;
   
   numN = int((Nmax-Nmin+dN)/dN);
   numE = int((Emax-Emin+dE)/dE);
   numZ = int((Zmax-Zmin+dZ)/dZ);
   numPh = int((Phmax-Phmin+dPh)/dPh);
   numT = int((Tmax-Tmin+dT)/dT);
   numP = int((Pmax-Pmin+dP)/dP);


   //create histogram vectors
   likeN.ReSize(numN);
   likeE.ReSize(numE);
   likeZ.ReSize(numZ);
   likePh.ReSize(numPh);
   likeT.ReSize(numT);
   likeP.ReSize(numP);  

   likeN = 0.0;
   likeE = 0.0;
   likeZ = 0.0;
   likePh = 0.0;
   likeT = 0.0;
   likeP = 0.0;

   //fill in histogram vectors
   for(i = 0; i < nParticles; i++)
   {
      //fill in likeN
      idx = closestPtUniformArray(particles[i].position[0], Nmin, Nmax,
                                  numN);
      likeN(idx+1) += particles[i].weight;

      //fill in likeE
      idx = closestPtUniformArray(particles[i].position[1], Emin, Emax,
                                  numE);
      likeE(idx+1) += particles[i].weight;
      
      //fill in likeZ
      idx = closestPtUniformArray(particles[i].position[2], Zmin, Zmax,
                                  numZ);
      likeZ(idx+1) += particles[i].weight;

      //fill in likePh
      idx = closestPtUniformArray(particles[i].attitude[0], Phmin, Phmax,
                                  numPh);
      likePh(idx+1) += particles[i].weight;

      //fill in likeT
      idx = closestPtUniformArray(particles[i].attitude[1], Tmin, Tmax,
                                  numT);
      likeT(idx+1) += particles[i].weight;

      //fill in likeP
      if(SEARCH_COMPASS_BIAS)
         idx = closestPtUniformArray(particles[i].attitude[2]
                                     +particles[i].compassBias, Pmin, Pmax,
                                     numP);
      else
         idx = closestPtUniformArray(particles[i].attitude[2], Pmin, Pmax,
                                     numP);
      likeP(idx+1) += particles[i].weight;
   }

   //write histogram information to file
   particlesFile << 1 << "\t" << Nmin << "\t" << Nmax << "\t" << numN << endl;
   particlesFile << likeN;
   particlesFile << 2 << "\t" << Emin << "\t" << Emax << "\t" << numE << endl;
   particlesFile << likeE;
   particlesFile << 3 << "\t" << Zmin << "\t" << Zmax << "\t" << numZ << endl;
   particlesFile << likeZ;
   particlesFile << 4 << "\t" << Phmin << "\t" << Phmax << "\t" << numPh << endl;
   particlesFile << likePh;
   particlesFile << 5 << "\t" << Tmin << "\t" << Tmax << "\t" << numT << endl;
   particlesFile << likeT;
   particlesFile << 6 << "\t" << Pmin << "\t" << Pmax << "\t" << numP << endl;
   particlesFile << likeP;

}


