Generic particle filter class. More...

#include <ParticleFilter.hpp>

Inheritance diagram for Estimation::ParticleFilter< CMD, MEAS, STATE >:

Collaboration diagram for Estimation::ParticleFilter< CMD, MEAS, STATE >:

Public Types
typedef Particle< CMD, MEAS, STATE >	ParticleType
	Alias for templated particle type.
typedef PFMeasurementModel < MEAS, STATE >	MeasModelType
	Alias for templated measurement model type.
typedef PFCommandModel< CMD, STATE >	CmdModelType
	Alias for templated command model type.
typedef STATE(*	InjectionFnType )(void *)
	Alias for templated injection function type.
Public Member Functions
	ParticleFilter (unsigned nParticles=100, bool injection=false)
	Default constructor.
virtual void	initialize (const std::vector< STATE > &states)
	Initialize particle set.
virtual void	CommandUpdate (const CMD &c)
	Update filter according to command.
virtual void	MeasurementUpdate (const MEAS &m)
	Update filter according to measurement.
unsigned	getNumParticles () const
	Get number of particles.
void	setNumParticles (unsigned n)
	Set number of particles.
void	setMeasurementModel (shared_ptr< MeasModelType > m)
	Set measurement model to use.
shared_ptr< MeasModelType >	getMeasurementModel () const
	Get measurement model in use.
void	setCommandModel (shared_ptr< CmdModelType > c)
	Set command model to use.
shared_ptr< CmdModelType >	getCommandModel () const
	Get command model in use.
bool	getRandomInjection () const
	Get the current setting for random particle injection.
bool	setRandomInjection (bool s)
	Turn random injection of particles on/off.
void	setRandomInjectionFunction (InjectionFnType fn, void *arg=NULL, bool enable=true)
	Set function to use for random injection.
InjectionFnType	getRandomInjectionFunction () const
	Get current random injection function pointer.
void *	getRandomInjectionArgument () const
	Get currently-set argument to injection function.
STATE	getMode () const
	Get estimate of mode of posterior distribution.
virtual STATE	getCurrentEstimate () const
	Returns the current state estimate.
bool	setFixedRandomInjectionRate (double r)
	Sets portion of particles to be randomly generated each iteration.
double	getFixedRandomInjectionRate () const
	Gets portion of particles to be randomly generated each iteration.
void	setDynamicRandomInjection (bool enable)
	Enables/disables dynamic random injection.
bool	getDynamicRandomInjection () const
	Enables/disables dynamic random injection.
double	getShortTermDRIRate () const
	Gets short-term constant for DRI.
double	getLongTermDRIRate () const
	Gets long-term constant for DRI.
bool	setDRIRates (double shortTerm, double longTerm)
	Sets short- and long-term constants for DRI.
void	setElitism (bool enable)
	Enables/disables elitism.
bool	getElitism () const
	Gets current elitism (on/off) setting.
Protected Member Functions
void	updateParticles (const CMD &c)
	Run command update on all particles.
void	computeLikelihoods (const MEAS &m)
	Run measurement update on all particles.
void	importanceSampling (int nParticles)
	Samples nParticles new particles from the last set.
virtual void	randomInjection (int nParticles)
	Samples nParticles new particles randomly.
double	rand_d ()
	Generates a random double ranged [0,1).
Protected Attributes
std::vector< ParticleType >	m_particles
	Current particle set.
std::vector< ParticleType >	m_prevParticles
	Old particle set.
shared_ptr< MeasModelType >	m_measModel
	Measurement model in use.
shared_ptr< CmdModelType >	m_cmdModel
	Command model in use.
unsigned	m_nParticles
	Number of particles.
double	m_totalLikelihood
	Current sum of particle likelihoods.
bool	m_randomInjection
	Random injection setting.
STATE(*	m_injectionFunction )(void *)
	Random injection function.
void *	m_injectionArg
	Random injection function argument.
double	m_longTermLikelihood
	Long-term likelihood estimate (for RI).
double	m_longTermRate
	Long-term likelihood moving average constant (for RI).
double	m_shortTermLikelihood
	Short-term likelihood estimate (for RI).
double	m_shortTermRate
	Short-term likelihood moving average constant (for RI).
double	m_randomPortion
	Portion of particles to randomly inject.
bool	m_initMovingAvgs
	Whether moving averages need initialization.
bool	m_elitism
	Elitism setting.
bool	m_dynamicInjection
	Dynamic injection setting.

Detailed Description

template<typename CMD, typename MEAS, typename STATE>
class Estimation::ParticleFilter< CMD, MEAS, STATE >

Generic particle filter class.

Simple model-independent implementation of particle filtering.

