sample.cpp

一个很好的粒子滤波算法

#include "sample.h"
#include "random_utils.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

/**
 * please note, a few of these functions were developed originally to be
 * used in a group robotics project; but they have been modified significantly
 * for this framework.
 */


/**
 * create a new Sample.  See Set for parameter descriptions
 */
Sample::Sample(int modelNum, 
	       int phaseLeft, double alphaLeft, double rhoLeft, 
	       int phaseRight, double alphaRight, double rhoRight,
	       double weight)
{

  Set(modelNum, phaseLeft, alphaLeft, rhoLeft, phaseRight, alphaRight,
      rhoRight, weight);
}

/**
 * create a new, unitialized sample (all params set to 0)
 */
Sample::Sample()
{
  Set(0,0,0,0,0, 0, 0, 0);
}

/**
 * set the parameters of the set
 * @param modelNum the index of the modelNum
 * @param phase indicates the current position in the model
 * @param alpha the amplitude scaling factor
 * @param rho the time scaling factor
 * @param weight -- the weight of this sample (proportional to its prob)
 */
void Sample::Set(int modelNum, 
		 int phaseLeft, double alphaLeft, double rhoLeft,
		 int phaseRight, double alphaRight, double rhoRight,
		 double weight)
{
  this->modelNum = modelNum;
  this->phaseLeft = phaseLeft;
  this->alphaLeft = alphaLeft;
  this->rhoLeft = rhoLeft;
  
  this->phaseRight = phaseRight;
  this->alphaRight = alphaRight;
  this->rhoRight = rhoRight;

  this->weight = weight;
}


/****************
 * definitions for SampleSet
 */

/**
 * create a new SampleSet, with numSamples
 * samples in it
 */
SampleSet::SampleSet(int numSamples, Model **models, int numModels)
{
  int i;
  //store info on the models
  this->numModels = numModels;
  this->models = models;

  this->numSamples = numSamples;
  
  samples = new Sample *[numSamples];
  nextSamples = new Sample *[numSamples];

  cumulativeSamples = new CumulativeSample *[numSamples];
  for(i = 0; i < numSamples; i++) {
    samples[i] = new Sample();   
    nextSamples[i] = new Sample();
    cumulativeSamples[i] = new CumulativeSample();
  }

  srand(time(NULL)); //seed random number generate  


  this->numObservations=0;
  Init();
}

/**
 * clean up memory allocated in constructor
 */
SampleSet::~SampleSet()
{
  for(int i = 0; i < numSamples; i++) {
    delete samples[i];
    delete nextSamples[i];
  }

  delete samples;
  delete nextSamples;
}

/**
 * for now, just initialize as in Black & Jepson paper
 */
void SampleSet::Init()
{
  for(int i = 0; i < numSamples; i++) {
    InitSample(samples[i], 0);
  }
}

/**
 * initialize a specific sample uniformly
 */
void SampleSet::InitSample(Sample *sample, int startPhase)
{
  double weight = 1.0 / numSamples;
  
  int modelNum;
  int phaseLeft;
  double alphaLeft;
  double rhoLeft;
  int phaseRight;
  double alphaRight;
  double rhoRight;
  
  int modelLength;
  double sqr;
  
 
  modelNum = RandomInt(0, numModels); //pick modelNum uniformly
  
  //pick phase as (1 - sqrt(y))/sqrt(y) 
  //must be a legitimate phase for the given model
  modelLength = models[modelNum]->GetLength();
  do {
    sqr = sqrt(RandomDouble(.00000000000001, 1));
    phaseLeft = startPhase + (int)((1.0 - sqr) / sqr);
  } while(phaseLeft > modelLength - 1);
  

  do {
    sqr = sqrt(RandomDouble(.00000000000001, 1));
    phaseRight = startPhase + (int)((1.0 - sqr) / sqr);
  } while(phaseRight > modelLength - 1);
  

  alphaLeft = RandomDouble(ALPHA_MIN, ALPHA_MAX);
  alphaRight = RandomDouble(ALPHA_MIN, ALPHA_MAX);

  rhoLeft = RandomDouble(RHO_MIN, RHO_MAX);
  rhoRight = RandomDouble(RHO_MIN, RHO_MAX);

  
  /*
    printf("initializing to: %d, %d, %f, %f, %f\n", modelNum, phase,
    alpha, rho, weight);
  */
 
  sample->Set(modelNum, phaseLeft, alphaLeft, rhoLeft, 
	      phaseRight, alphaRight, rhoRight, weight);
}

/**
 * update with new observations
 */
