#include <visp3/core/vpParticleFilter.h>

Classes
struct	vpParticlesWithWeights

Public Types
typedef struct vpParticleFilter::vpParticlesWithWeights	vpParticlesWithWeights

typedef std::function< vpColVector(const vpColVector &, const vpColVector &)>	vpStateAddFunction

typedef std::function< vpColVector(const vpColVector &, const double &)>	vpProcessFunction

typedef std::function< vpColVector(const vpColVector &, const vpColVector &, const double &)>	vpCommandStateFunction

typedef std::function< double(const vpColVector &, const MeasurementsType &)>	vpLikelihoodFunction

typedef std::function< vpColVector(const std::vector< vpColVector > &, const std::vector< double > &, const vpStateAddFunction &)>	vpFilterFunction

typedef std::function< bool(const unsigned int &, const std::vector< double > &)>	vpResamplingConditionFunction

typedef std::function< vpParticlesWithWeights(const std::vector< vpColVector > &, const std::vector< double > &)>	vpResamplingFunction

Public Member Functions
VP_EXPLICIT	vpParticleFilter (const unsigned int &N, const std::vector< double > &stdev, const long &seed=-1, const int &nbThreads=-1)

virtual	~vpParticleFilter ()

void	init (const vpColVector &x0, const vpProcessFunction &f, const vpLikelihoodFunction &l, const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, const vpFilterFunction &filterFunc=weightedMean, const vpStateAddFunction &addFunc=simpleAdd)

void	init (const vpColVector &x0, const vpCommandStateFunction &bx, const vpLikelihoodFunction &l, const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, const vpFilterFunction &filterFunc=weightedMean, const vpStateAddFunction &addFunc=simpleAdd)

void	filter (const MeasurementsType &z, const double &dt, const vpColVector &u=vpColVector())

void	predict (const double &dt, const vpColVector &u=vpColVector())

void	update (const MeasurementsType &z)

vpColVector	computeFilteredState ()

void	setProcessFunction (const vpProcessFunction &f)

void	setCommandStateFunction (const vpCommandStateFunction &bx)

void	setLikelihoodFunction (const vpLikelihoodFunction &likelihood)

void	setFilterFunction (const vpFilterFunction &filterFunc)

void	setCheckResamplingFunction (const vpResamplingConditionFunction &resamplingCondFunc)

void	setResamplingFunction (const vpResamplingFunction &resamplingFunc)

Static Public Member Functions
static vpColVector	simpleAdd (const vpColVector &a, const vpColVector &toAdd)

static vpColVector	weightedMean (const std::vector< vpColVector > &particles, const std::vector< double > &weights, const vpStateAddFunction &addFunc)

static bool	simpleResamplingCheck (const unsigned int &N, const std::vector< double > &weights)

static vpParticlesWithWeights	simpleImportanceResampling (const std::vector< vpColVector > &particles, const std::vector< double > &weights)

Detailed Description

template<typename MeasurementsType>
class vpParticleFilter< MeasurementsType >

The class permits to use a Particle Filter.

Template Parameters

MeasurementsType The class that corresponds to the measurements used to compute the weights of the Particle Filter

Be $\textbf{x}_i \in \textit{S}$ a particle representing the internal state of the PF, with $i \in {0 \dots N - 1}$ and $\textit{S}$ the state space. To each particle is associated a weight that represents its likelihood knowing the measurements and is used to compute the filtered state $\textbf{x}_{filtered} \in \textit{S}$ .

The first step of the PF is the prediction step. During this step, the particles of the PF are projected forward in time. Be $f(\textbf{x}_i, \Delta t) : \textit{S} \times R \rightarrow \textit{S}$ the process function that project the forward in time. All the particles pass through the function , and some noise $\epsilon$ is independently added to each of them to form the new particles:

$\textbf{x}_i(t + \Delta t) = f( \textbf{x}_i(t) , \Delta t ) + \epsilon$