Template arguments <CMD, MEAS, STATE> specify the data types to use for commands, measurements, and state respectively. The command model and measurement model implement the respective interactions of commands and measurements with state. They can be defined by subclassing PFCommandModel and PFMeasurementModel and then passing the results to setCommandModel() and setMeasurementModel() respectively.

Implementation generally follows the particle filter descrption in

Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. Cambridge, MA: The MIT Press, 2005. Print.

Following the notation from that text, with state, measurement, and command at time $t$ denoted respectively as $x_t$ , $z_t$ , and $u_t$ , and the set of all items up to time $t$ as $\cdot_{1:t}$ , the particle filter estimates $p(x_t | z_{1:t}, u_{1:t})$ , the probability of the state given all measurements and commands up to the current time. In order to do that, it requires functions to evaluate $p(x_t | x_{t-1}, u_t)$ and $ p(z_t | x_t) $ . This is accomplished by subclassing PFCommandModel and PFMeasurementModel respectively and overriding PFCommandModel::updateState() and PFMeasurementModel::getLikelihood(). A particle filter templated with the appropriate types can then be set to use those models with the commands setCommandModel() and setMeasurementModel().

A single timestep of estimation (issuing one command and receiving one measurement) can be carried out through the superclass function BayesFilter::update().

A couple options affect the process of generating a new set of particles from the current set:

Elitism - If elitism is enabled, then the current particle with the highest likelihood will automatically be added to the new set.

Random Particle Injection - To prevent particle deprivation (when particles are resampled unluckily such that no particles remain near the target) or deal with the "kidnapped robot" problem, an option to perform random particle injection at each time step is available. It is disabled by default, but can be enabled with setRandomInjection() after specifying a function to use to generate the random particles (with setRandomInjectionFunction(); it can not be enabled without setting an injection function).

With this option enabled, the number of particles to be randomly generated is determined in two ways:

If setFixedRandomInjectionRate() is called, then, each iteration, the filter will generate a fixed number of random particles equal to the total number of particles left (after elitism) multiplied by the portion specified.

Additionally, By default, or after calling setDynamicRandomInjection(), the filter will dynamically select a number of particles to generate via importance sampling vs through random injection based on estimates of the short term vs. long term average of the particle population's likelihood values, as described in Probabilistic Robotics, section 8.3.5.

Unless either static or dynamic injection (or both) are enabled, no particles will be randomly injected, even if random injection is enabled.

See ParticleFilter1D.hpp for example usage.

See also:: PFCommandModel; PFMeasurementModel

Author:: Zach Pezzementi

Definition at line 263 of file ParticleFilter.hpp.

Member Typedef Documentation

template<typename CMD , typename MEAS , typename STATE >

typedef PFCommandModel<CMD, STATE> Estimation::ParticleFilter< CMD, MEAS, STATE >::CmdModelType

Alias for templated command model type.

Definition at line 273 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

typedef STATE(* Estimation::ParticleFilter< CMD, MEAS, STATE >::InjectionFnType)(void *)

Alias for templated injection function type.

Definition at line 275 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

typedef PFMeasurementModel<MEAS, STATE> Estimation::ParticleFilter< CMD, MEAS, STATE >::MeasModelType

Alias for templated measurement model type.

Definition at line 271 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

typedef Particle<CMD, MEAS, STATE> Estimation::ParticleFilter< CMD, MEAS, STATE >::ParticleType

Alias for templated particle type.

Definition at line 269 of file ParticleFilter.hpp.

Constructor & Destructor Documentation

template<typename CMD , typename MEAS , typename STATE >

Estimation::ParticleFilter< CMD, MEAS, STATE >::ParticleFilter	(	unsigned	nParticles = `100`,
		bool	injection = `false`
	)			`[inline]`

Default constructor.

Parameters:

	nParticles	number of particles to use
	injection	whether to enable random injection

Author:: Zach Pezzementi

Definition at line 285 of file ParticleFilter.hpp.

Member Function Documentation

template<typename CMD , typename MEAS , typename STATE >

virtual void Estimation::ParticleFilter< CMD, MEAS, STATE >::CommandUpdate ( const CMD & c ) [inline, virtual]

Update filter according to command.

Parameters:

c

command

Updates all particles according to the command. Each particle's state is modified based on the command model.

Author:: Zach Pezzementi

Implements Estimation::BayesFilter< CMD, MEAS, STATE >.

Definition at line 319 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::computeLikelihoods ( const MEAS & m ) [protected]

Run measurement update on all particles.

Parameters:

m

measurement

Computes the likelihood of each particle given the current measurement.

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

shared_ptr<CmdModelType> Estimation::ParticleFilter< CMD, MEAS, STATE >::getCommandModel ( ) const [inline]

