00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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://babel.isa.uma.es/mrpt/index.php/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 MRPTDLLIMPEXP 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://babel.isa.uma.es/mrpt/index.php/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://babel.isa.uma.es/mrpt/index.php/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://babel.isa.uma.es/mrpt/index.php/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 MRPTDLLIMPEXP 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 §ion); 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", the number of samples for searching the maximum likelihood value (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 00156 00157 /** Statistics for being returned from the "execute" method. 00158 */ 00159 struct MRPTDLLIMPEXP TParticleFilterStats 00160 { 00161 TParticleFilterStats() : ESS_beforeResample(0), weightsVariance_beforeResample (0) { } 00162 double ESS_beforeResample; 00163 double weightsVariance_beforeResample; 00164 }; 00165 00166 /** Default constructor. 00167 * After creating the PF object, set the options in CParticleFilter::m_options, then execute steps through CParticleFilter::executeOn. 00168 */ 00169 CParticleFilter( ); 00170 00171 virtual ~CParticleFilter() {} 00172 00173 /** Executes a complete prediction + update step of the selected particle filtering algorithm. 00174 * The member CParticleFilter::m_options must be set before calling this to settle the algorithm parameters. 00175 * 00176 * \param obj The object representing the probability distribution function (PDF) which apply the particle filter algorithm to. 00177 * \param action A pointer to an action in the form of a CActionCollection, or NULL if there is no action. 00178 * \param observation A pointer to observations in the form of a CSensoryFrame, or NULL if there is no observation. 00179 * \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). 00180 * 00181 * \sa CParticleFilterCapable, executeOn 00182 */ 00183 void executeOn( 00184 CParticleFilterCapable &obj, 00185 const mrpt::slam::CActionCollection *action, 00186 const mrpt::slam::CSensoryFrame *observation, 00187 TParticleFilterStats *stats = NULL); 00188 00189 00190 /** The options to be used in the PF, must be set before executing any step of the particle filter. 00191 */ 00192 CParticleFilter::TParticleFilterOptions m_options; 00193 00194 }; // End of class def. 00195 00196 } // end namespace 00197 } // end namespace 00198 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:20:53 EDT 2009 |