Public Types
enum	InitModes { INIT_MODE_UNIFORM =0, INIT_MODE_GAUSSIAN }

enum	EstimationModes { MEAN =0, WEIGHTED_MEAN, ROBUST_MEAN, BEST_PARTICLE }

Public Member Functions
	ParticleFilter ()

	ParticleFilter (const ParticleFilter &rhs)

virtual	~ParticleFilter ()

PARTICLE &	operator[] (const unsigned int &i)

const PARTICLE &	operator[] (const unsigned int &i) const

PARTICLE &	operator() (const unsigned int &i)

const PARTICLE &	operator() (const unsigned int &i) const

ParticleFilter &	operator= (const ParticleFilter &rhs)

virtual bool	init (const unsigned int numParticles, const Vector< VectorFloat > &initModel, const VectorFloat &processNoise, const VectorFloat &measurementNoise)

virtual bool	initParticles (const UINT numParticles)

virtual bool	filter (SENSOR_DATA &data)

virtual bool	clear ()

virtual bool	reset ()

bool	getInitialized () const

unsigned int	getNumParticles () const

unsigned int	getStateVectorSize () const

unsigned int	getNumDeadParticles () const

unsigned int	getInitMode () const

unsigned int	getEstimationMode () const

Float	getEstimationLikelihood () const

Float	getWeightSum () const

VectorFloat	getStateEstimation () const

Vector< PARTICLE >	getParticles ()

Vector< PARTICLE >	getOldParticles ()

VectorFloat	setProcessNoise () const

bool	setVerbose (const bool verbose)

bool	setNormalizeWeights (const bool normWeights)

bool	setResampleThreshold (const Float resampleThreshold)

bool	setEstimationMode (const unsigned int estimationMode)

bool	setInitMode (const unsigned int initMode)

bool	setInitModel (const Vector< VectorFloat > initModel)

bool	setProcessNoise (const VectorFloat &processNoise)

bool	setMeasurementNoise (const VectorFloat &measurementNoise)

Protected Member Functions
virtual bool	predict (PARTICLE &p)

virtual bool	update (PARTICLE &p, SENSOR_DATA &data)

virtual bool	normalizeWeights ()

virtual bool	computeEstimate ()

virtual bool	checkForResample ()

virtual bool	resample ()

virtual bool	preFilterUpdate (SENSOR_DATA &data)

virtual bool	postFilterUpdate (SENSOR_DATA &data)

Float	gauss (Float x, Float mu, Float sigma)

Float	rbf (const Float x, const Float mu, Float sigma, Float weight=1.0)

Float	rbf (const VectorFloat &x, const VectorFloat &mu, Float sigma, Float weight=1.0)

Float	SQR (const Float x)

Protected Attributes
bool	initialized
	A flag that indicates if the filter has been initialized.

bool	verbose
	A flag that indicates if warning and info messages should be printed.

bool	normWeights
	A flag that indicates if the weights should be normalized at each filter iteration.

unsigned int	numParticles
	The number of particles in the filter.

unsigned int	stateVectorSize
	The size of the state Vector (x)

unsigned int	initMode
	The mode used to initialize the particles, this should be one of the InitModes enums.

unsigned int	estimationMode
	The estimation mode (used to compute the state estimation)

unsigned int	numDeadParticles

Float	minimumWeightThreshold
	Any weight below this value will not be resampled.

Float	robustMeanWeightDistance
	The distance parameter used in the ROBUST_MEAN estimation mode.

Float	estimationLikelihood
	The likelihood of the estimated state.

Float	wNorm
	Stores the total weight norm value.

Float	wDotProduct
	Stores the dot product of all the weights, used to test for degeneracy.

Float	resampleThreshold
	The threshold below which the particles will be resampled.

VectorFloat	x
	The state estimation.

Vector< VectorFloat >	initModel
	The noise model for the initial starting guess.

VectorFloat	processNoise
	The noise covariance in the system.

VectorFloat	measurementNoise
	The noise covariance in the measurement.

Vector< PARTICLE > &	particles
	A reference to the current active particle Vector.

Vector< PARTICLE >	particleDistributionA
	A Vector of particles, this will either hold the particles before or after a resample.

Vector< PARTICLE >	particleDistributionB
	A Vector of particles, this will either hold the particles before or after a resample.

VectorFloat	cumsum
	The cumulative sum Vector used for resampling the particles.

Random	rand
	A random number generator.

WarningLog	warningLog

ErrorLog	errorLog

Detailed Description

template<class PARTICLE, class SENSOR_DATA>
class ParticleFilter< PARTICLE, SENSOR_DATA >

Definition at line 41 of file ParticleFilter.h.

Constructor & Destructor Documentation

template<class PARTICLE, class SENSOR_DATA>

