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 <stdlib.h>
#include <assert.h>

#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);
	}
}