void SampleSet::Update(double hvelLeft, double vvelLeft, double hvelRight,
		       double vvelRight)
{
  Sample *sample, *nextSample;
  int modelNum, phaseLeft, phaseRight;
  double alphaLeft, rhoLeft, oldAlphaLeft, oldRhoLeft;
  double alphaRight, rhoRight, oldAlphaRight, oldRhoRight; 
  double weight;
  
  AddObservation(hvelLeft, vvelLeft, hvelRight, vvelRight);
  MakeCumulativeTable(samples, numSamples);

  //samples to reinit
  int numHoldout = (int)(REINIT_PERCENT * numSamples);

  for(int i = 0; i < numHoldout; i++) {
    nextSamples[i]->SetWeight(-1);
  }

  int numBad = numHoldout; //number of "bad" samples that need to be reinited
  double totalWeight = 0.0;
  for(int i = numHoldout; i < numSamples; i++) {
    sample = ChooseRandomSample();
    nextSample = nextSamples[i]; 

    for(int numAttempts = 0; numAttempts < MAX_UPDATE_ATTEMPTS;
	numAttempts++) {
      modelNum = sample->GetModelNum();
      
      phaseLeft = (int)((double)sample->GetPhaseLeft() + sample->GetRhoLeft() +
			RandomGaussian(0, DIFFUSION_MODEL_NUM));
      
      phaseRight = (int)((double)sample->GetPhaseRight() + sample->GetRhoRight() +
			 RandomGaussian(0, DIFFUSION_MODEL_NUM));


      int length = models[modelNum]->GetLength();
      if(phaseLeft >= length || phaseRight >= length) {
	nextSample->SetWeight(-1); //will be reinited later
	numBad++;
	//InitSample(nextSample); //past the end of the phase, reinit
	break; //break out of attempts loop
      } else {
	oldAlphaLeft = sample->GetAlphaLeft();

	do {
	  alphaLeft = oldAlphaLeft + RandomGaussian(0, DIFFUSION_ALPHA);
	} while(alphaLeft < ALPHA_MIN || alphaLeft > ALPHA_MAX);

	oldAlphaRight = sample->GetAlphaRight();


	do {
	  alphaRight = oldAlphaRight + RandomGaussian(0, DIFFUSION_ALPHA);
	} while(alphaRight < ALPHA_MIN || alphaRight > ALPHA_MAX);

	
	oldRhoLeft = sample->GetRhoLeft();
	do {
	  rhoLeft = oldRhoLeft + RandomGaussian(0, DIFFUSION_RHO);
	} while(rhoLeft < RHO_MIN || rhoLeft > RHO_MAX);
	
	oldRhoRight = sample->GetRhoRight();
	
	do {
	  rhoRight = oldRhoRight + RandomGaussian(0, DIFFUSION_RHO);
	} while(rhoRight < RHO_MIN || rhoRight > RHO_MAX);


	weight = CalcWeight(modelNum, phaseLeft, rhoLeft, alphaLeft, 
			    phaseRight, rhoRight, alphaRight); 
	
	if(weight > 0.0) {
	  nextSample->Set(modelNum, phaseLeft, alphaLeft, rhoLeft, 
			  phaseRight, alphaRight, rhoRight, weight);
	  totalWeight += weight;
	  break;
	}
      }

      //our last time through the loop and we've failed
      if(numAttempts == MAX_UPDATE_ATTEMPTS - 1) {
	nextSample->SetWeight(-1); //will be reinited later
	numBad++;
	//InitSample(nextSample); //generate a random initial sample
      }
    }

  }

  //renormalize
  totalWeight += numBad * (totalWeight / (numSamples - numBad)); 
					    
  for(int i = 0; i < numSamples; i++) {
    nextSample = nextSamples[i];
    weight = nextSample->GetWeight();
    if(weight == -1) {
      InitSample(nextSample, numObservations);
    } else {
      nextSample->SetWeight(weight/totalWeight);
    }
  }

  //swap buffers
  Sample **temp = samples;
  samples = nextSamples;
  nextSamples = temp;

}

/**
 * calculate the weight of a sample, based on its parameters.
 * Uses equation 1 in Black & Jepson to calc p(z_t|s_t)
 * to look back over a time window and see how well
 * the observations match those predicted by the
 * model
 * @return the weight of the sample
 */
double SampleSet::CalcWeight(int modelNum, int phaseLeft, double rhoLeft, 
			     double alphaLeft, int phaseRight,
			     double rhoRight, double alphaRight) 
{
  double constant = 1 / (sqrt(2.0 * PI) * SIGMA_I);
  //double dividObs = numObservations == 1 ? 1 : numObservations-1;
  double dividObs = numObservations;
  double divisor = 2.0 * SIGMA_I * SIGMA_I * dividObs;

  Model *model = models[modelNum];
  double hvelLeftErrorSum = 0.0;
  double vvelLeftErrorSum = 0.0;

  double hvelRightErrorSum = 0.0;
  double vvelRightErrorSum = 0.0;

  double error;
  for(int j = 0; j < numObservations; j++) {
    error = hvelLeftObservations[numObservations - 1 - j] - 
      alphaLeft * model->InterpolateLeftHvel(phaseLeft - rhoLeft*((double)j));

    error = error * error;
    hvelLeftErrorSum += error;


    error = vvelLeftObservations[numObservations - 1 - j] - 
      alphaLeft *  model->InterpolateLeftVvel(phaseLeft - rhoLeft*((double)j));

    error = error * error;
    vvelLeftErrorSum += error;

    error = hvelRightObservations[numObservations - 1 - j] - 
      alphaRight * model->InterpolateRightHvel(phaseRight - rhoRight*((double)j));

    error = error * error;
    hvelRightErrorSum += error;


    error = vvelRightObservations[numObservations - 1 - j] - 
      alphaRight *  model->InterpolateRightVvel(phaseRight - rhoRight*((double)j));

    error = error * error;
    vvelRightErrorSum += error;
  }

  double hvelLeftWeight = constant * exp(-hvelLeftErrorSum / divisor);
  double vvelLeftWeight = constant * exp(-vvelLeftErrorSum / divisor);

  double hvelRightWeight = constant * exp(-hvelRightErrorSum / divisor);
  double vvelRightWeight = constant * exp(-vvelRightErrorSum / divisor);

  double weight = hvelLeftWeight * vvelLeftWeight * hvelRightWeight * vvelRightWeight;
  //printf("weight = %e\n", weight);
  return weight;
}

