particlefilter.h
Go to the documentation of this file.
00001 // $Id: particlefilter.h 29830 2009-01-14 15:10:41Z kgadeyne $
00002 
00003  /***************************************************************************
00004  *   This library is free software; you can redistribute it and/or         *
00005  *   modify it under the terms of the GNU General Public                   *
00006  *   License as published by the Free Software Foundation;                 *
00007  *   version 2 of the License.                                             *
00008  *                                                                         *
00009  *   As a special exception, you may use this file as part of a free       *
00010  *   software library without restriction.  Specifically, if other files   *
00011  *   instantiate templates or use macros or inline functions from this     *
00012  *   file, or you compile this file and link it with other files to        *
00013  *   produce an executable, this file does not by itself cause the         *
00014  *   resulting executable to be covered by the GNU General Public          *
00015  *   License.  This exception does not however invalidate any other        *
00016  *   reasons why the executable file might be covered by the GNU General   *
00017  *   Public License.                                                       *
00018  *                                                                         *
00019  *   This library 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 GNU     *
00022  *   Lesser General Public License for more details.                       *
00023  *                                                                         *
00024  *   You should have received a copy of the GNU General Public             *
00025  *   License along with this library; if not, write to the Free Software   *
00026  *   Foundation, Inc., 59 Temple Place,                                    *
00027  *   Suite 330, Boston, MA  02111-1307  USA                                *
00028  *                                                                         *
00029  ***************************************************************************/
00030 
00031 #ifndef __PARTICLE_FILTER__
00032 #define __PARTICLE_FILTER__
00033 
00034 #include "filter.h"
00035 #include "../pdf/conditionalpdf.h"
00036 #include "../pdf/mcpdf.h"
00037 
00038 // RS stands for Resample Scheme
00039 // TODO: Work this better out
00040 #define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
00041 #define MULTINOMIAL_RS 0
00042 /* Carpenter, Clifford and Fearnhead:
00043    Efficient implementation of particle
00044    filters for non-linear systems.
00045 
00046    See eg.
00047 
00048    @Article{        gordon93,
00049    author =      {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.},
00050    title =       {Novel approach to nonlinear/non-Gaussian state estimation},
00051    journal =     {IEE Proceedings-F},
00052    year =        {1993},
00053    volume =      {140},
00054    number =      {2},
00055    pages =       {107--113},
00056    annote =      {Multinomial Sampling}}
00057 */
00058 
00059 #define SYSTEMATIC_RS 1
00060 // A lot of possible systematic approaches to resampling are described
00061 // in literature.  The goal is to reduce the Monte Carlo Variance of
00062 // Particle filters.  One example is
00063 /*
00064 @Article{        carpenter99-improved,
00065   author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
00066   title =        {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems},
00067   journal =      {Radar, Sonar and Navigation, IEE Proceedings -},
00068   year =         {1999},
00069   volume =       {146},
00070   number =       {1},
00071   pages =        {2--7},
00072   month =        {February},
00073   annote =       {Describes systematic resampling, variance reduction}
00074 }
00075 */
00076 
00077 // KG TODO Check if systematic resampling = deterministic resampling
00078 // as described by kitagawa
00079 
00080 #define STRATIFIED_RS 2
00081 // Generate N samples not uniformly on [0,1] but generate 1 sample
00082 // uniformly in for each "stratum" u_j ~ U(j-1/N,j/N)
00083 // Variance reduction!
00084 /*
00085   @Article{         kitagawa96,
00086   author =       {Kitagawa, G.},
00087   title =        {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models },
00088   journal =      {Journal of Computational and Graphical Statistics},
00089   year =         {1996},
00090   volume =       {5},
00091   number =       {1},
00092   pages =        {1--25},
00093   annote =       {describes deterministic and stratified resampling}
00094   }
00095 */
00096 
00097 #define RESIDUAL_RS 3
00098 // sample "deterministically" the integer part of N \times w_j , the
00099 // perform multinomial sampling for the resulting N - \sum [N \times
00100 // w_j] where [] denotes the integer part.
00101 /*
00102   See eg. p.19
00103   @Article{         liuchen98,
00104   author =       {Liu, J. S. and Chen, R.},
00105   title =        {Sequential {M}onte {C}arlo methods for dynamic systems},
00106   journal =      {Journal of the American Statistical Association},
00107   year =         {1998},
00108   volume =       {93},
00109   pages =        {1032--1044}}
00110 */
00111 
00112 #define MINIMUM_VARIANCE_RS 4
00113 /*
00114   See eg.
00115   @TechReport{     carpenter99,
00116   author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
00117   title =        {Building robust simulation-based filters for evolving data sets},
00118   institution =  {Department of Statistics, University of Oxford},
00119   year =         {99},
00120   note =         {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}}
00121 */
00122 
00123 namespace BFL
00124 {
00125 
00127 
00159   template <typename StateVar, typename MeasVar> class ParticleFilter
00160     : public Filter<StateVar,MeasVar>
00161     {
00162     protected:
00163       virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
00164                                   const StateVar& u,
00165                                   MeasurementModel<MeasVar,StateVar>* const measmodel,
00166                                   const MeasVar& z,
00167                                   const StateVar& s);
00168 
00170 
00174       ConditionalPdf<StateVar,StateVar> * _proposal;
00175 
00177       WeightedSample<StateVar>  _sample;
00179       vector<WeightedSample<StateVar> > _old_samples;
00181       vector<WeightedSample<StateVar> > _new_samples;
00183       vector<Sample<StateVar> > _new_samples_unweighted;
00185       typename vector<WeightedSample<StateVar> >::iterator _os_it;
00187       typename vector<WeightedSample<StateVar> >::iterator _ns_it;
00188 
00190 
00193       int _resamplePeriod;
00194 
00196       double _resampleThreshold;
00197 
00199       int _resampleScheme;
00200 
00202       bool _dynamicResampling;
00203 
00205       bool _proposal_depends_on_meas;
00206 
00208       bool _created_post;
00209 
00211 
00219       virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
00220                                         const StateVar & u,
00221                                         MeasurementModel<MeasVar,StateVar> * const measmodel,
00222                                         const MeasVar & z,
00223                                         const StateVar & s);
00224 
00226 
00232       virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
00233                                          const StateVar & u,
00234                                          MeasurementModel<MeasVar,StateVar> * const measmodel,
00235                                          const MeasVar & z,
00236                                          const StateVar & s);
00237 
00239 
00241       virtual bool DynamicResampleStep();
00242 
00244 
00246       virtual bool StaticResampleStep();
00247 
00249       virtual bool Resample();
00250 
00251     public:
00253 
00264       ParticleFilter(MCPdf<StateVar> * prior,
00265                      ConditionalPdf<StateVar,StateVar> * proposal,
00266                      int resampleperiod = 0,
00267                      double resamplethreshold = 0,
00268                      int resamplescheme = DEFAULT_RS);
00269 
00270 
00272 
00284       ParticleFilter(MCPdf<StateVar> * prior,
00285                      MCPdf<StateVar> * post,
00286                      ConditionalPdf<StateVar,StateVar> * proposal,
00287                      int resampleperiod = 0,
00288                      double resamplethreshold = 0,
00289                      int resamplescheme = DEFAULT_RS);
00290 
00292       virtual ~ParticleFilter();
00294 
00296       ParticleFilter(const ParticleFilter<StateVar,MeasVar> & filt);
00297 
00299 
00304       virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf);
00305 
00307 
00309       ConditionalPdf<StateVar,StateVar> * ProposalGet();
00310 
00311       // implement virtual function
00312       virtual MCPdf<StateVar> * PostGet();
00313     };
00314 
00315 #include "particlefilter.cpp"
00316 
00317 } // End namespace BFL
00318 
00319 #endif // __PARTICLE_FILTER__


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Thu Feb 11 2016 22:31:57