The second step of the PF is to update the weights associated to each particle based on new measurements. The update is based on the likelihood of a particle based on the measurements $\textbf{z} \in \textit{M}$ , where $\textit{M}$ is the measurement space. Be $l: \textit{S} \times \textit{M} \rightarrow [0; 1.]$ the likelihood function, we have:

$w_i = l(\textbf{x}_i, \textbf{z})$

After an update, a check is performed to see if the PF is not degenerated (i.e. if the weigths of most particles became very low). If the PF became degenerated, the particles are resampled depending on a resampling scheme. Different kind of checks and of resampling algorithms exist in the litterature. In this class, we implemented the Simple Resampling algorithm in a dedicated method and let to the user the possibility of writing user-defined check and resampling methods.

Finally, we can compute the new state estimate $\textbf{x}_{filtered}$ by performing a weighted mean of the particles $\textbf{x}_i$ . Be $\textbf{w} = (w_0 \dots w_{N-1})^T \in R^N$ , $\textbf{x} = {\textbf{x}_0 \dots \textbf{x}_{N-1}} \in \textit{S}^N$ and $wm: R^N \times \textit{S}^N \rightarrow \textit{S}$ the weighted mean function of the state space $\textit{S}$ , we have:

$\textbf{x}_{filtered} = wm(\textbf{w}, \textbf{x})$

Examples: catchParticleFilter.cpp, pf-nonlinear-complex-example.cpp, pf-nonlinear-example.cpp, tutorial-pf-curve-fitting-all.cpp, tutorial-pf-curve-fitting-pf.cpp, and tutorial-pf.cpp.

Definition at line 96 of file vpParticleFilter.h.

Member Typedef Documentation

◆ vpCommandStateFunction

template<typename MeasurementsType >

typedef std::function<vpColVector(const vpColVector &, const vpColVector &, const double &)> vpParticleFilter< MeasurementsType >::vpCommandStateFunction

Command model function, which projects a particle forward in time according to the command and its previous state. The first argument is the command(s), the second is the particle and the third is the period. The return is the updated particle after period seconds.

Definition at line 130 of file vpParticleFilter.h.

◆ vpFilterFunction

template<typename MeasurementsType >

typedef std::function<vpColVector(const std::vector<vpColVector> &, const std::vector<double> &, const vpStateAddFunction &)> vpParticleFilter< MeasurementsType >::vpFilterFunction

Filter function, which computes the filtered state of the particle filter. The first argument is the vector containing the particles, the second argument is the associated vector of weights and the third argument is a function to perform an addition in the state space. The return is the corresponding filtered state.

Definition at line 148 of file vpParticleFilter.h.

◆ vpLikelihoodFunction

template<typename MeasurementsType >

typedef std::function<double(const vpColVector &, const MeasurementsType &)> vpParticleFilter< MeasurementsType >::vpLikelihoodFunction

Likelihood function, which evaluates the likelihood of a particle with regard to the measurements. The first argument is the particle that is evaluated. The second argument is the measurements vector. The return is the likelihood of the particle, which equals to 0 when the particle does not match at all the measurements and to 1 when it matches completely.

Definition at line 139 of file vpParticleFilter.h.

◆ vpParticlesWithWeights

template<typename MeasurementsType >

typedef struct vpParticleFilter::vpParticlesWithWeights vpParticleFilter< MeasurementsType >::vpParticlesWithWeights

Structure of vectors for which the i^th element of the weights vector is associated to the i^th element of the particles vectors.

◆ vpProcessFunction

template<typename MeasurementsType >

typedef std::function<vpColVector(const vpColVector &, const double &)> vpParticleFilter< MeasurementsType >::vpProcessFunction

Process model function, which projects a particle forward in time. The first argument is a particle, the second is the period and the return is the particle projected in the future.

Definition at line 122 of file vpParticleFilter.h.

◆ vpResamplingConditionFunction

template<typename MeasurementsType >

typedef std::function<bool(const unsigned int &, const std::vector<double> &)> vpParticleFilter< MeasurementsType >::vpResamplingConditionFunction

Function that takes as argument the number of particles and the vector of weights associated to each particle and returns true if the resampling must be performed and false otherwise.

Definition at line 155 of file vpParticleFilter.h.