/**
 * we keep a window of old observations of length
 * WINDOW; add a new observations to it
 */
void SampleSet::AddObservation(double hvelLeft, double vvelLeft, 
			       double hvelRight, double vvelRight)
{
  if(numObservations < WINDOW) {
    hvelLeftObservations[numObservations] = hvelLeft;
    vvelLeftObservations[numObservations] = vvelLeft;
    hvelRightObservations[numObservations] = hvelRight;
    vvelRightObservations[numObservations] = vvelRight;
    numObservations++;
  } else { //shift down one
    for(int i = 0; i < WINDOW-1; i++) {
      hvelLeftObservations[i] = hvelLeftObservations[i+1];
      vvelLeftObservations[i] = vvelLeftObservations[i+1];      
      hvelRightObservations[i] = hvelRightObservations[i+1];
      vvelRightObservations[i] = vvelRightObservations[i+1];      

    }

    hvelLeftObservations[WINDOW-1] = hvelLeft;
    vvelLeftObservations[WINDOW-1] = vvelLeft;   
    hvelRightObservations[WINDOW-1] = hvelRight;
    vvelRightObservations[WINDOW-1] = vvelRight;    
  }
}

/**
 * build up a table of cumulative weights
 */
void SampleSet::MakeCumulativeTable(Sample **samplesToAccumulate, int nSamples)
{
  double weightSoFar = 0.0;
  Sample *sample;
  CumulativeSample *cumSample;
  double modelWeights[numModels];
  for(int i = 0; i < numModels; i++) {
    modelWeights[i] = 0.0;
  }

  for(int i = 0; i < nSamples; i++) {
    sample = samplesToAccumulate[i];
    modelWeights[sample->GetModelNum()] += sample->GetWeight();
    cumSample = cumulativeSamples[i];
    weightSoFar += sample->GetWeight();
    cumSample->cumulativeWeight = weightSoFar;
    cumSample->sample = sample;
  }

  for(int i = 0; i < numModels; i++) {
    printf("%f\t", modelWeights[i]);
    //printf("modelWeights[%d] = %f\n", i, modelWeights[i]);
  }
  //printf("Cumulative total: %e\n\n", weightSoFar);
  

}

/**
 * choose a random sample from the cumulative table,
 * by picking a uniformly random number and then
 * choosing the sample in the cumulative table that is
 * >= that number by the least amount
 */
Sample *SampleSet::ChooseRandomSample()
{
  double total = cumulativeSamples[numSamples-1]->cumulativeWeight;
  
  double searchKey = RandomDouble(0, total);
  

  CumulativeSample **cumSamplePtr = 
    (CumulativeSample **)bsearch(&searchKey, cumulativeSamples, 
				 numSamples, sizeof(CumulativeSample *),
				 CumulativeSample::Comparator);
  if(cumSamplePtr == NULL) {
    printf("Error, binary search yielded NULL. THIS SHOULD NOT HAPPEN\n");
    printf("total = %f, searchKey = %f\n", total, searchKey);

    return samples[0]; //return a default value to keep from crashing
  }

  CumulativeSample *cumSample = *cumSamplePtr; //deref our pointer
 
  return cumSample->sample;
}



/****************************
 * definitions for CumulativeSample class
 */


/**
 * used by binary search,
 * @param key should be a pointer to a double which is the 
 * cumulative weight we are looking for
 * @param element is a pointer to a CumulativeSample * in the array to see
 * if the key is less than, greater than, or equal to this element
 * @return -1 if the key is less than, 0 if equal, 1 if greater than element
 */
int CumulativeSample::Comparator(const void *key, const void *element)
{
  double cumWeight = *((double *)key);
  CumulativeSample *cumSample = *((CumulativeSample **)element);
  
  if(cumWeight < cumSample->cumulativeWeight - cumSample->sample->GetWeight())
    return -1;
  if(cumWeight > cumSample->cumulativeWeight)
    return 1;

  return 0; //cumWeight lies in the range spanned by this item
}