ParticleFilter< ParticleT > Class Template Reference

Implements a particle filter with support for a variety of applications through the usage of arbitrary combination of a variety of models and policies. More...

#include <ParticleFilter.h>

Inheritance diagram for ParticleFilter< ParticleT >:

[legend]

Detailed Description

template<typename ParticleT>
class ParticleFilter< ParticleT >

Implements a particle filter with support for a variety of applications through the usage of arbitrary combination of a variety of models and policies.

The particle type is passed as a template parameter, which provides the implementation advantage of being able to directly store arrays of particles as contiguous blocks in memory. This allows better cache coherency and enables platform-specific acceleration tricks, such as SIMD calls.

There are a number of embedded classes which together form the implementation of the particle filter. The advantage of this architecture is that you can mix and match any combination of modules to get the features needed for your application.

SensorModel: pass one of these to updateSensors in order to evaluate the particles as new information is discovered. You may have several different sensors at the same time, simply create a model for each type of sensor, and pass it to the filter when updated.
MotionModel: modifies particle states based on the expected outcome of any controls you might have over the system. See DeadReckoningBehavior for an example. Generally, you will install one motion model, and this model will be given a opportunity to update expected particle state before each sensor update. (unless you pass 'false' to updateSensors()). MotionModel can be NULL if you have no control over the system.
DistributionPolicy: defines how to generate random particles and "jiggle" existing ones. The default DistributionPolicy is usually specified via a typedef in the particle itself, and is stored as a property of the ResamplingPolicy (next item) since that is what will use it.
ResamplingPolicy: Following a sensor update, you may wish to re-evaluate the particles in use, making copies of the "good" particles, and dropping those which are not matching sensor values. If you receive a group of different sensor readings, you may want to hold off on resampling until they have all been applied for better evaluation of the particles before selecting which to replicate. Similarly, if your sensors are noisy, you may want to take several readings before allowing resampling so you don't kill off all the "good" particles based on a bad reading. Pass 'false' to updateSensors() or use the delayCount parameter of LowVarianceResamplingPolicy. The resampling policy can be 'NULL' if you never want to resample, but it defaults to an instance of LowVarianceResamlingPolicy.

Generally, preparing to use a particle filter requires these prerequisites:

write your particle class and its associated distribution policy (see LocalizationParticle and LocalizationParticleDistributionPolicy)
write a sensor model to evaluate particles using sensors you'll have available (see DualCoding::ShapeSensorModel)
write a motion model if you have any knowledge of modifications to the state (see HolonomicMotionModel and DeadReckoningBehavior)

Once these are available, usage goes something like this:

create particle filter, optionally passing motion model and/or resampling policy
customize parameters for resampling and distribution policies
while active (note these are all "as needed", in no particular order):
- update motion model whenever there's a change in controls (e.g. call setVelocity() on a HolonomicMotionModel)
- create/pass SensorModel(s) for any measurements obtained (e.g. call updateSensors() on the particle filter)
- call getEstimate() on the particle filter to obtain current state estimate

Remember that the particle filter takes responsibility for deallocating all policies and the motion model when they are removed. Do not attempt to reuse them between particle filters. SensorModels are the only exception -- they are not retained between calls to updateSensors, so you can reuse them.

Definition at line 125 of file ParticleFilter.h.

List of all members.

Classes
class	DistributionPolicy
	A distribution policy provides the ability to randomize ("redistribute") or tweak the values of a group of particles. More...
class	LowVarianceResamplingPolicy
	This class provides a generic, default ResamplingPolicy. It is based on the low variance resampling policy algorithm found in "Probabilistic Robotics" by Sebastian Thrun, Wolfram Burgard, Dieter Fox. More...
class	MotionModel
	A motion model is retained by the particle filter to query before evaluating sensor measurements so all known influences are accounted for before testing the particles. More...
class	ResamplingPolicy
	The resampling policy focuses the particle filter on those particles which are performing well, and dropping those which are poorly rated. More...
class	SensorModel
	A sensor model is used to update particle weights to account based on each particle's ability to explain observations taken from the system. More...
Public Types
typedef ParticleT	particle_type
	redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
typedef std::vector < particle_type >	particle_collection
	the collection type we'll be using to store the particles
typedef particle_collection::size_type	index_t
	index type for refering to particles within the collection
Public Member Functions
	ParticleFilter (unsigned int numParticles, MotionModel mm=NULL, ResamplingPolicy rs=new LowVarianceResamplingPolicy)
	Constructor for the particle filter, specify number of particles and optionally pass a motion model and resampling policy.
virtual	~ParticleFilter ()
	Destructor.
virtual MotionModel *	getMotionModel () const
	Returns the current motion model (motion).
virtual void	installMotionModel (MotionModel *mm)
	Reassigns the motion model, deleting the old one; motion model can be NULL.
virtual ResamplingPolicy *	getResamplingPolicy () const
	Returns the current resampling policy (resampler).