Get command model in use.

Definition at line 372 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

virtual STATE Estimation::ParticleFilter< CMD, MEAS, STATE >::getCurrentEstimate ( ) const [inline, virtual]

Returns the current state estimate.

Author:: Zach Pezzementi

Implements Estimation::BayesFilter< CMD, MEAS, STATE >.

Definition at line 460 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::getDynamicRandomInjection ( ) const [inline]

Enables/disables dynamic random injection.

See also:: setDynamicRandomInjection

Author:: Zach Pezzementi

Definition at line 513 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::getElitism ( ) const [inline]

Gets current elitism (on/off) setting.

See also:: setElitism

Author:: Zach Pezzementi

Definition at line 582 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::getFixedRandomInjectionRate ( ) const [inline]

Gets portion of particles to be randomly generated each iteration.

See also:: setFixedRandomInjectionRate

Author:: Zach Pezzementi

Definition at line 489 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::getLongTermDRIRate ( ) const [inline]

Gets long-term constant for DRI.

Gets long-term constant for dynamic random injection.

See also:: setDynamicRandomInjection

Author:: Zach Pezzementi

Definition at line 541 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

shared_ptr<MeasModelType> Estimation::ParticleFilter< CMD, MEAS, STATE >::getMeasurementModel ( ) const [inline]

Get measurement model in use.

Definition at line 360 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

STATE Estimation::ParticleFilter< CMD, MEAS, STATE >::getMode ( ) const

Get estimate of mode of posterior distribution.

Returns the state of the particle with the highest likelihood.

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

unsigned Estimation::ParticleFilter< CMD, MEAS, STATE >::getNumParticles ( ) const [inline]

Get number of particles.

Definition at line 338 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::getRandomInjection ( ) const [inline]

Get the current setting for random particle injection.

Definition at line 378 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void* Estimation::ParticleFilter< CMD, MEAS, STATE >::getRandomInjectionArgument ( ) const [inline]

Get currently-set argument to injection function.

See also:: setRandomInjectionFunction; getRandomInjectionFunction

Author:: Zach Pezzementi

Definition at line 446 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

InjectionFnType Estimation::ParticleFilter< CMD, MEAS, STATE >::getRandomInjectionFunction ( ) const [inline]

Get current random injection function pointer.

See also:: setRandomInjectionFunction; getRandomInjectionArgument

Author:: Zach Pezzementi

Definition at line 433 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::getShortTermDRIRate ( ) const [inline]

Gets short-term constant for DRI.

Gets short-term constant for dynamic random injection.

See also:: setDynamicRandomInjection

Author:: Zach Pezzementi

Definition at line 527 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::importanceSampling ( int nParticles ) [protected]

Samples nParticles new particles from the last set.

Performs importance sampling, resampling the from the current set of particles with probability proportional to their importance, to generate a new particle set.

This implementation uses low variance sampling, as described in "Probabilistic Robotics", by Thrun et al., section 4.3.4.

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

virtual void Estimation::ParticleFilter< CMD, MEAS, STATE >::initialize ( const std::vector< STATE > & states ) [virtual]

Initialize particle set.

Parameters:

states

vector of initial states

For each item of the given vector, creates a particle with state corresponding to that entry.

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

virtual void Estimation::ParticleFilter< CMD, MEAS, STATE >::MeasurementUpdate ( const MEAS & m ) [virtual]

Update filter according to measurement.

Parameters:

m

measurement

Updates all particles according to the measurement. Estimates each particle's likelihood according to the measurement model and generates a new particle set through importance sampling.

Author:: Zach Pezzementi

Implements Estimation::BayesFilter< CMD, MEAS, STATE >.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::rand_d ( ) [inline, protected]

Generates a random double ranged [0,1).

Definition at line 637 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

virtual void Estimation::ParticleFilter< CMD, MEAS, STATE >::randomInjection ( int nParticles ) [protected, virtual]

Samples nParticles new particles randomly.

Parameters:

nParticles

number of particles to inject

Randomly injects the specified number of particles into the set, generating them with the given particle injection function.

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setCommandModel ( shared_ptr< CmdModelType > c ) [inline]

Set command model to use.

Definition at line 366 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::setDRIRates	(	double	shortTerm,
		double	longTerm
	)

Sets short- and long-term constants for DRI.

Sets the weights for maintaining short-term and long-term moving weighted averages of particle likelihood values, used for dynamically choosing the number of particles to randomly inject at each iteration.

Both inputs must be between 0 and 1, with shortTerm > longTerm. Default values are 0.5 and 0.1.

See also:: setDynamicRandomInjection

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setDynamicRandomInjection ( bool enable ) [inline]

