particle.h

关于时下流行的粒子滤波程序的源码

//////////////////////////////////////////////////////////////////////////////////////
//
//	Header file for an abstract class to describe particle filter
//
//
//	XinFan 2003.5.25
//
//Reference:
//	[1] S. Arulampalam and S. Maskell and N. Gordon and T. Clapp,"A Tutorial on Particle Filters for On-line 
//		Non-linear/Non-Gaussian Bayesian Tracking",IEEE Transactions On Signal Processing, Vol. 50(2), 
//		pages 174-188, February 2002.
//	[2] Jun S. Liu and Rong Chen, "Sequential {Monte Carlo} Methods for Dynamic Systems", 
//		Journal of the American Statistical Association, Vol. 93, No. 443, pp.1032--1044, 1998
//	[3] Gordon, N., Salmond, D., and Smith, A. ." Novel approach to nonlinear/non-Gaussian 
//		Bayesian state estimation". IEE Proc. F, 140, 2, 107-113.
//
#ifndef	_PARTICLE_H
#define _PARTICLE_H

//Enumerate Inference Methods with Monte Carlo Samples, Ref. [2]
enum MCEST
{
	MCEST_MEAN,
	MCEST_MAP,
	MCEST_RAO,
	MCEST_DELAY
};

class CParticle
{
public:
	CParticle();
	CParticle(int nStateDim, int nSamplesNum);
	//CParticle(int nStateDim, int nSamplesNum, int nTimeStep);
	virtual ~CParticle();
public:
	//Predict Equation			
	virtual void UpdateByTime(int nStep) = 0;
	//Evaluate weights with measurments
	virtual void EvalWeight(int nStep) = 0;		
	//Get estimated states
	virtual void * GetState(int nStep) const = 0;
	//Initialization, customized by applications
	void Initialize(){}
	//Estimate the state at current step
	void EstState(MCEST est);						
	//A simple resample routine, see ref.[3]
	void Resample(int *index);						
//////////////////////////////////////////////////////////////////
//	Convenient Access Functions
//////////////////////////////////////////////////////////////////
public:
protected:
	//the mean of the samples with corresponding weights
	void MeanSample(float *mean,  float** const Samples);
	//Normalize sample weights
	void NormWeights();	
	//Calculate Cumulative probability
	void CalCumulative();							

//member variables
protected:
	int m_nStDim;			//Dimension of state vector
							//Since the mesurement various with applications
							//Mesurements are not defined in this abstract class
	int m_nCurStep;			//Indicate the current step number
	int m_nSamplesNum;		//Number of the Samples
	float *m_flState;		//Estimated state
	float **m_flSamples;	//Array of the Sample Vectors, each row of which is a sample
	float *m_flConfidence;	//Confidence for each sample
	float *m_flCumulative;	//Cumulative confidence
	float *m_flPriorProb;	//Evaluated Prior Transition Probability
	float *m_flProposalProb;//Evaluated Proposal Probablity
	//float ***m_flStream;	//Stream of Sample Vectors
};



#endif