Main MRPT website > C++ reference
MRPT logo

CParticleFilter.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPARTICLEFILTER_H
00029 #define CPARTICLEFILTER_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/utils/CDebugOutputCapable.h>
00033 #include <mrpt/utils/CLoadableOptions.h>
00034 
00035 namespace mrpt
00036 {
00037         namespace slam
00038         {
00039                 class CActionCollection;
00040                 class CSensoryFrame;
00041         }
00042 
00043         /** The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorithms.
00044           */
00045         namespace bayes
00046         {
00047                 class CParticleFilterCapable;
00048 
00049                 /** 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.
00050                  *   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>
00051                  *
00052                  * For a complete example and further details, see the <a href="http://www.mrpt.org/Particle_Filter_Tutorial">Particle Filter tutorial</a>.
00053                  *
00054                  *   The basic SIR algorithm (pfStandardProposal) consists of:
00055                  *              - Execute a prediction with the given "action".
00056                  *              - Update the weights of the particles using the likelihood of the "observation".
00057                  *              - Normalize weights.
00058                  *              - Perform resampling if the ESS is below the threshold options.BETA.
00059                  *
00060                  *
00061                  * \sa mrpt::poses::CPoseParticlesPDF
00062                  */
00063                 class BASE_IMPEXP CParticleFilter : public mrpt::utils::CDebugOutputCapable
00064                 {
00065                 public:
00066 
00067                         /** Defines different types of particle filter algorithms.
00068                           *  The defined SIR implementations are:
00069                           *             - pfStandardProposal: Standard proposal distribution + weights according to likelihood function.
00070                           *             - pfAuxiliaryPFStandard: An auxiliary PF using the standard proposal distribution.
00071                           *             - pfOptimalProposal: Use the optimal proposal distribution (where available!, usually this will perform approximations)
00072                           *             - 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>).
00073                           *
00074                           * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes">resampling schemes</a>.
00075                           */
00076                         enum TParticleFilterAlgorithm
00077                         {
00078                                 pfStandardProposal = 0,
00079                                 pfAuxiliaryPFStandard,
00080                                 pfOptimalProposal,
00081                                 pfAuxiliaryPFOptimal
00082                         };
00083 
00084                         /** Defines the different resampling algorithms.
00085                           *  The implemented resampling methods are:
00086                           *             - prMultinomial (Default): Uses standard select with replacement (draws M random uniform numbers)
00087                           *             - prResidual: The residual or "remainder" method.
00088                           *             - prStratified: The stratified resampling, where a uniform sample is drawn for each of M subdivisions of the range (0,1].
00089                           *             - prSystematic: A single uniform sample is drawn in the range (0,1/M].
00090                           *
00091                           * See the theoretical discussion in <a href="http://www.mrpt.org/Resampling_Schemes">resampling schemes</a>.
00092                           */
00093                         enum TParticleResamplingAlgorithm
00094                         {
00095                                 prMultinomial = 0,
00096                                 prResidual,
00097                                 prStratified,
00098                                 prSystematic
00099                         };
00100 
00101                         /** The configuration of a particle filter.
00102                           */
00103                         struct BASE_IMPEXP TParticleFilterOptions : public mrpt::utils::CLoadableOptions
00104                         {
00105                         public:
00106                                 /** Initilization of default parameters
00107                                  */
00108                                 TParticleFilterOptions();
00109 
00110                                 /** See mrpt::utils::CLoadableOptions
00111                                   */
00112                                 void  loadFromConfigFile(
00113                                         const mrpt::utils::CConfigFileBase      &source,
00114                                         const std::string &section);
00115 
00116                                 /** See mrpt::utils::CLoadableOptions
00117                                   */
00118                                 void  dumpToTextStream(mrpt::utils::CStream     &out) const;
00119 
00120                                 /** A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (default=false).
00121                                   */
00122                                 bool    adaptiveSampleSize;
00123 
00124                                 /** The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0.5)
00125                                   */
00126                                 double  BETA;
00127 
00128                                 /** The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (default=1)
00129                                   */
00130                                 unsigned int sampleSize;
00131 
00132                                 /** 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)
00133                                   */
00134                                 unsigned int pfAuxFilterOptimal_MaximumSearchSamples;
00135 
00136                                 /** 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).
00137                                   */
00138                                 double          powFactor;
00139 
00140                                 /** The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilities.
00141                                   */
00142                                 TParticleFilterAlgorithm                PF_algorithm;
00143 
00144                                 /** The resampling algorithm to use (default=prMultinomial).
00145                                   */
00146                                 TParticleResamplingAlgorithm    resamplingMethod;
00147 
00148 
00149                                 /** 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.
00150                                   *  This is done to assure that the rejection sampling doesn't get stuck in an infinite loop trying to get an acceptable sample.
00151                                   *  Default = 15 (in logarithmic likelihood)
00152                                   */
00153                                 double max_loglikelihood_dyn_range;
00154 
00155                                 /** Only for PF_algorithm==pfAuxiliaryPFStandard:
00156                                   * If false, the APF will predict the first stage weights just at the mean of the prior of the next time step.
00157                                   * If true, these weights will be estimated as described in the papers for the "pfAuxiliaryPFOptimal" method, i.e. through a monte carlo simulation.
00158                                   *  In that case, "pfAuxFilterOptimal_MaximumSearchSamples" is the number of MC samples used.
00159                                   */
00160                                 bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo;
00161 
00162                                 bool verbose;  //!< Enable extra messages for each PF iteration (Default=false)
00163 
00164                                 /** (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.
00165                                   */
00166                                 bool pfAuxFilterOptimal_MLE;
00167                         };
00168 
00169 
00170                         /** Statistics for being returned from the "execute" method.
00171                           */
00172                         struct BASE_IMPEXP TParticleFilterStats
00173                         {
00174                                 TParticleFilterStats() : ESS_beforeResample(0), weightsVariance_beforeResample (0) {  }
00175                                 double          ESS_beforeResample;
00176                                 double          weightsVariance_beforeResample;
00177                         };
00178 
00179                         /** Default constructor.
00180                          *   After creating the PF object, set the options in CParticleFilter::m_options, then execute steps through CParticleFilter::executeOn.
00181                          */
00182                         CParticleFilter( );
00183 
00184                         virtual ~CParticleFilter() {}
00185 
00186                         /** Executes a complete prediction + update step of the selected particle filtering algorithm.
00187                          *    The member CParticleFilter::m_options must be set before calling this to settle the algorithm parameters.
00188                          *
00189                          * \param obj           The object representing the probability distribution function (PDF) which apply the particle filter algorithm to.
00190                          * \param action                A pointer to an action in the form of a CActionCollection, or NULL if there is no action.
00191                          * \param observation   A pointer to observations in the form of a CSensoryFrame, or NULL if there is no observation.
00192                          * \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).
00193                          *
00194                          * \sa CParticleFilterCapable, executeOn
00195                          */
00196                         void  executeOn(
00197                                 CParticleFilterCapable                  &obj,
00198                                 const mrpt::slam::CActionCollection   *action,
00199                                 const mrpt::slam::CSensoryFrame     *observation,
00200                                 TParticleFilterStats            *stats = NULL);
00201 
00202 
00203                         /** The options to be used in the PF, must be set before executing any step of the particle filter.
00204                           */
00205                         CParticleFilter::TParticleFilterOptions         m_options;
00206 
00207                 }; // End of class def.
00208 
00209         } // end namespace
00210 } // end namespace
00211 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011