ParticleFilter< PARTICLE, SENSOR_DATA >::ParticleFilter ( )

inline

Default Constructor

Definition at line 47 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

ParticleFilter< PARTICLE, SENSOR_DATA >::ParticleFilter ( const ParticleFilter< PARTICLE, SENSOR_DATA > & rhs )

inline

Copy Constructor

Definition at line 69 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual ParticleFilter< PARTICLE, SENSOR_DATA >::~ParticleFilter ( )

inlinevirtual

Default Destructor

Definition at line 76 of file ParticleFilter.h.

Member Function Documentation

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::checkForResample ( )

inlineprotectedvirtual

This function checks to see if the particles should be resampled for the next iteration of filtering. This is a virtual function, so you can override it in your derived class if needed.

Returns: returns true if the particles should be resampled, false otherwise

Definition at line 734 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::clear ( )

inlinevirtual

Clears any previous setup.

Returns: returns true if the filter was cleared successfully, false otherwise

Reimplemented in ParticleClassifierParticleFilter.

Definition at line 292 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::computeEstimate ( )

inlineprotectedvirtual

The function computes the state estimate, using the current particles. The exact method used to compute the estimate depends on the current estimationMode. This is a virtual function, so you can override it in your derived class if needed.

Returns: returns true if the state estimation was correctly computed, false otherwise

Definition at line 634 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::filter ( SENSOR_DATA & data )

inlinevirtual

The main filter function that should be called at each time step.

Parameters

SENSOR_DATA &data: the template data structure containing any data you need to pass to the update function.

Returns: returns true if the filter was run successfully, false otherwise

Definition at line 224 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::gauss	(	Float	x,
		Float	mu,
		Float	sigma
	)

inlineprotected

Computes the Gaussian likelihood for the input x, given mu and sigma.

Parameters

Float	x: the x value for the Gaussian distrubution
Float	mu: the mu value for the Gaussian distrubution
Float	sigma: the sigma value for the Gaussian distrubution

Returns: returns the Gaussian probabilty for the input x, given mu and sigma

Definition at line 855 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::getEstimationLikelihood ( ) const

inline

Gets how sure the particle filter is about the estimated state. This will be a Float in the range of [0 1] (or INF if the likelihood is INF).

Returns: returns a Float representing the estimation likelihood

Definition at line 395 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

unsigned int ParticleFilter< PARTICLE, SENSOR_DATA >::getEstimationMode ( ) const

inline

Gets the current estimationMode.

Returns: returns an unsigned int representing the current estimationMode

Definition at line 385 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::getInitialized ( ) const

inline

Returns: returns if the particle filter has been initialized, false otherwise

Definition at line 340 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

unsigned int ParticleFilter< PARTICLE, SENSOR_DATA >::getInitMode ( ) const

inline

Gets the current initMode.

Returns: returns an unsigned int representing the current initMode

Definition at line 376 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

unsigned int ParticleFilter< PARTICLE, SENSOR_DATA >::getNumDeadParticles ( ) const

inline

Gets the number of dead particles. Dead particles are particles with weights of INF.

Returns: returns the number of dead particles

Definition at line 367 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

unsigned int ParticleFilter< PARTICLE, SENSOR_DATA >::getNumParticles ( ) const

inline

Gets the number of particles.

Returns: returns the number of particles if the filter is initialized, zero otherwise

Definition at line 349 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Vector< PARTICLE > ParticleFilter< PARTICLE, SENSOR_DATA >::getOldParticles ( )

inline

The function returns the Vector of particles before they were resampled.