Enables/disables dynamic random injection.

See also:: getDynamicRandomInjection

Author:: Zach Pezzementi

Definition at line 501 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setElitism ( bool enable ) [inline]

Enables/disables elitism.

See also:: getElitism

Author:: Zach Pezzementi

Definition at line 570 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::setFixedRandomInjectionRate ( double r ) [inline]

Sets portion of particles to be randomly generated each iteration.

See also:: getFixedRandomInjectionRate

Author:: Zach Pezzementi

Definition at line 473 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setMeasurementModel ( shared_ptr< MeasModelType > m ) [inline]

Set measurement model to use.

Definition at line 354 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setNumParticles ( unsigned n ) [inline]

Set number of particles.

Definition at line 344 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::setRandomInjection ( bool s )

Turn random injection of particles on/off.

Parameters:

s

set on (true) or off (false)

Returns:: the resulting setting

Random injection is only enabled if an injection function has already been set, so you can check for success with the return value.

See also:: setRandomInjectionFunction

Author:: Zach Pezzementi

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::setRandomInjectionFunction	(	InjectionFnType	fn,
		void *	arg = `NULL`,
		bool	enable = `true`
	)			`[inline]`

Set function to use for random injection.

Parameters:

	fn	random injection function to use
	arg	argument to pass to injection function
	enable	whether to enable random injection

Specifies the function to use to generate new random particles if random particle injection is enabled. The optional second parameter says whether to also turn on random injection and defaults to enabling it.

See also:: getRandomInjectionFunction; getRandomInjectionArgument

Author:: Zach Pezzementi

Definition at line 416 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void Estimation::ParticleFilter< CMD, MEAS, STATE >::updateParticles ( const CMD & c ) [protected]

Run command update on all particles.

Parameters:

c

command

Author:: Zach Pezzementi

Member Data Documentation

template<typename CMD , typename MEAS , typename STATE >

shared_ptr<CmdModelType> Estimation::ParticleFilter< CMD, MEAS, STATE >::m_cmdModel [protected]

Command model in use.

Definition at line 649 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::m_dynamicInjection [protected]

Dynamic injection setting.

Definition at line 677 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::m_elitism [protected]

Elitism setting.

Definition at line 675 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::m_initMovingAvgs [protected]

Whether moving averages need initialization.

Definition at line 673 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

void* Estimation::ParticleFilter< CMD, MEAS, STATE >::m_injectionArg [protected]

Random injection function argument.

Definition at line 660 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

STATE(* Estimation::ParticleFilter< CMD, MEAS, STATE >::m_injectionFunction)(void *) [protected]

Random injection function.

Definition at line 658 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_longTermLikelihood [protected]

Long-term likelihood estimate (for RI).

Definition at line 663 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_longTermRate [protected]

Long-term likelihood moving average constant (for RI).

Definition at line 665 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

shared_ptr<MeasModelType> Estimation::ParticleFilter< CMD, MEAS, STATE >::m_measModel [protected]

Measurement model in use.

Definition at line 647 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

unsigned Estimation::ParticleFilter< CMD, MEAS, STATE >::m_nParticles [protected]

Number of particles.

Definition at line 651 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

std::vector<ParticleType> Estimation::ParticleFilter< CMD, MEAS, STATE >::m_particles [protected]

Current particle set.

Definition at line 643 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

std::vector<ParticleType> Estimation::ParticleFilter< CMD, MEAS, STATE >::m_prevParticles [protected]

Old particle set.

Definition at line 645 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

bool Estimation::ParticleFilter< CMD, MEAS, STATE >::m_randomInjection [protected]

Random injection setting.

Definition at line 656 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_randomPortion [protected]

Portion of particles to randomly inject.

Definition at line 671 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_shortTermLikelihood [protected]

Short-term likelihood estimate (for RI).

Definition at line 667 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_shortTermRate [protected]

Short-term likelihood moving average constant (for RI).

Definition at line 669 of file ParticleFilter.hpp.

template<typename CMD , typename MEAS , typename STATE >

double Estimation::ParticleFilter< CMD, MEAS, STATE >::m_totalLikelihood [protected]

Current sum of particle likelihoods.

Definition at line 653 of file ParticleFilter.hpp.

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

Estimation/BayesFilters/ParticleFilter/ParticleFilter.hpp

Estimation::ParticleFilter< CMD, MEAS, STATE > Class Template Reference

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<typename CMD, typename MEAS, typename STATE> class Estimation::ParticleFilter< CMD, MEAS, STATE >

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<typename CMD, typename MEAS, typename STATE>
class Estimation::ParticleFilter< CMD, MEAS, STATE >