◆ vpResamplingFunction

template<typename MeasurementsType >

typedef std::function<vpParticlesWithWeights(const std::vector<vpColVector> &, const std::vector<double> &)> vpParticleFilter< MeasurementsType >::vpResamplingFunction

Function that takes as argument the vector of particles and the vector of associated weights. It returns a pair < new_vector_particles, new_weights >.

Definition at line 161 of file vpParticleFilter.h.

◆ vpStateAddFunction

template<typename MeasurementsType >

typedef std::function<vpColVector(const vpColVector &, const vpColVector &)> vpParticleFilter< MeasurementsType >::vpStateAddFunction

Function that computes either the equivalent of an addition in the state space. The first argument is the vector to which we must add something and the second argument is the thing to be added. The return is the result of this operation.

Definition at line 115 of file vpParticleFilter.h.

Constructor & Destructor Documentation

◆ vpParticleFilter()

template<typename MeasurementsType >

vpParticleFilter< MeasurementsType >::vpParticleFilter	(	const unsigned int &	N,
		const std::vector< double > &	stdev,
		const long &	seed = `-1`,
		const int &	nbThreads = `-1`
	)

Construct a new vpParticleFilter object.

Parameters

[in]	N	The number of particles.
[in]	stdev	The standard deviations of the noise, each item correspond to one component of the state.
[in]	seed	The seed to use to create the noise generators. A negative value makes the seed to be based on the current time.
[in]	nbThreads	The number of threads the user would like to use. Negative value to set the maximum number of threads available when using OpenMP.

Definition at line 419 of file vpParticleFilter.h.

References vpTime::measureTimeMicros(), vpUniRand::setSeed(), and vpUniRand::uniform().

◆ ~vpParticleFilter()

template<typename MeasurementsType >

virtual vpParticleFilter< MeasurementsType >::~vpParticleFilter ( )

inlinevirtual

Definition at line 175 of file vpParticleFilter.h.

Member Function Documentation

◆ computeFilteredState()

template<typename MeasurementsType >

vpColVector vpParticleFilter< MeasurementsType >::computeFilteredState

Compute the filtered state from the particles and their associated weights.

Returns: vpColVector The filtered state.

Definition at line 554 of file vpParticleFilter.h.

◆ filter()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::filter	(	const MeasurementsType &	z,
		const double &	dt,
		const vpColVector &	u = `vpColVector()`
	)

Perform first the prediction step and then the update step. If needed, resampling will also be performed.

Parameters

[in]	z	The new measurement.
[in]	dt	The time in the future we must predict.
[in]	u	The command(s) given to the system, if the impact of the system is known.

Warning: To use the commands, use the dedicated constructor or call vpUnscentedKalman::setCommandStateFunction beforehand. In the second case, the process function will be ignored.

Definition at line 515 of file vpParticleFilter.h.

◆ init() [1/2]

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::init	(	const vpColVector &	x0,
		const vpCommandStateFunction &	bx,
		const vpLikelihoodFunction &	l,
		const vpResamplingConditionFunction &	checkResamplingFunc,
		const vpResamplingFunction &	resamplingFunc,
		const vpFilterFunction &	filterFunc = `weightedMean`,
		const vpStateAddFunction &	addFunc = `simpleAdd`
	)

Set the guess of the initial state.

Parameters

[in]	x0	Guess of the initial state.
[in]	bx	Process model function, which projects the particles forward in time based on the previous state and on the input commands. The first argument is the command vector, the second is a particle and the last is the period. The return is the particle projected in the future.
[in]	l	Likelihood function, that evaluates how much a particle matches the measurements. 0 means that the particle does not match the measurement at all and 1 that it maches completely.
[in]	checkResamplingFunc	The function that returns true when the filter starts to degenerate and false otherwise.
[in]	resamplingFunc	The resampling function that generate new particles and associated weights when the filter starts to degenerate.
[in]	filterFunc	The function to compute the filtered state from the particles and their weights.
[in]	addFunc	The function that permits to perform an addition in the state space.

Definition at line 493 of file vpParticleFilter.h.

References vpException::dimensionError, and vpArray2D< Type >::size().

◆ init() [2/2]

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::init	(	const vpColVector &	x0,
		const vpProcessFunction &	f,
		const vpLikelihoodFunction &	l,
		const vpResamplingConditionFunction &	checkResamplingFunc,
		const vpResamplingFunction &	resamplingFunc,
		const vpFilterFunction &	filterFunc = `weightedMean`,
		const vpStateAddFunction &	addFunc = `simpleAdd`
	)

Set the guess of the initial state.

Parameters

[in]	x0	Guess of the initial state.
[in]	f	Process model function, which projects the particles forward in time. The first argument is a particle, the second is the period and the return is the particle projected in the future.
[in]	l	Likelihood function, that evaluates how much a particle matches the measurements. 0 means that the particle does not match the measurement at all and 1 that it maches completely.
[in]	checkResamplingFunc	The function that returns true when the filter starts to degenerate and false otherwise.
[in]	resamplingFunc	The resampling function that generate new particles and associated weights when the filter starts to degenerate.
[in]	filterFunc	The function to compute the filtered state from the particles and their weights.
[in]	addFunc	The function that permits to perform an addition in the state space.

Definition at line 471 of file vpParticleFilter.h.

References vpException::dimensionError, and vpArray2D< Type >::size().

◆ predict()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::predict	(	const double &	dt,
		const vpColVector &	u = `vpColVector()`
	)

Predict the new state based on the last state and how far in time we want to predict.

Parameters

[in]	dt	The time in the future we must predict.
[in]	u	The command(s) given to the system, if the impact of the system is known.

Warning: To use the commands, use the dedicated constructor or call vpUnscentedKalman::setCommandStateFunction beforehand. In the second case, the process function will be ignored.

Definition at line 522 of file vpParticleFilter.h.

◆ setCheckResamplingFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setCheckResamplingFunction ( const vpResamplingConditionFunction & resamplingCondFunc )

inline

Set the function that returns true when the filter starts to degenerate and false otherwise.

Parameters

resamplingCondFunc The evaluation function to use.

Definition at line 318 of file vpParticleFilter.h.

◆ setCommandStateFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setCommandStateFunction ( const vpCommandStateFunction & bx )

inline

Set the command function to use when projecting the particles in the future.

Parameters

bx	The command function to use.

Warning: It will deactivate the process function.

Definition at line 283 of file vpParticleFilter.h.

◆ setFilterFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setFilterFunction ( const vpFilterFunction & filterFunc )

inline

Set the filter function that compute the filtered state from the particles and their associated weights.

Parameters

filterFunc The filtering function to use.

Definition at line 307 of file vpParticleFilter.h.

◆ setLikelihoodFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setLikelihoodFunction ( const vpLikelihoodFunction & likelihood )

inline

Set the likelihood function that updates the weights of the particles based on the new measurements.

Parameters

likelihood The likelihood function.

Definition at line 296 of file vpParticleFilter.h.

◆ setProcessFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setProcessFunction ( const vpProcessFunction & f )

inline

Set the process function to use when projecting the particles in the future.

Parameters

f	The process function to use.

Warning: It will deactive the command function.

Definition at line 269 of file vpParticleFilter.h.

◆ setResamplingFunction()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::setResamplingFunction ( const vpResamplingFunction & resamplingFunc )

inline

Set the resampling function that generate new particles and associated weights when the filter starts to degenerate.

Parameters

resamplingFunc The resampling function to use.

Definition at line 329 of file vpParticleFilter.h.

◆ simpleAdd()

template<typename MeasurementsType >

static vpColVector vpParticleFilter< MeasurementsType >::simpleAdd	(	const vpColVector &	a,
		const vpColVector &	toAdd
	)

inlinestatic

Simple function to compute an addition, which just does $\textbf{res} = \textbf{a} + \textbf{toAdd}$ .

Parameters

[in]	a	Vector to which we must add something.
[in]	toAdd	The something we must add to a .

Returns: vpColVector $\textbf{res} = \textbf{a} + \textbf{toAdd}$

Definition at line 341 of file vpParticleFilter.h.

◆ simpleImportanceResampling()

template<typename MeasurementsType >

vpParticleFilter< MeasurementsType >::vpParticlesWithWeights vpParticleFilter< MeasurementsType >::simpleImportanceResampling	(	const std::vector< vpColVector > &	particles,
		const std::vector< double > &	weights
	)

static

Function implementing the resampling of a Simple Importance Resampling Particle Filter.

Parameters

[in]	particles	Vector containing the particles.
[in]	weights	Vector containing the associated weights.

Returns: vpParticlesWithWeights A pair of vector of particles and vector of associated weights.

Definition at line 589 of file vpParticleFilter.h.

References vpParticleFilter< MeasurementsType >::vpParticlesWithWeights::m_particles, and vpParticleFilter< MeasurementsType >::vpParticlesWithWeights::m_weights.

◆ simpleResamplingCheck()

template<typename MeasurementsType >

bool vpParticleFilter< MeasurementsType >::simpleResamplingCheck	(	const unsigned int &	N,
		const std::vector< double > &	weights
	)

static

Returns true if the following condition is fulfilled, or if all the particles diverged: $\frac{2}{\sum_i (\frac{w_i}{\sum_j w_j})^2} < N$ .

Parameters

[in]	N	The number of particles.
[in]	weights	The weights associated to each particle.

Returns: true Resampling must be performed.; false Resampling is not needed.

Definition at line 574 of file vpParticleFilter.h.

◆ update()

template<typename MeasurementsType >

void vpParticleFilter< MeasurementsType >::update ( const MeasurementsType & z )

Update the weights of the particles based on a new measurement. The weights will be normalized (i.e. each weight will be divided by the sum of the weights).

Parameters

[in] z The measurements at the current timestep.

Definition at line 535 of file vpParticleFilter.h.

References vpParticleFilter< MeasurementsType >::vpParticlesWithWeights::m_particles, and vpParticleFilter< MeasurementsType >::vpParticlesWithWeights::m_weights.

◆ weightedMean()

template<typename MeasurementsType >

vpColVector vpParticleFilter< MeasurementsType >::weightedMean	(	const std::vector< vpColVector > &	particles,
		const std::vector< double > &	weights,
		const vpStateAddFunction &	addFunc
	)

static

Simple function to compute a weighted mean, which just does $\textbf{res} = \sum^{N-1}_{i=0} weights[i] \textbf{particles}[i]$ .

Parameters

[in]	particles	Vector that contains all the particles.
[in]	weights	Vector that contains the weights associated to the particles.
[in]	addFunc	How to perform the addition.

Returns: vpColVector $\textbf{res} = \sum^{N-1}_{i=0} weights[i] \textbf{particles}[i]$

Definition at line 560 of file vpParticleFilter.h.

References vpException::dimensionError, and vpArray2D< Type >::size().

Classes

Public Types

Public Member Functions

Static Public Member Functions

Detailed Description

template<typename MeasurementsType> class vpParticleFilter< MeasurementsType >

Member Typedef Documentation

◆ vpCommandStateFunction

◆ vpFilterFunction

◆ vpLikelihoodFunction

◆ vpParticlesWithWeights

◆ vpProcessFunction

◆ vpResamplingConditionFunction

◆ vpResamplingFunction

◆ vpStateAddFunction

Constructor & Destructor Documentation

◆ vpParticleFilter()

◆ ~vpParticleFilter()

Member Function Documentation

◆ computeFilteredState()

◆ filter()

◆ init() [1/2]

◆ init() [2/2]

◆ predict()

◆ setCheckResamplingFunction()

◆ setCommandStateFunction()

◆ setFilterFunction()

◆ setLikelihoodFunction()

◆ setProcessFunction()

◆ setResamplingFunction()

◆ simpleAdd()

◆ simpleImportanceResampling()

◆ simpleResamplingCheck()

◆ update()

◆ weightedMean()

template<typename MeasurementsType>
class vpParticleFilter< MeasurementsType >