mixtureParticleFilter.h
Go to the documentation of this file.
00001 // Copyright (C) 2009 Tinne De Laet <first dot last at gmail dot com>
00002 // $Id: mixtureParticlefilter.h 2009-02-03 tdelaet $
00003 
00004  /***************************************************************************
00005  *   This library is free software; you can redistribute it and/or         *
00006  *   modify it under the terms of the GNU General Public                   *
00007  *   License as published by the Free Software Foundation;                 *
00008  *   version 2 of the License.                                             *
00009  *                                                                         *
00010  *   As a special exception, you may use this file as part of a free       *
00011  *   software library without restriction.  Specifically, if other files   *
00012  *   instantiate templates or use macros or inline functions from this     *
00013  *   file, or you compile this file and link it with other files to        *
00014  *   produce an executable, this file does not by itself cause the         *
00015  *   resulting executable to be covered by the GNU General Public          *
00016  *   License.  This exception does not however invalidate any other        *
00017  *   reasons why the executable file might be covered by the GNU General   *
00018  *   Public License.                                                       *
00019  *                                                                         *
00020  *   This library is distributed in the hope that it will be useful,       *
00021  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00022  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00023  *   Lesser General Public License for more details.                       *
00024  *                                                                         *
00025  *   You should have received a copy of the GNU General Public             *
00026  *   License along with this library; if not, write to the Free Software   *
00027  *   Foundation, Inc., 59 Temple Place,                                    *
00028  *   Suite 330, Boston, MA  02111-1307  USA                                *
00029  *                                                                         *
00030  ***************************************************************************/
00031 
00032 #ifndef __MIXTURE_PARTICLE_FILTER__
00033 #define __MIXTURE_PARTICLE_FILTER__
00034 
00035 #include "filter.h"
00036 #include "../pdf/conditionalpdf.h"
00037 #include "../pdf/mcpdf.h"
00038 #include "../pdf/mixture.h"
00039 
00040 // RS stands for Resample Scheme
00041 // TODO: Work this better out
00042 #define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
00043 #define MULTINOMIAL_RS 0
00044 /* Carpenter, Clifford and Fearnhead:
00045    Efficient implementation of particle
00046    filters for non-linear systems.
00047 
00048    See eg.
00049 
00050    @Article{        gordon93,
00051    author =      {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.},
00052    title =       {Novel approach to nonlinear/non-Gaussian state estimation},
00053    journal =     {IEE Proceedings-F},
00054    year =        {1993},
00055    volume =      {140},
00056    number =      {2},
00057    pages =       {107--113},
00058    annote =      {Multinomial Sampling}}
00059 */
00060 
00061 #define SYSTEMATIC_RS 1
00062 // A lot of possible systematic approaches to resampling are described
00063 // in literature.  The goal is to reduce the Monte Carlo Variance of
00064 // Particle filters.  One example is
00065 /*
00066 @Article{        carpenter99-improved,
00067   author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
00068   title =        {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems},
00069   journal =      {Radar, Sonar and Navigation, IEE Proceedings -},
00070   year =         {1999},
00071   volume =       {146},
00072   number =       {1},
00073   pages =        {2--7},
00074   month =        {February},
00075   annote =       {Describes systematic resampling, variance reduction}
00076 }
00077 */
00078 
00079 // KG TODO Check if systematic resampling = deterministic resampling
00080 // as described by kitagawa
00081 
00082 #define STRATIFIED_RS 2
00083 // Generate N samples not uniformly on [0,1] but generate 1 sample
00084 // uniformly in for each "stratum" u_j ~ U(j-1/N,j/N)
00085 // Variance reduction!
00086 /*
00087   @Article{         kitagawa96,
00088   author =       {Kitagawa, G.},
00089   title =        {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models },
00090   journal =      {Journal of Computational and Graphical Statistics},
00091   year =         {1996},
00092   volume =       {5},
00093   number =       {1},
00094   pages =        {1--25},
00095   annote =       {describes deterministic and stratified resampling}
00096   }
00097 */
00098 
00099 #define RESIDUAL_RS 3
00100 // sample "deterministically" the integer part of N \times w_j , the
00101 // perform multinomial sampling for the resulting N - \sum [N \times
00102 // w_j] where [] denotes the integer part.
00103 /*
00104   See eg. p.19
00105   @Article{         liuchen98,
00106   author =       {Liu, J. S. and Chen, R.},
00107   title =        {Sequential {M}onte {C}arlo methods for dynamic systems},
00108   journal =      {Journal of the American Statistical Association},
00109   year =         {1998},
00110   volume =       {93},
00111   pages =        {1032--1044}}
00112 */
00113 
00114 #define MINIMUM_VARIANCE_RS 4
00115 /*
00116   See eg.
00117   @TechReport{     carpenter99,
00118   author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
00119   title =        {Building robust simulation-based filters for evolving data sets},
00120   institution =  {Department of Statistics, University of Oxford},
00121   year =         {99},
00122   note =         {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}}
00123 */
00124 
00125 namespace BFL
00126 {
00127 
00129 
00153   template <typename StateVar, typename MeasVar> class MixtureParticleFilter
00154     : public Filter<StateVar,MeasVar>
00155     {
00156     protected:
00157       virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
00158                                   const StateVar& u,
00159                                   MeasurementModel<MeasVar,StateVar>* const measmodel,
00160                                   const MeasVar& z,
00161                                   const StateVar& s);
00162 
00164 
00168       ConditionalPdf<StateVar,StateVar> * _proposal;
00169 
00171       WeightedSample<StateVar>  _sample;
00173       vector<vector<WeightedSample<StateVar> > > _old_samplesVec;
00175       vector<vector<WeightedSample<StateVar> > > _new_samplesVec;
00177       vector< vector<Sample<StateVar> > > _new_samples_unweightedVec;
00179       typename vector<WeightedSample<StateVar> >::iterator _os_it;
00181       typename vector<WeightedSample<StateVar> >::iterator _ns_it;
00183       vector<Probability> _newMixtureWeights;
00185       vector<Probability> _sumWeights;
00186 
00188 
00191       int _resamplePeriod;
00192 
00194       double _resampleThreshold;
00195 
00197       int _resampleScheme;
00198 
00200       bool _dynamicResampling;
00201 
00203       bool _proposal_depends_on_meas;
00204 
00206       bool _created_post;
00207 
00209       //Mixture Pdf.
00213       int _maintainMixturePeriod;
00214 
00216 
00224       virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
00225                                         const StateVar & u,
00226                                         MeasurementModel<MeasVar,StateVar> * const measmodel,
00227                                         const MeasVar & z,
00228                                         const StateVar & s);
00229 
00231 
00240       virtual bool ProposalStepInternalOne(int component, SystemModel<StateVar> * const sysmodel,
00241                                         const StateVar & u,
00242                                         MeasurementModel<MeasVar,StateVar> * const measmodel,
00243                                         const MeasVar & z,
00244                                         const StateVar & s);
00245 
00247 
00253       virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
00254                                          const StateVar & u,
00255                                          MeasurementModel<MeasVar,StateVar> * const measmodel,
00256                                          const MeasVar & z,
00257                                          const StateVar & s);
00258 
00260 
00267       virtual bool UpdateWeightsInternalOne(int component, SystemModel<StateVar> * const sysmodel,
00268                                          const StateVar & u,
00269                                          MeasurementModel<MeasVar,StateVar> * const measmodel,
00270                                          const MeasVar & z,
00271                                          const StateVar & s);
00272 
00274       virtual bool DynamicResampleStep();
00275 
00277 
00280       virtual bool DynamicResampleStepOne(int component);
00281 
00283 
00285       virtual bool StaticResampleStep();
00286 
00288       virtual bool Resample();
00289 
00291 
00293       virtual bool ResampleOne(int component);
00294 
00296 
00298       virtual bool MaintainMixtureStep();
00299 
00301       virtual bool MaintainMixture();
00302 
00303 
00304     public:
00306 
00318       MixtureParticleFilter(Mixture<StateVar> * prior,
00319                      ConditionalPdf<StateVar,StateVar> * proposal,
00320                      int resampleperiod = 0,
00321                      double resamplethreshold = 0,
00322                      int resamplescheme = DEFAULT_RS,
00323              int maintainMixturePeriod = 1 );
00324 
00325 
00327 
00340       MixtureParticleFilter(Mixture<StateVar> * prior,
00341                      Mixture<StateVar> * post,
00342                      ConditionalPdf<StateVar,StateVar> * proposal,
00343                      int resampleperiod = 0,
00344                      double resamplethreshold = 0,
00345                      int resamplescheme = DEFAULT_RS,
00346              int maintainMixturePeriod = 1 );
00347 
00349       virtual ~MixtureParticleFilter();
00351 
00353       MixtureParticleFilter(const MixtureParticleFilter<StateVar,MeasVar> & filt);
00354 
00356       virtual void Reset(Mixture<StateVar> * prior);
00357 
00359 
00364       virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf);
00365 
00367 
00369       ConditionalPdf<StateVar,StateVar> * ProposalGet();
00370 
00371       // implement virtual function
00372       virtual Mixture<StateVar> * PostGet();
00373     };
00374 
00375 #include "mixtureParticleFilter.cpp"
00376 
00377 } // End namespace BFL
00378 
00379 #endif // __MIXTURE_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 Sun Oct 5 2014 22:29:53