sample.h

一个很好的粒子滤波算法

#ifndef _SAMPLE_H_
#define _SAMPLE_H_

#include "model.h"

//30% scaling
#define ALPHA_MIN 0.5 //0.7
#define ALPHA_MAX 1.5 //1.3
#define RHO_MIN 0.5  //0.7
#define RHO_MAX 1.5 //1.3

//in black and Jepson they take standard deviations as 1.0 for model
//trajectories (?)
#define SIGMA_I 1.0

#define DIFFUSION_RHO .1 //.01
#define DIFFUSION_MODEL_NUM .1 //.05
#define DIFFUSION_ALPHA .1 //.02
#define WINDOW 10

#define MAX_UPDATE_ATTEMPTS 10

//the percent of the sample set we reinitialize
//at each update to avoid local maxima
#define REINIT_PERCENT (.1)

#define PI 3.14159265358979323846264338327


class Sample
{
 public:
  Sample(int modelNum, int phaseLeft, double alphaLeft, double rhoLeft, 
	 int phaseRight, double alphaRight, double rhoRight, double weight);  
  Sample();

  ~Sample() { };
  
   void Set(int modelNum, int phaseLeft, double alphaLeft, double rhoLeft, 
	    int phaseRight, double alphaRight, double rhoRight, double weight);
   void SetWeight(double weight) { this->weight = weight; }

   double GetWeight() { return weight;}
   int GetModelNum() { return modelNum; }
   int GetPhaseLeft() { return phaseLeft; }
   double GetAlphaLeft() { return alphaLeft; }
   double GetRhoLeft() { return rhoLeft; }

   int GetPhaseRight() { return phaseRight; }
   double GetAlphaRight() { return alphaRight; }
   double GetRhoRight() { return rhoRight; }


 private:
  int modelNum;
  int phaseLeft;
  double alphaLeft;
  double rhoLeft;

  int phaseRight;
  double alphaRight;
  double rhoRight;

  double weight;
 
};

/**
 * a simple class to hold a Sample and the cumulative weight of that
 * sample in a cumulative table. 
 * Also has a comparator to be used by bsearch
 */
class CumulativeSample 
{
 public:
  CumulativeSample() { sample = NULL; cumulativeWeight = 0;}

  Sample *sample;
  double cumulativeWeight;

  static int Comparator(const void *key, const void *element);
};

class SampleSet
{
 public:
  SampleSet(int numSamples, Model **models, int numModels);
  ~SampleSet();
  void Init();
  void InitSample(Sample *sample, int startPhase);
  //update the set with new data
  void Update(double hvelLeft, double vvelLeft, double hvelRight, double vvelRight); 

 private:
  void AddObservation(double hvelLeft, double vvelLeft, double hvelRight,
		      double vvelRight);
  void MakeCumulativeTable(Sample **samplesToAccumulate, int nSamples);
  Sample *ChooseRandomSample();
  double CalcWeight(int modelNum, int phaseLeft, double rhoLeft, 
		    double alphaLeft, int phaseRight, double rhoRight,
		    double alphaRight); 

  int numSamples;
  Sample **samples; //array of pointers to Samples
  Sample **nextSamples; //used during prediction phase

  CumulativeSample **cumulativeSamples; //cumulative table of samples

  int numModels;  //number of models
  Model **models; //array of ptrs to models

  double hvelLeftObservations[WINDOW];
  double vvelLeftObservations[WINDOW];
  double hvelRightObservations[WINDOW];
  double vvelRightObservations[WINDOW];

  int numObservations;  
};

#endif _SAMPLE_H_