Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
15
16namespace mrpt
17{
18 namespace obs { class CSensoryFrame; class CActionCollection; }
19
20 /** The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorithms. \ingroup mrpt_base_grp
21 */
22 namespace bayes
23 {
25
26 /** 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.
27 * 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>
28 *
29 * For a complete example and further details, see the <a href="http://www.mrpt.org/Particle_Filter_Tutorial" >Particle Filter tutorial</a>.
30 *
31 * The basic SIR algorithm (pfStandardProposal) consists of:
32 * - Execute a prediction with the given "action".
33 * - Update the weights of the particles using the likelihood of the "observation".
34 * - Normalize weights.
35 * - Perform resampling if the ESS is below the threshold options.BETA.
36 *
37 * \ingroup mrpt_base_grp
38 * \sa mrpt::poses::CPoseParticlesPDF
39 */
41 {
42 public:
43
44 /** Defines different types of particle filter algorithms.
45 * The defined SIR implementations are:
46 * - pfStandardProposal: Standard proposal distribution + weights according to likelihood function.
47 * - pfAuxiliaryPFStandard: An auxiliary PF using the standard proposal distribution.
48 * - pfOptimalProposal: Use the optimal proposal distribution (where available!, usually this will perform approximations)
49 * - 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>).
50 *
51 * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes" >resampling schemes</a>.
52 */
54 {
55 pfStandardProposal = 0,
58 pfAuxiliaryPFOptimal
59 };
60
61 /** Defines the different resampling algorithms.
62 * The implemented resampling methods are:
63 * - prMultinomial (Default): Uses standard select with replacement (draws M random uniform numbers)
64 * - prResidual: The residual or "remainder" method.
65 * - prStratified: The stratified resampling, where a uniform sample is drawn for each of M subdivisions of the range (0,1].
66 * - prSystematic: A single uniform sample is drawn in the range (0,1/M].
67 *
68 * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes" >resampling schemes</a>.
69 */
71 {
72 prMultinomial = 0,
75 prSystematic
76 };
77
78 /** The configuration of a particle filter.
79 */
81 {
82 public:
83 TParticleFilterOptions(); //!< Initilization of default parameters
84 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
85 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
86
87 bool adaptiveSampleSize; //!< A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (default=false).
88 double BETA; //!< The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0.5)
89 unsigned int sampleSize; //!< The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (default=1)
90
91 /** 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)
92 */
94 double powFactor; //!< 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).
95 TParticleFilterAlgorithm PF_algorithm; //!< The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilities.
96 TParticleResamplingAlgorithm resamplingMethod; //!< The resampling algorithm to use (default=prMultinomial).
97
98 /** 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.
99 * This is done to assure that the rejection sampling doesn't get stuck in an infinite loop trying to get an acceptable sample.
100 * Default = 15 (in logarithmic likelihood)
101 */
103
104 /** Only for PF_algorithm==pfAuxiliaryPFStandard:
105 * If false, the APF will predict the first stage weights just at the mean of the prior of the next time step.
106 * If true, these weights will be estimated as described in the papers for the "pfAuxiliaryPFOptimal" method, i.e. through a monte carlo simulation.
107 * In that case, "pfAuxFilterOptimal_MaximumSearchSamples" is the number of MC samples used.
108 */
110
111 bool verbose; //!< Enable extra messages for each PF iteration (Default=false)
112 bool pfAuxFilterOptimal_MLE; //!< (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.
113 };
114
115
116 /** Statistics for being returned from the "execute" method. */
118 {
119 TParticleFilterStats() : ESS_beforeResample(0), weightsVariance_beforeResample (0) { }
122 };
123
124 /** Default constructor.
125 * After creating the PF object, set the options in CParticleFilter::m_options, then execute steps through CParticleFilter::executeOn.
126 */
128
129 virtual ~CParticleFilter() {}
130
131 /** Executes a complete prediction + update step of the selected particle filtering algorithm.
132 * The member CParticleFilter::m_options must be set before calling this to settle the algorithm parameters.
133 *
134 * \param obj The object representing the probability distribution function (PDF) which apply the particle filter algorithm to.
135 * \param action A pointer to an action in the form of a CActionCollection, or NULL if there is no action.
136 * \param observation A pointer to observations in the form of a CSensoryFrame, or NULL if there is no observation.
137 * \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).
138 *
139 * \sa CParticleFilterCapable, executeOn
140 */
143 const mrpt::obs::CActionCollection *action,
144 const mrpt::obs::CSensoryFrame *observation,
145 TParticleFilterStats *stats = NULL);
146
147
148 /** The options to be used in the PF, must be set before executing any step of the particle filter.
149 */
151
152 }; // End of class def.
153
154 } // end namespace
155} // end namespace
156#endif
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...
CParticleFilter()
Default constructor.
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=NULL)
Executes a complete prediction + update step of the selected particle filtering algorithm.
Declares a class for storing a collection of robot actions.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This class allows loading and storing values and vectors of different types from a configuration text...
This base class provides a common printf-like method to send debug information to std::cout,...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
bool verbose
Enable extra messages for each PF iteration (Default=false)
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
TParticleFilterOptions()
Initilization of default parameters.
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true,...
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0....
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
unsigned int sampleSize
The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (defaul...
Statistics for being returned from the "execute" method.



Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:34:44 UTC 2022