#include <CParticleFilter.h>

Public Member Functions
void	allocate (int num_particles)
mcl::pose	averageOverNBestAndRandomize (int N, int M=0, float vx=0, float vy=0, float va=0)
	CParticleFilter ()
	default constructor
mcl::pose	getDistributionMean (bool doWeighting=false)
Eigen::Matrix3d	getDistributionVariances ()
void	initializeNormalRandom (mcl::pose p0, mcl::pose variance, int size)
void	initializeUniform (mcl::pose Pmin, mcl::pose Pmax, mcl::pose dP)
void	myfree ()
void	normalize ()
void	predict (mcl::pose dP, mcl::pose std)
void	predict (mcl::pose dP, float Q[4])
void	print ()
void	resize (int n)
void	saveToFile (int Fileind)
void	SIRUpdate ()
void	updateLikelihood (float *lik)
	~CParticleFilter ()
	destructor
Public Attributes
mcl::pose	average
	Distribution mean.
bool	isAvgSet
	Tells if the distribution mean has been calculated.
float	Lik
	Likelihood of the distribution.
ownRandom	myrand
	The random number class.
int	NumOfParticles
	Number of particles in distribution.
float	outLiers
	Percentage of the outlier measurements in distribution.
TPoseParticle *	Particles
	Particle distribution.
int	size
	reserved memory Size (number of reserved particles!)
mcl::pose	variance
	Distribution variance.
Private Member Functions
void	hpsrt (int *indx)
Private Attributes
TPoseParticle *	tmp
	Particle distribution for SIR (Maintained for efficiency reasons here)

Detailed Description

The Particle filter

Definition at line 358 of file CParticleFilter.h.

Constructor & Destructor Documentation

CParticleFilter::CParticleFilter ( )

default constructor

Definition at line 9 of file CParticleFilter.cpp.

CParticleFilter::~CParticleFilter ( )

destructor

Definition at line 17 of file CParticleFilter.cpp.

Member Function Documentation

void CParticleFilter::allocate ( int num_particles )

Allocates memory for the distribution num_particles The amount of particles you want to reserve

Allocates memory for the distribution. You should do this before using the filter num_particles The amount of particles you want to reserve

Definition at line 25 of file CParticleFilter.cpp.

mcl::pose CParticleFilter::averageOverNBestAndRandomize	(	int	N,
		int	M = `0`,
		float	vx = `0`,
		float	vy = `0`,
		float	va = `0`
	)

Computes the best indices and returns the average

Parameters:

N	The best particles
M	Option to randomize the worst M
vx	variance x
vy	variance y
va	variance a

< find indices for the best ones

Definition at line 519 of file CParticleFilter.cpp.

mcl::pose CParticleFilter::getDistributionMean ( bool doWeighting = false )

Get the mean values of the distribution

Returns:: mcl::pose

Definition at line 59 of file CParticleFilter.cpp.

Eigen::Matrix3d CParticleFilter::getDistributionVariances ( )

Calculate the variance of the distribution

Calculate the variance of the distribution //xx xy xa// //xy yy ya// //xa ya aa//

Definition at line 113 of file CParticleFilter.cpp.

void CParticleFilter::hpsrt ( int * indx ) [private]

Heap Sort algorithm

Parameters:

*indx	The index of values after sort
N	The number of values

Definition at line 565 of file CParticleFilter.cpp.

void CParticleFilter::initializeNormalRandom	(	mcl::pose	p0,
		mcl::pose	variance,
		int	size
	)

Initializes the filter by sampling from normal distribution with mean in and variance defined by

Definition at line 157 of file CParticleFilter.cpp.

void CParticleFilter::initializeUniform	(	mcl::pose	Pmin,
		mcl::pose	Pmax,
		mcl::pose	dP
	)

Initializes the distribution using uniform distribution The box Pmin and Pmax sets the borders and dP sets the step size Pmin the minimum values Pmax maximum values dP step size

< number of particles for each component

If something stupid should happen above

Definition at line 182 of file CParticleFilter.cpp.

void CParticleFilter::myfree ( )

Frees the reserved memory

Definition at line 48 of file CParticleFilter.cpp.

void CParticleFilter::normalize ( )

Performs the normalization step i.e. according to updated likelyhoods the probability of each particle is calculated and the whole distribution gets probablity of 1

Definition at line 303 of file CParticleFilter.cpp.

void CParticleFilter::predict	(	mcl::pose	dP,
		mcl::pose	std
	)

Performs the prediction step Moves the cloud according to estimated movement() and adds noise according to Assumes independed noise (i.e the x,y,a are not correlated, which is not really true)

Parameters:

dP	relative movement between the scans
std	The standard deviation of the noise that is independently added to each component

Modified 25.9.2008 to induce the noise into the differential movement instead of into the trajectory in world frame

<estimates

Generate noise from normal distribution

Definition at line 330 of file CParticleFilter.cpp.

void CParticleFilter::predict	(	mcl::pose	dP,
		float	Q[4]
	)

Performs prediction step with system noise covariance matrix Moves the particles according to and samples the noise Note that the angle noise is not correlated with the position using covariance matrix [4] dP relative motion between steps Q[4] Uncertainty as covariance (Var(X) Var(XY) Var(Y) STD(A)!!)

Performs prediction step with system noise covariance matrix Moves the particles according to and samples the noise Note that the angle noise is not correlated with the position using covariance matrix [4] dP relative motion between steps Q[4] Uncertainty as covariance (Var(X) Var(XY) Var(Y) STD(A)!!) FIXME: NOT TESTED

Transform the correlated noise according to the heading of the particle FIXME: Remember to test the validity as this is not tested

Definition at line 371 of file CParticleFilter.cpp.

void CParticleFilter::print ( )

Prints some status data

Definition at line 480 of file CParticleFilter.cpp.

void CParticleFilter::resize ( int n )

Resizes the distribution so that n particles from indices *ind will be saved NOTE:: The memory will not be reallocated

n the size of *ind and the "new" vector

Resizes the distribution so that n particles from indices *ind will be saved NOTE:: The memory will not be reallocated n the size of the "new" vector

< find indices for the best ones

Definition at line 495 of file CParticleFilter.cpp.

void CParticleFilter::saveToFile ( int Fileind )

Definition at line 616 of file CParticleFilter.cpp.

void CParticleFilter::SIRUpdate ( )

Performs the Sample Importance Resampling (SIR) algorithm for the distribution The algorithm chooses the best particles (with respect to the probability) and resamples these.

You should have updated the likelihoods and normalized the distribution before running this Also, it might be smart not to run this in every iteration, since the distribution looses accuracy due to the "discretation"

j++; ///WAS HERE until 30.7.2008

Definition at line 226 of file CParticleFilter.cpp.

void CParticleFilter::updateLikelihood ( float * lik )

You Can use this to update the vector of likelihoods Most probably you will use this update step so that you give the whole filter to some likelihood function, which updates the public data TPoseParticle *Particles directly however, this is here if you like to use it :)

Parameters: