Paticle filter in mrpt and using tips

The following C ++ classes are the base for different pf implementations all except SS mrpt:

• Mrpt: Bayes: cparticlefiltercapable: this virtual class defines the interface that any participant based PDF class must implement in order to be executed by a cparticlefilter.
• Mrpt: Bayes: cparticlefilter: the class that executes iterations onCparticlefiltercapableObject.
• The container model mrpt: Bayes: cparticlefilterdata is used by some classes which are not really intended to be processed with a PF. it just represent a C ++ template for sets of weighted samples a any given type, and common memory-keeping tasks.

Ref: http://babel.isa.uma.es/mrpt/reference/stable/classmrpt_1_1bayes_1_1_c_particle_filter_capable.html#fc87a617ea5582e4ba80bb3c065c4785

Both the specificParticle Filter AlgorithmTo run andResampling SchemeTo use can be independently selected in the options structuremrpt: Bayes: cparticlefilter: tparticipant filteroptions:

• 1 pf Algorithms-See alsoDescription of the algorithms.
• Pfstandardproposal: Standard proposal distribution + weights according to likelihood function.
• Pfauxiliarypfstandard: An auxiliary pf using the standard proposal distribution.
• Pfoptimalproposal: UseExactOptimal proposal distribution (where available !, Usually this will perform approximations)
• Pfauxiliarypfoptimal: Use the optimal proposal and a auxiliary particle filter.
• In addition,AdaptivesamplesizeCan select whether to use a dynamic number of samples, or not. Take into account that only a subset of all the possible combinations of algorithms may be implemented for each problem.

1.1 sequential importance resampling- Sir( Pfstandardproposal)

Standard proposal distribution + weights according to likelihood function.

1.2 auxiliary particle filter- <G id = "1"> </G>( Pfauxiliarypfstandard)

This method was introduced by Pitt and Shephard in 1999 [1].

Let's assume the filtered posterior is described by the followingMWeighted samples:

P (X_t | Z _ {1: t}) \ approx \ sum _ {I = 1} ^ m \ Omega ^ {(I )} _ t \ Delta \ left (X_t-x ^ {(I) }_t \ right) "src =" http://babel.isa.uma.es/mrpt/images/math/3/c/d/3cdd4cfc3d8580a769a9526d59acb229.png ">

Then, each step in the algorithm consists of first drawing a sample of the particle IndexKWhich will be propragated fromT−1 into the new stepT. These indexes are auxiliary variables only used as an intermediary step, hence the name of the algorithm. the indexes are drawn according to the likelihood of some reference point which in some way is related to the transition modelXT|XT−1 (for example, the mean, a sample, etc .):

K ^ {(I)} \ sim p (I = k | z_t) \ propto \ Omega ^ {(I )} _ t p (z_t | \ Mu ^ {(I)} _ t) "src =" http://babel.isa.uma.es/mrpt/images/math/c/9/6/c961f53923c79f87e9279b5713111c69.png ">

This is repeatedI= 1, 2 ,...,M, And using these indexes we can now draw the conditional samples:

X_t ^ {(I)} \ sim p (X_t | x ^ {k ^ {(I) }}_{ T-1}) "src =" http://babel.isa.uma.es/mrpt/images/math/2/7/5/275bb8c02c98a5620dabac65d104d494.png ">

Finally, the weights are updated to account for the mismatch between the likelihood at the actual sample and the predicted point:

\ Omega_t ^ {(I)} \ propto \ frac {P (z_t | x ^ {(I)} _ t )} {P (z_t | \ Mu ^ {k ^ {(I) }}_ t)} "src =" http://babel.isa.uma.es/mrpt/images/math/4/7/7/4776b51b4a6bfe7a860b6994625d68ca.png ">

1.3 Optimal Sampling ( Pfoptimalproposal)

Use the exact optimal proposal distribution (where available !, Usually this will perform approximations ).

In the case of the RBPF-SLAM implementation, this method follows [1].

1.4 approximate optimal sampling ( Pfauxiliarypfoptimal)

Use the optimal proposal and a auxiliary particle filter (see paper [1]).

 

2 resampling Algorithms-See also Description of the algorithms.

• Prmultinomial(Default): uses standard select with replacement (draws M random uniform numbers)
• Prresidual: The residual or "remainder" method.
• Prstratified: The stratified resampling, where a uniform sample is drawn for each of M subdivisions of the range (0, 1].
• Prsystematic: A single uniform sample is drawn in the range (0, 1/M].

Both the specificParticle Filter AlgorithmTo run andResampling SchemeTo use can be independently selected in the options structuremrpt: Bayes: cparticlefilter: tparticipant filteroptions:

This section reviews four different strategies for resampling a set of participants whose normalized weights are given by ω [I]I= 1 ,...,N.

The methods are explained using a visual analogy with a "Wheel" whose perimeter is assigned to the different participant in such a way that the length associated toI'Th particle is proportional to its weight ω [I]. Therefore, picking a random ction in this "Wheel" implies choosing a particle with a probability proportional to its weight. for a more formal description of the methods, please refer to the excellent paper by douc, CAPP é and moulines.

2.1 Prmultinomial(Default)

The most straighforward method, where n independent random numbers are generated to pick a particle from the old set. in the "Wheel" analogy, specified strated in the figure below, This method consists of picking n independent random directions.

The name of this method comes from the fact that the probability mass function for the duplication countsNIIs the multinomial distribution with the weights as parameters.

2.2 Prresidual

This method comprises of two stages. Firstly, participant are sampled deterministically by picking copies ofI'Th particle. Then, multinomial sampling is already Med with the residual Weights

2.3 Prstratified

In this method, the "Wheel" representing the old set of particles is divided into N equally-sized segments, as represented in the figure below. then, N uniform numbers are independently generated like in multinomial sampling, but instead of Mapping each draw to the entire circumference, they are mapped to its corresponding partition.

2.4 Prsystematic

Also calledUniversal sampling, This popular technique draws only one random number, I. e. One ction in the "Wheel", with the others N-1 directions being fixed at 1/N increments from the random pick.

3. pf using tips

#pragma once#include <mrpt/core.h>using namespace mrpt;using namespace mrpt::bayes;using namespace mrpt::gui;using namespace mrpt::math;using namespace mrpt::slam;using namespace mrpt::utils;using namespace mrpt::random;using namespace std;struct CParticleVehicleData{float x,y, vx,vy; // Vehicle state (position & velocities)};class CRangeBearingParticleFilter :public mrpt::bayes::CParticleFilterCapable, public mrpt::bayes::CParticleFilterData<CParticleVehicleData>{IMPLEMENT_PARTICLE_FILTER_CAPABLE(CParticleVehicleData);public:CRangeBearingParticleFilter(void);~CRangeBearingParticleFilter(void);public:/** Update the m_particles, predicting the posterior of robot pose and map after a movement command.*  This method has additional configuration parameters in "options".*  Performs the update stage of the RBPF, using the sensed Sensorial Frame:**   \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.*   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.** \sa options*/void  prediction_and_update_pfStandardProposal(const mrpt::slam::CActionCollection* action,const mrpt::slam::CSensoryFrame* observation,const bayes::CParticleFilter::TParticleFilterOptions &PF_options );void initializeParticles(size_t  numParticles);/** Computes the average velocity & position*/void getMean( float &x, float &y, float &vx, float &vy );};