Main MRPT website > C++ reference
MRPT logo
CParticleFilter.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CPARTICLEFILTER_H
10 #define CPARTICLEFILTER_H
11 
12 #include <mrpt/utils/utils_defs.h>
15 
16 namespace mrpt
17 {
18  namespace slam
19  {
20  class CActionCollection;
21  class CSensoryFrame;
22  }
23 
24  /** The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorithms. \ingroup mrpt_base_grp
25  */
26  namespace bayes
27  {
29 
30  /** This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilterAlgorithm) any bayes::CParticleFilterCapable class can implement: it is the invoker of particle filter algorithms.
31  * The particle filter is executed on a probability density function (PDF) described by a CParticleFilterCapable object, passed in the constructor or alternatively through the CParticleFilter::executeOn method.<br>
32  *
33  * For a complete example and further details, see the <a href="http://www.mrpt.org/Particle_Filter_Tutorial" >Particle Filter tutorial</a>.
34  *
35  * The basic SIR algorithm (pfStandardProposal) consists of:
36  * - Execute a prediction with the given "action".
37  * - Update the weights of the particles using the likelihood of the "observation".
38  * - Normalize weights.
39  * - Perform resampling if the ESS is below the threshold options.BETA.
40  *
41  * \ingroup mrpt_base_grp
42  * \sa mrpt::poses::CPoseParticlesPDF
43  */
45  {
46  public:
47 
48  /** Defines different types of particle filter algorithms.
49  * The defined SIR implementations are:
50  * - pfStandardProposal: Standard proposal distribution + weights according to likelihood function.
51  * - pfAuxiliaryPFStandard: An auxiliary PF using the standard proposal distribution.
52  * - pfOptimalProposal: Use the optimal proposal distribution (where available!, usually this will perform approximations)
53  * - pfAuxiliaryPFOptimal: Use the optimal proposal and a auxiliary particle filter (see <a href="http://www.mrpt.org/Paper:An_Optimal_Filtering_Algorithm_for_Non-Parametric_Observation_Models_in_Robot_Localization_(ICRA_2008)" >paper</a>).
54  *
55  * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes" >resampling schemes</a>.
56  */
58  {
59  pfStandardProposal = 0,
62  pfAuxiliaryPFOptimal
63  };
64 
65  /** Defines the different resampling algorithms.
66  * The implemented resampling methods are:
67  * - prMultinomial (Default): Uses standard select with replacement (draws M random uniform numbers)
68  * - prResidual: The residual or "remainder" method.
69  * - prStratified: The stratified resampling, where a uniform sample is drawn for each of M subdivisions of the range (0,1].
70  * - prSystematic: A single uniform sample is drawn in the range (0,1/M].
71  *
72  * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes" >resampling schemes</a>.
73  */
75  {
76  prMultinomial = 0,
79  prSystematic
80  };
81 
82  /** The configuration of a particle filter.
83  */
85  {
86  public:
87  /** Initilization of default parameters
88  */
90 
91  /** See mrpt::utils::CLoadableOptions
92  */
93  void loadFromConfigFile(
94  const mrpt::utils::CConfigFileBase &source,
95  const std::string &section);
96 
97  /** See mrpt::utils::CLoadableOptions
98  */
99  void dumpToTextStream(mrpt::utils::CStream &out) const;
100 
101  /** A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (default=false).
102  */
104 
105  /** The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0.5)
106  */
107  double BETA;
108 
109  /** The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (default=1)
110  */
111  unsigned int sampleSize;
112 
113  /** In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStandard" only if pfAuxFilterStandard_FirstStageWeightsMonteCarlo = true) the number of samples for searching the maximum likelihood value and also to estimate the "first stage weights" (see papers!) (default=100)
114  */
116 
117  /** An optional step to "smooth" dramatic changes in the observation model to affect the variance of the particle weights, eg weight*=likelihood^powFactor (default=1 = no effects).
118  */
119  double powFactor;
120 
121  /** The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilities.
122  */
124 
125  /** The resampling algorithm to use (default=prMultinomial).
126  */
128 
129 
130  /** Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-priori estimate) below the maximum from all the samples - max_loglikelihood_dyn_range, then the particle is directly discarded.
131  * This is done to assure that the rejection sampling doesn't get stuck in an infinite loop trying to get an acceptable sample.
132  * Default = 15 (in logarithmic likelihood)
133  */
135 
136  /** Only for PF_algorithm==pfAuxiliaryPFStandard:
137  * If false, the APF will predict the first stage weights just at the mean of the prior of the next time step.
138  * If true, these weights will be estimated as described in the papers for the "pfAuxiliaryPFOptimal" method, i.e. through a monte carlo simulation.
139  * In that case, "pfAuxFilterOptimal_MaximumSearchSamples" is the number of MC samples used.
140  */
142 
143  bool verbose; //!< Enable extra messages for each PF iteration (Default=false)
144 
145  /** (Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true, do not perform rejection sampling, but just the most-likely (ML) particle found in the preliminary weight-determination stage.
146  */
148  };
149 
150 
151  /** Statistics for being returned from the "execute" method.
152  */
154  {
155  TParticleFilterStats() : ESS_beforeResample(0), weightsVariance_beforeResample (0) { }
158  };
159 
160  /** Default constructor.
161  * After creating the PF object, set the options in CParticleFilter::m_options, then execute steps through CParticleFilter::executeOn.
162  */
163  CParticleFilter( );
164 
165  virtual ~CParticleFilter() {}
166 
167  /** Executes a complete prediction + update step of the selected particle filtering algorithm.
168  * The member CParticleFilter::m_options must be set before calling this to settle the algorithm parameters.
169  *
170  * \param obj The object representing the probability distribution function (PDF) which apply the particle filter algorithm to.
171  * \param action A pointer to an action in the form of a CActionCollection, or NULL if there is no action.
172  * \param observation A pointer to observations in the form of a CSensoryFrame, or NULL if there is no observation.
173  * \param stats An output structure for gathering statistics of the particle filter execution, or set to NULL if you do not need it (see CParticleFilter::TParticleFilterStats).
174  *
175  * \sa CParticleFilterCapable, executeOn
176  */
177  void executeOn(
179  const mrpt::slam::CActionCollection *action,
180  const mrpt::slam::CSensoryFrame *observation,
181  TParticleFilterStats *stats = NULL);
182 
183 
184  /** The options to be used in the PF, must be set before executing any step of the particle filter.
185  */
187 
188  }; // End of class def.
189 
190  } // end namespace
191 } // end namespace
192 #endif
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Statistics for being returned from the "execute" method.
Declares a class for storing a collection of robot actions.
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
This virtual class defines the interface that any particles based PDF class must implement in order t...
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
bool verbose
Enable extra messages for each PF iteration (Default=false)
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
The configuration of a particle filter.
unsigned int sampleSize
The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (defaul...
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
This base class provides a common printf-like method to send debug information to std::cout...
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo