www.pudn.com > Particlefilter_code.zip > Particle.cpp

////////////////////////////////////////////////////////////////////////////////////// 
// 
//	Implementation 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. 
// 
#include  
#include  
 
#include "Particle.h" 
#include "MemAlloc.h" 
#include "utility.h" 
 
#ifndef NULL 
#define NULL 0 
#endif 
/////////////////////////////////////////////////////////////////////////////////////// 
// 
//	Constructors and Deconstructors 
// 
/////////////////////////////////////////////////////////////////////////////////////// 
CParticle::CParticle() 
{ 
	m_nStDim = 0; 
	m_nSamplesNum = 0; 
} 
CParticle::CParticle(int nStateDim, int nSamplesNum) 
{ 
	m_nStDim = nStateDim; 
	m_nSamplesNum = nSamplesNum; 
 
	if (nStateDim == 0 || nSamplesNum == 0) 
		return; 
	m_flState = (float *) malloc(nStateDim * sizeof(float)); 
	m_flConfidence = (float *) malloc(nSamplesNum * sizeof(float)); 
	m_flCumulative = (float *) malloc(nSamplesNum * sizeof(float)); 
	m_flPriorProb = (float *) malloc(nSamplesNum * sizeof(float)); 
	m_flProposalProb = (float *)malloc(nSamplesNum * sizeof(float)); 
 
	m_flSamples = NULL; 
	AllocMem(m_flSamples, (long)nStateDim, (long)nSamplesNum); 
} 
CParticle::~CParticle() 
{ 
	if (m_nStDim != 0 && m_nSamplesNum != 0) 
	{ 
		free(m_flState); 
		free(m_flConfidence); 
		free(m_flCumulative); 
		free(m_flPriorProb); 
		free(m_flProposalProb); 
		m_flSamples = FreeMem(m_flSamples); 
	} 
} 
 
////////////////////////////////////////////////////////////////////////////////////// 
// 
//	Common routines for all type particle filters  
// 
////////////////////////////////////////////////////////////////////////////////////// 
 
//////////////////////////////////////////////////////////////////// 
// 
//	Resample(int *index) 
//Purpos: 
//	Return the resampled index 
//Params: 
//	index[IN]/[OUT] -------------------------- index for the samples 
// 
//////////////////////////////////////////////////////////////////// 
void CParticle::Resample(int *index) 
{ 
	if (index == NULL) 
		return ; 
	float urandnum = 0;  
	int i, j; 
	//TODO: 
	//	Replaced by bi-Search 
	//	XinFan 2003.5.25 
	for (i = 0; i < m_nSamplesNum; i++) 
	{ 
		urandnum = uniform_random(); 
		for (j = 0; j < m_nSamplesNum - 1; j++) 
		{ 
			if (*(m_flCumulative + j) >= urandnum) break; 
		} 
//		*(index + i) = j; 
		*(index + i) = (j > (m_nSamplesNum - 1)) ?   
			(m_nSamplesNum - 1) : j; 
//		*(index + i) = i; 
	} 
} 
////////////////////////////////////////////////////////////////////// 
// 
//	float * MeanSample(float **Samples) 
// 
//Purpose: 
//	Obtain the mean of the samples with corresponding weights 
////////////////////////////////////////////////////////////////////// 
void CParticle::MeanSample(float *mean, float ** const Samples) 
{ 
	memset(mean, 0, m_nStDim * sizeof(float)); 
	float *samples_i = NULL; 
	float weight = 0.0; 
	for (int i = 0; i < m_nSamplesNum; i++)	 
	{ 
		//the i-th Sample 
		samples_i = *(Samples + i); 
		weight = *(m_flConfidence + i); 
		for (int j = 0; j < m_nStDim; j++) 
		{ 
			//With weights 
			*(mean + j) += *(samples_i + j) * weight;			 
		} 
	} 
	// 
	//Since the weights are normlized  
	// 
//	for (i = 0; i < m_nStDim; i++) 
//	{ 
//		*(mean + i) /= m_nSamplesNum; 
//	} 
 
} 
///////////////////////////////////////////////////////////////////// 
// 
//	NormWeights() 
/////////// 
//Purpose: 
//	Normalize sample weights 
//////////////////////////////////////////////////////////////////// 
void CParticle::NormWeights() 
{ 
	float sum = 0.0; 
	for (int i = 0; i < m_nSamplesNum; i++) 
	{ 
		sum += *(m_flConfidence + i); 
	} 
	for (i = 0; i < m_nSamplesNum; i++) 
	{ 
		*(m_flConfidence + i) /= sum; 
	} 
} 
/////////////////////////////////////////////////////////////////// 
// 
// CalCumulative() 
////////////////// 
//Purpose: 
//	Calculate Cumulative Probability 
/////////////////////////////////////////////////////////////////// 
void CParticle::CalCumulative() 
{ 
	memset(m_flCumulative, 0, m_nSamplesNum * sizeof(float)); 
	*m_flCumulative = *m_flConfidence; 
	for (int i = 1; i < m_nSamplesNum; i++) 
	{ 
		*(m_flCumulative + i) = *(m_flCumulative + i - 1) +  
			*(m_flConfidence + i); 
	} 
}