virtual void	installResamplingPolicy (ResamplingPolicy *rs)
	Reassigns the resampling policy, deleting the old one; resampling policy can be NULL (although not recommended...).
virtual void	setResampleDelay (unsigned int d)
	Sets the resampling policy's resampleDelay, which controls how many sensor updates to process before resampling the particles; policy must be a LowVarianceResamplingPolicy.
virtual void	setMinAcceptableWeight (float w)
	Sets the resampling policy's minimum acceptable weight for a particle; policy must be a LowVarianceResamplingPolicy.
virtual void	setMaxRedistribute (float r)
	If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::maxRedistribute; otherwise will display a warning.
virtual void	setVarianceScale (float s)
	If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::varianceScale; otherwise will display a warning.
virtual void	updateMotion ()
	Allows you to manually request a position update -- you might want to call this before using getEstimate's state information.
virtual void	updateSensors (SensorModel &sm, bool updateMot=true, bool doResample=true)
	Applies the sensor model's evaluation to the particles, optionally updating the motion model and resampling first.
virtual void	resample ()
	A manual call to trigger resampling.
virtual void	resetWeights (float w)
	Assigns the specified weight value to all of the particles.
virtual void	resetFilter (float w)
	Requests that the resampler's distribution policy randomly distribute all of the particles, and reset weights to w.
virtual const particle_type &	getEstimate () const
	Returns the weighted mean of all the particles.
virtual const particle_type &	getVariance ()
	Returns the variance of the particles.
virtual void	computeVariance ()
	Computes the variance of the particles; no-op here: override in subclass.
virtual particle_collection &	getParticles ()
	Returns a reference to particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency).
virtual const particle_collection &	getParticles () const
	Returns a reference to particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency).
virtual void	setPosition (const particle_type &pos, float jiggleVariance=0)
	if you know the position in state space, pass it here, along with a positive varianceScale if you want some jiggle from the distribution policy
virtual float	getConfidenceInterval () const
	Returns a confidence interval based on the particle_type's sumSqErr implementation (see ParticleBase::sumSqErr()).
virtual void	resizeParticles (unsigned int numParticles)
	Adjusts the size of the particle collection -- more particles gives better coverage, but more computation.
Static Public Member Functions
static bool	weightLess (const particle_type a, const particle_type b)
	< used for sorting particles in resizeParticles() to drop the least weighted particles first
Public Attributes
particle_collection	particles
	Storage of the particles (no particular order).
particle_type	estimate
	Weighted mean of all the particles; weight is the weight of the best particle.
particle_type	variance
	variance of the particles
bool	varianceValid
	True if the particles haven't changed since the variance was last computed.
MotionModel *	motion
	motion model, can be NULL if you have no control or knowledge of changes in the system
ResamplingPolicy *	resampler
	resampling policy refocuses filter on "good" particles, can be NULL but filter won't work well without a resampler
bool	hasEvaluation
	set to true following each call to updateSensors, and false following resample() or resetWeights(); avoids repeated resamplings
Private Member Functions
	ParticleFilter (const ParticleFilter &)
	don't call (copy constructor)
ParticleFilter &	operator= (const ParticleFilter &)
	don't call (assignment operator)

Member Typedef Documentation

template<typename ParticleT>

typedef particle_collection::size_type ParticleFilter< ParticleT >::index_t

index type for refering to particles within the collection

Definition at line 129 of file ParticleFilter.h.

template<typename ParticleT>

typedef std::vector<particle_type> ParticleFilter< ParticleT >::particle_collection

the collection type we'll be using to store the particles

Definition at line 128 of file ParticleFilter.h.

template<typename ParticleT>

typedef ParticleT ParticleFilter< ParticleT >::particle_type

redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef

Definition at line 127 of file ParticleFilter.h.

Constructor & Destructor Documentation

template<typename ParticleT>

ParticleFilter< ParticleT >::ParticleFilter	(	unsigned int	numParticles,
		MotionModel *	mm = `NULL`,
		ResamplingPolicy *	rs = `new LowVarianceResamplingPolicy`
	)			`[explicit]`

Constructor for the particle filter, specify number of particles and optionally pass a motion model and resampling policy.

The particle filter assumes responsibility for eventual deallocation of the motion model and resampling policy

Definition at line 318 of file ParticleFilter.h.

template<typename ParticleT>

virtual ParticleFilter< ParticleT >::~ParticleFilter ( ) [virtual]

Destructor.

Definition at line 326 of file ParticleFilter.h.

template<typename ParticleT>

ParticleFilter< ParticleT >::ParticleFilter ( const ParticleFilter< ParticleT > & ) [private]

don't call (copy constructor)

Member Function Documentation

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::computeVariance ( ) [virtual]

Computes the variance of the particles; no-op here: override in subclass.

Reimplemented in DualCoding::ShapeBasedParticleFilter.

Definition at line 430 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::getVariance().

template<typename ParticleT>

virtual float ParticleFilter< ParticleT >::getConfidenceInterval ( ) const [virtual]

