Paticle Filter in MRPT and using tips

来源：互联网发布：美工钢笔字帖编辑：程序博客网时间：2024/05/17 14:24

The following C++ classes are the base for different PF implementations all across MRPT:

mrpt::bayes::CParticleFilterCapable: This virtual class defines the interface that any particles based PDF class must implement in order to be executed by a CParticleFilter.
mrpt::bayes::CParticleFilter: The class that executes iterations on a CParticleFilterCapable object.
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 specific particle filter algorithm to run and the resampling scheme to use can be independently selected in the options structuremrpt::bayes::CParticleFilter::TParticleFilterOptions:

1 PF algorithms - See also the description of the algorithms.

pfStandardProposal: Standard proposal distribution + weights according to likelihood function.
pfAuxiliaryPFStandard: An auxiliary PF using the standard proposal distribution.
pfOptimalProposal: Use the exact optimal proposal distribution (where available!, usually this will perform approximations)
pfAuxiliaryPFOptimal: Use the optimal proposal and a auxiliary particle filter.
In addition, adaptiveSampleSize can 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 - APF (pfAuxiliaryPFStandard)

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

Let's assume the filtered posterior is described by the following M weighted samples:

$p(x_t|z_{1:t}) \approx \sum_{i=1}^M \omega^{(i)}_t \delta \left( x_t - x^{(i)}_t \right)$

Then, each step in the algorithm consists of first drawing a sample of the particle index k which will be propragated from t − 1 into the new step t. 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 $\mu^{(i)}_t$ which in some way is related to the transition model x_t | x_{t − 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 )$

This is repeated for i = 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})$

Finally, the weights are updated to account for the mismatch between the likelihood at the actual sample and the predicted point $\mu_t^{k^{(i)}}$ :

$\omega_t^{(i)} \propto \frac{p( z_t | x^{(i)}_t) } { p( z_t | \mu^{k^{(i)}}_t) }$

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 the 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 specific particle filter algorithm to run and the resampling scheme to use can be independently selected in the options structuremrpt::bayes::CParticleFilter::TParticleFilterOptions:

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

The methods are explained using a visual analogy with a "wheel" whose perimeter is assigned to the different particles in such a way that the length associated to the i 'th particle is proportional to its weight ω^[i]. Therefore, picking a random direction 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, illustrated 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 counts N_i is the multinomial distribution with the weights as parameters.

Image:resampling_multim.png

2.2 prResidual

This method comprises of two stages. Firstly, particles are sampled deterministically by picking $N_i=\lfloor N \omega^{[i]} \rfloor$ copies of the i'th particle. Then, multinomial sampling is performed with the residual weights

$\tilde{\omega}^{[i]}=\omega^{[i]} - N_i/N$

Image:resampling_residual.png

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.

Image:resampling_strat.png

2.4 prSystematic

Also called universal sampling, this popular technique draws only one random number, i.e. one direction in the "wheel", with the others N-1 directions being fixed at 1/N increments from the random pick.

Image:resampling_system.png

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 );};