Returns: returns a Vector with the old particles (i.e. before they were resampled

Definition at line 431 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Vector< PARTICLE > ParticleFilter< PARTICLE, SENSOR_DATA >::getParticles ( )

inline

The function returns the current Vector of particles. These are the particles that have been resampled.

Returns: returns a Vector with the current particles

Definition at line 422 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

VectorFloat ParticleFilter< PARTICLE, SENSOR_DATA >::getStateEstimation ( ) const

inline

Gets the current state estimation Vector.

Returns: returns a VectorFloat containing the current state estimation

Definition at line 413 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

unsigned int ParticleFilter< PARTICLE, SENSOR_DATA >::getStateVectorSize ( ) const

inline

Gets the number of dimensions in the state Vector.

Returns: returns the number of dimensions in the state Vector the filter is initialized, zero otherwise

Definition at line 358 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::getWeightSum ( ) const

inline

Gets this sum of all the weights. This is the value that is used to normalize the weights.

Returns: returns a Float representing the sum of all the weights

Definition at line 404 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::init	(	const unsigned int	numParticles,
		const Vector< VectorFloat > &	initModel,
		const VectorFloat &	processNoise,
		const VectorFloat &	measurementNoise
	)

inlinevirtual

Initializes the particles. The size of the init model sets the number of dimensions in the state Vector. The size of the process noise and measurement noise Vectors can be different, depending on the exact problem you are using the ParticleFilter to solve.

Definition at line 160 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::initParticles ( const UINT numParticles )

inlinevirtual

The function initializes the particles at time t=0. This is a virtual function, so you can override it in your derived class if needed.

Returns: returns true if the state estimation was correctly computed, false otherwise

Definition at line 195 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::normalizeWeights ( )

inlineprotectedvirtual

This function normalizes the particle weights so they sum to 1. This is a virtual function, so you can override it in your derived class if needed.

Returns: returns true if the particle weights were normalized successfully, false otherwise

Definition at line 588 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

PARTICLE& ParticleFilter< PARTICLE, SENSOR_DATA >::operator() ( const unsigned int & i )

inline

Provides direct access to the i'th particle before it was resampled. It is the user's responsibility to ensure i is within bounds.

Parameters

const unsigned int &i: the index of the particle you want

Returns: returns a reference to the i'th particle (if i is valid)

Definition at line 105 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

const PARTICLE& ParticleFilter< PARTICLE, SENSOR_DATA >::operator() ( const unsigned int & i ) const

inline

Provides direct constant access to the i'th particle before it was resampled. It is the user's responsibility to ensure i is within bounds.

Parameters

const unsigned int &i: the index of the particle you want

Returns: returns a reference to the i'th particle (if i is valid)

Definition at line 115 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

ParticleFilter& ParticleFilter< PARTICLE, SENSOR_DATA >::operator= ( const ParticleFilter< PARTICLE, SENSOR_DATA > & rhs )

inline

Defines the equals operator.

Definition at line 122 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

PARTICLE& ParticleFilter< PARTICLE, SENSOR_DATA >::operator[] ( const unsigned int & i )

inline

Provides direct access to the i'th particle. It is the user's responsibility to ensure i is within bounds.

Parameters

const unsigned int &i: the index of the particle you want

Returns: returns a reference to the i'th particle (if i is valid)

Definition at line 85 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

const PARTICLE& ParticleFilter< PARTICLE, SENSOR_DATA >::operator[] ( const unsigned int & i ) const

inline

Provides direct constant access to the i'th particle. It is the user's responsibility to ensure i is within bounds.

Parameters

const unsigned int &i: the index of the particle you want

Returns: returns a reference to the i'th particle (if i is valid)

Definition at line 95 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::postFilterUpdate ( SENSOR_DATA & data )

inlineprotectedvirtual

This function lets you define a custom post filter update if needed.

Parameters

SENSOR_DATA &data: a reference to the sensor data input to the main filter function.

Returns: returns true if the update was completed successfully, false otherwise

Reimplemented in ParticleClassifierParticleFilter.

Definition at line 843 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::predict ( PARTICLE & p )

inlineprotectedvirtual

This is the main predict function. This function must be implemented in your derived class.

Parameters

PARTICLE &p: the current particle that should be passed through your prediction code

Returns: returns true if the particle prediction was updated successfully, false otherwise

Reimplemented in ParticleClassifierParticleFilter, and FSMParticleFilter.

Definition at line 562 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::preFilterUpdate ( SENSOR_DATA & data )

inlineprotectedvirtual

This function lets you define a custom pre filter update if needed.

Parameters

SENSOR_DATA &data: a reference to the sensor data input to the main filter function.

Returns: returns true if the update was completed successfully, false otherwise

Reimplemented in ParticleClassifierParticleFilter.

Definition at line 834 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::rbf	(	const Float	x,
		const Float	mu,
		Float	sigma,
		Float	weight = `1.0`
	)

inlineprotected

Computes the Radial Basic Function (RBF) for the input x, given mu, alpha, and the RBF weight. For speed, this function does not check to make sure the size of x and mu are the same. The user must therefore ensure that mu has the same size as x before they call this function.

Parameters

const	Float x: the x value for the RBF function
const	Float mu: the center of the RBF function
Float	sigma: the sigma value for the RBF function
Float	weight: the weight for this RBF function. Default value=1.0

Returns: returns the RBF function output for input x, given mu, alpha and the weight

Definition at line 870 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::rbf	(	const VectorFloat &	x,
		const VectorFloat &	mu,
		Float	sigma,
		Float	weight = `1.0`
	)

inlineprotected

Computes the Radial Basic Function (RBF) for the input x, given mu, alpha, and the RBF weight. For speed, this function does not check to make sure the size of x and mu are the same. The user must therefore ensure that mu has the same size as x before they call this function.

Parameters

const	VectorFloat &x: the x value for the RBF function
const	VectorFloat &mu: the center of the RBF function
Float	sigma: the sigma value for the RBF function
Float	weight: the weight for this RBF function. Default value=1.0

Returns: returns the RBF function output for input x, given mu, alpha and the weight

Definition at line 885 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::resample ( )

inlineprotectedvirtual

The main resample function. Resamples the particles based on the particles weights. This is a virtual function, so you can override it in your derived class if needed.

Returns: returns true if the particles were correctly resampled, false otherwise

Definition at line 744 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setEstimationMode ( const unsigned int estimationMode )

inline

Sets the estimation mode. This should be one of the EstimationModes.

Parameters

const unsigned int estimationMode: the new estimation mode (must be one of the EstimationModes enums)

Returns: returns true if the estimationMode was successfully updated, false otherwise

Definition at line 485 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setInitMode ( const unsigned int initMode )

inline

Sets the init mode. This should be one of the InitModes.

Parameters

const unsigned int initMode: the new init mode (must be one of the InitMode enums)

Returns: returns true if the initMode was successfully updated, false otherwise

Definition at line 501 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setInitModel ( const Vector< VectorFloat > initModel )

inline

Sets the init model. This is the noise model that is used for the initial starting guess when the user calls the initParticles(UINT numParticles) function.

The new init model Vector size must match the size of the current init model Vector.

Parameters

const Vector< VectorFloat > initModel: a Vector containing the new init model for the particle filter

Returns: returns true if the initModel was successfully updated, false otherwise

Definition at line 519 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setMeasurementNoise ( const VectorFloat & measurementNoise )

inline

Sets the measurement noise. The new measurement noise Vector size must match the size of the current measurement noise Vector.

Parameters

const VectorFloat &measurementNoise: a Vector containing the new measurement noise for the particle filter

Returns: returns true if the measurementNoise was successfully updated, false otherwise

Definition at line 547 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setNormalizeWeights ( const bool normWeights )

inline

Sets if the particle weights should be updated at each filter iteration.

Parameters

const bool normWeights: the new normWeights mode

Returns: returns true if the normWeights was successfully updated, false otherwise

Definition at line 461 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

VectorFloat ParticleFilter< PARTICLE, SENSOR_DATA >::setProcessNoise ( ) const

inline

Gets the process noise Vector.

Returns: returns a Float Vector containing the process noise

Definition at line 440 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setProcessNoise ( const VectorFloat & processNoise )

inline

Sets the process noise. The new process noise Vector size must match the size of the current process noise Vector.

Parameters

const VectorFloat &processNoise: a Vector containing the new process noise for the particle filter

Returns: returns true if the processNoise was successfully updated, false otherwise

Definition at line 533 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setResampleThreshold ( const Float resampleThreshold )

inline

Sets the threshold used to determine if the particles should be resampled. The particles will be resampled if the wNorm value is less than the resampleThreshold. The resampleThreshold should be in the range [0 1], normally something like: 1.0e-20 works well.

Parameters

const Float resampleThreshold: the new resampleThreshold

Returns: returns true if the parameter was successfully updated, false otherwise

Definition at line 474 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

bool ParticleFilter< PARTICLE, SENSOR_DATA >::setVerbose ( const bool verbose )

inline

Sets the verbose mode.

Parameters

const bool verbose: the new verbose mode

Returns: returns true if the verboseMode was successfully updated, false otherwise

Definition at line 450 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

Float ParticleFilter< PARTICLE, SENSOR_DATA >::SQR ( const Float x )

inlineprotected

Computes the square of the input.

Parameters

const Float x: the value you want to compute the square of

Returns: returns the square of the input

Definition at line 900 of file ParticleFilter.h.

template<class PARTICLE, class SENSOR_DATA>

virtual bool ParticleFilter< PARTICLE, SENSOR_DATA >::update	(	PARTICLE &	p,
		SENSOR_DATA &	data
	)

inlineprotectedvirtual

This is the main update function in which the weight of the particle should be computed, based on the particles current estimated state and the latest sensor data.

This function must be implemented in your derived class.

Parameters

PARTICLE	&p: the current particle
SENSOR_DATA	&data: the current sensor data

Returns: returns true if the particle update was updated successfully, false otherwise

Reimplemented in ParticleClassifierParticleFilter, and FSMParticleFilter.

Definition at line 577 of file ParticleFilter.h.

The documentation for this class was generated from the following file:

ParticleFilter.h

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<class PARTICLE, class SENSOR_DATA> class ParticleFilter< PARTICLE, SENSOR_DATA >

Constructor & Destructor Documentation

Member Function Documentation

template<class PARTICLE, class SENSOR_DATA>
class ParticleFilter< PARTICLE, SENSOR_DATA >