Returns a confidence interval based on the particle_type's sumSqErr implementation (see ParticleBase::sumSqErr()).

Definition at line 443 of file ParticleFilter.h.

template<typename ParticleT>

virtual const particle_type& ParticleFilter< ParticleT >::getEstimate ( ) const [virtual]

Returns the weighted mean of all the particles.

Definition at line 420 of file ParticleFilter.h.

template<typename ParticleT>

virtual MotionModel* ParticleFilter< ParticleT >::getMotionModel ( ) const [virtual]

Returns the current motion model (motion).

Definition at line 329 of file ParticleFilter.h.

template<typename ParticleT>

virtual const particle_collection& ParticleFilter< ParticleT >::getParticles ( ) const [virtual]

Returns a reference to particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency).

Definition at line 433 of file ParticleFilter.h.

template<typename ParticleT>

virtual particle_collection& ParticleFilter< ParticleT >::getParticles ( ) [virtual]

Returns a reference to particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency).

Definition at line 432 of file ParticleFilter.h.

template<typename ParticleT>

virtual ResamplingPolicy* ParticleFilter< ParticleT >::getResamplingPolicy ( ) const [virtual]

Returns the current resampling policy (resampler).

Definition at line 334 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::setMaxRedistribute(), ParticleFilter< LocalizationParticle >::setMinAcceptableWeight(), ParticleFilter< LocalizationParticle >::setResampleDelay(), and ParticleFilter< LocalizationParticle >::setVarianceScale().

template<typename ParticleT>

virtual const particle_type& ParticleFilter< ParticleT >::getVariance ( ) [virtual]

Returns the variance of the particles.

Definition at line 423 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::installMotionModel ( MotionModel * mm ) [virtual]

Reassigns the motion model, deleting the old one; motion model can be NULL.

Definition at line 331 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::installResamplingPolicy ( ResamplingPolicy * rs ) [virtual]

Reassigns the resampling policy, deleting the old one; resampling policy can be NULL (although not recommended...).

Definition at line 336 of file ParticleFilter.h.

template<typename ParticleT>

ParticleFilter& ParticleFilter< ParticleT >::operator= ( const ParticleFilter< ParticleT > & ) [private]

don't call (assignment operator)

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::resample ( ) [virtual]

A manual call to trigger resampling.

Definition at line 398 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::updateSensors().

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::resetFilter ( float w ) [virtual]

Requests that the resampler's distribution policy randomly distribute all of the particles, and reset weights to w.

You might want to do this if you believe you have been "kidnapped" by some unmodeled motion to a new area of state space, and need to restart the filter to determine the new location.

Definition at line 414 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::ParticleFilter().

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::resetWeights ( float w ) [virtual]

Assigns the specified weight value to all of the particles.

Definition at line 405 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::resetFilter().

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::resizeParticles ( unsigned int numParticles ) [virtual]

Adjusts the size of the particle collection -- more particles gives better coverage, but more computation.

You may wish to shrink the number of particles when the confidence interval is small or particle weights are high, and increase particles when the filter is getting "lost".

Reimplemented in DualCoding::ShapeBasedParticleFilter.

Definition at line 452 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::setMaxRedistribute ( float r ) [virtual]

If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::maxRedistribute; otherwise will display a warning.

Definition at line 357 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::setMinAcceptableWeight ( float w ) [virtual]

Sets the resampling policy's minimum acceptable weight for a particle; policy must be a LowVarianceResamplingPolicy.

Definition at line 348 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::setPosition	(	const particle_type &	pos,
		float	jiggleVariance = `0`
	)			`[virtual]`

if you know the position in state space, pass it here, along with a positive varianceScale if you want some jiggle from the distribution policy

Definition at line 436 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::setResampleDelay ( unsigned int d ) [virtual]

Sets the resampling policy's resampleDelay, which controls how many sensor updates to process before resampling the particles; policy must be a LowVarianceResamplingPolicy.

Definition at line 339 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::setVarianceScale ( float s ) [virtual]

If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::varianceScale; otherwise will display a warning.

Definition at line 366 of file ParticleFilter.h.

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::updateMotion ( ) [virtual]

Allows you to manually request a position update -- you might want to call this before using getEstimate's state information.

Definition at line 376 of file ParticleFilter.h.

Referenced by ParticleFilter< LocalizationParticle >::updateSensors().

template<typename ParticleT>

virtual void ParticleFilter< ParticleT >::updateSensors	(	SensorModel &	sm,
		bool	updateMot = `true`,
		bool	doResample = `true`
	)			`[virtual]`

Applies the sensor model's evaluation to the particles, optionally updating the motion model and resampling first.

If you are applying a group of sensor readings, you probably only want to update motion for the first one (since no motion is occuring between the readings if they were taken at the same time), and may want to hold off on resampling until the end (so the particles are better evaluated). If using the default LowVarianceResamplingPolicy, see also LowVarianceResamplingPolicy::resampleDelay.

Definition at line 387 of file ParticleFilter.h.

template<typename ParticleT>