particlefilter.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00002 //
00003 // This program is free software; you can redistribute it and/or modify
00004 // it under the terms of the GNU Lesser General Public License as published by
00005 // the Free Software Foundation; either version 2.1 of the License, or
00006 // (at your option) any later version.
00007 //
00008 // This program is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011 // GNU Lesser General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU Lesser General Public License
00014 // along with this program; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00016 //
00017 // $Id: particlefilter.cpp 29787 2008-12-09 19:38:08Z kgadeyne $
00018 
00019 #include "particlefilter.h"
00020 #include "../pdf/mcpdf.h"
00021 
00022 #define SV StateVar
00023 #define MV MeasVar
00024 #define MeasModel MeasurementModel
00025 
00026 #define STATE_AND_MEAS_VAR_DIFFERENT
00027 
00028 template <typename SV, typename MV>
00029 ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
00030                                       ConditionalPdf<SV,SV> * proposal,
00031                                       int resampleperiod,
00032                                       double resamplethreshold,
00033                                       int resamplescheme)
00034   : Filter<SV,MV>(prior)
00035   , _proposal(proposal)
00036   , _sample(WeightedSample<SV>(prior->DimensionGet()))
00037   , _resampleScheme(resamplescheme)
00038   , _created_post(true)
00039 {
00040   /* Initialize Post, at time = 0, post = prior
00041      To be more clean, this should be done in the filter base class,
00042      but this is impossible because of the pure virtuals...
00043   */
00044   this->_post = new MCPdf<SV>(prior->NumSamplesGet(),prior->DimensionGet());
00045   // Post is equal to prior at timetep 1
00046   /* Note: Dirty cast should be avoided by not demanding an MCPdf as
00047      prior and just sample from the prior instead  :-(
00048   */
00049   bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00050   assert(ret);
00051 
00052   // Initialise lists of samples
00053   _old_samples = (prior->ListOfSamplesGet());
00054   _new_samples = _old_samples;
00055 
00056 
00057   // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
00058   assert(!(resampleperiod == 0 && resamplethreshold == 0));
00059   assert(!(resampleperiod != 0 && resamplethreshold != 0));
00060 
00061   // dynamic resampling
00062   if (resampleperiod == 0)
00063    _dynamicResampling = true;
00064   // fixed period resampling
00065   else
00066     _dynamicResampling = false;
00067   _resamplePeriod = resampleperiod;
00068   _resampleThreshold = resamplethreshold;
00069 }
00070 
00071 
00072 
00073 template <typename SV, typename MV>
00074 ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
00075                                       MCPdf<SV> * post,
00076                                       ConditionalPdf<SV,SV> * proposal,
00077                                       int resampleperiod,
00078                                       double resamplethreshold,
00079                                       int resamplescheme)
00080   : Filter<SV,MV>(prior),
00081     _proposal(proposal),
00082     _resampleScheme(resamplescheme),
00083     _created_post(false)
00084 {
00085   this->_post = post;
00086   // Post is equal to prior at timestep 1
00087   /* Note: Dirty cast should be avoided by not demanding an MCPdf as
00088      prior and just sample from the prior instead  :-(
00089   */
00090   bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00091   assert(ret);
00092 
00093   // Initialise lists of samples
00094   _old_samples = (prior->ListOfSamplesGet());
00095   _new_samples = _old_samples;
00096 
00097   // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
00098   assert(!(resampleperiod == 0 && resamplethreshold == 0));
00099   assert(!(resampleperiod != 0 && resamplethreshold != 0));
00100 
00101   // dynamic resampling
00102   if (resampleperiod == 0)
00103    _dynamicResampling = true;
00104   // fixed period resampling
00105   else
00106     _dynamicResampling = false;
00107 
00108   _resamplePeriod = resampleperiod;
00109   _resampleThreshold = resamplethreshold;
00110 }
00111 
00112 
00113 
00114 
00115 template <typename SV, typename MV>
00116 ParticleFilter<SV,MV>::~ParticleFilter()
00117 {
00118   if (_created_post)
00119     delete this->_post;
00120 }
00121 
00122 template <typename SV, typename MV>
00123 ParticleFilter<SV,MV>::ParticleFilter(const ParticleFilter<SV,MV> & filter)
00124   : Filter<SV,MV>(filter),
00125     _created_post(true)
00126 {
00127   // Copy constructor of MCPdf
00128   // Probably a bug...
00129   this->_post = new MCPdf<SV>(dynamic_cast<MCPdf<SV> *>(filter._post));
00130 }
00131 
00132 template <typename SV, typename MV> void
00133 ParticleFilter<SV,MV>::ProposalSet(ConditionalPdf<SV,SV> * const cpdf)
00134 {
00135   _proposal = cpdf;
00136 }
00137 
00138 template <typename SV, typename MV> ConditionalPdf<SV,SV> *
00139 ParticleFilter<SV,MV>::ProposalGet()
00140 {
00141   return _proposal;
00142 }
00143 
00144 template <typename SV, typename MV> bool
00145 ParticleFilter<SV,MV>::ProposalStepInternal(SystemModel<SV> * const sysmodel,
00146                                             const SV & u,
00147                                             MeasurementModel<MV,SV> * const measmodel,
00148                                             const MV & z,
00149                                             const SV & s)
00150 {
00151   // Get all samples from the current post through proposal density
00152   _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00153 
00154   _ns_it = _new_samples.begin();
00155   for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
00156     {
00157       const SV& x_old = _os_it->ValueGet();
00158       _proposal->ConditionalArgumentSet(0,x_old);
00159 
00160       if (!sysmodel->SystemWithoutInputs())
00161         {
00162           _proposal->ConditionalArgumentSet(1,u);
00163           if (this->_proposal_depends_on_meas)
00164             {
00165           #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00166               _proposal->ConditionalArgumentSet(2,z);
00167               if (!measmodel->SystemWithoutSensorParams())
00168                 _proposal->ConditionalArgumentSet(3,s);
00169               #endif
00170             }
00171 
00172         }
00173       else // System without inputs
00174         {
00175           if (this->_proposal_depends_on_meas)
00176             {
00177           #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00178               _proposal->ConditionalArgumentSet(1,z);
00179               if (!measmodel->SystemWithoutSensorParams())
00180                 _proposal->ConditionalArgumentSet(2,s);
00181               #endif
00182 
00183             }
00184         }
00185       // Bug, make sampling method a parameter!
00186       _proposal->SampleFrom(_sample, DEFAULT,NULL);
00187       _ns_it->ValueSet(_sample.ValueGet());
00188       _ns_it->WeightSet(_os_it->WeightGet());
00189       _ns_it++;
00190     }
00191 
00192   (this->_timestep)++;
00193 
00194   // Update the list of samples
00195   return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00196 
00197 }
00198 
00199 
00200 template <typename SV, typename MV> bool
00201 ParticleFilter<SV,MV>::UpdateWeightsInternal(SystemModel<SV> * const sysmodel,
00202                                              const SV & u,
00203                                              MeasurementModel<MV,SV> * const measmodel,
00204                                              const MV & z,
00205                                              const SV & s)
00206 {
00207   Probability weightfactor = 1;
00208 
00209   // Update the weights
00210   // Same remarks as for the system update!
00211   _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00212   _os_it = _old_samples.begin();
00213 
00214   for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00215     {
00216       const SV& x_new = _ns_it->ValueGet();
00217       const SV& x_old = _os_it->ValueGet();
00218 
00219       if (sysmodel == NULL)
00220         {
00221           if (measmodel->SystemWithoutSensorParams() == true)
00222             weightfactor = measmodel->ProbabilityGet(z,x_new);
00223           else
00224             weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00225         }
00226       else // We do have a system model
00227         {
00228       _proposal->ConditionalArgumentSet(0,x_old);
00229           if (measmodel->SystemWithoutSensorParams() == true)
00230             {
00231               weightfactor = measmodel->ProbabilityGet(z,x_new);
00232               if (sysmodel->SystemWithoutInputs() == false)
00233                 {
00234                   _proposal->ConditionalArgumentSet(1,u);
00235                   if (this->_proposal_depends_on_meas){
00236                     #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00237                     _proposal->ConditionalArgumentSet(2,z);
00238                     #endif
00239                   }
00240 
00241                   if (_proposal->ProbabilityGet(x_new) != 0)
00242                     weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,x_old,u) / _proposal->ProbabilityGet(x_new) );
00243                   else weightfactor = 0;
00244                 }
00245               else // we do have a system without inputs
00246                 {
00247                   if (this->_proposal_depends_on_meas){
00248                     #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00249                     _proposal->ConditionalArgumentSet(1,z);
00250                     #endif
00251                   }
00252                   if ( _proposal->ProbabilityGet(x_new) != 0)
00253                     weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,_os_it->ValueGet()) / _proposal->ProbabilityGet(x_new) );
00254                   else weightfactor = 0;
00255                 }
00256             }
00257           else // System with sensor Parameters
00258             {
00259               weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00260             }
00261         }
00262       _ns_it->WeightSet(_ns_it->WeightGet() * weightfactor);
00263 
00264       _os_it++;
00265     }
00266   // Update the sample list of post the SumofWeights of the pdf
00267   return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00268 
00269 }
00270 
00271 template <typename SV, typename MV> bool
00272 ParticleFilter<SV,MV>::DynamicResampleStep()
00273 {
00274   // Resampling?
00275   bool resampling = false;
00276   double sum_sq_weigths = 0.0;
00277 
00278   // Resampling if necessary
00279   if ( this->_dynamicResampling)
00280     {
00281       // Check if sum of 1 / \sum{(wi_normalised)^2} < threshold
00282       // This is the criterion proposed by Liu
00283       // BUG  foresee other methods of approximation/calculating
00284       // effective sample size.  Let the user implement this in !
00285       _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00286       for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00287         {
00288           sum_sq_weigths += pow(_ns_it->WeightGet(),2);
00289         }
00290       if ((1.0 / sum_sq_weigths) < _resampleThreshold)
00291         {
00292           // #define __RESAMPLE_DEBUG__
00293 #ifdef __RESAMPLE_DEBUG__
00294           cout << "resampling now: " << this->_timestep
00295                << "\tN_eff: " << (1.0 / sum_sq_weigths) << endl;
00296 #endif // __RESAMPLE_DEBUG__
00297           resampling = true;
00298         }
00299     }
00300   if (resampling == true)
00301     return this->Resample();
00302   else
00303     return true;
00304 }
00305 
00306 
00307 template <typename SV, typename MV> bool
00308 ParticleFilter<SV,MV>::StaticResampleStep()
00309 {
00310   // Resampling if necessary
00311   if ( (!this->_dynamicResampling) &&  (((this->_timestep) % _resamplePeriod) == 0) && (this->_timestep != 0))
00312     return this->Resample();
00313   return true;
00314 }
00315 
00316 
00317 template <typename SV, typename MV> bool
00318 ParticleFilter<SV,MV>::UpdateInternal(SystemModel<StateVar>* const sysmodel,
00319                                       const StateVar& u,
00320                                       MeasurementModel<MeasVar,StateVar>* const measmodel,
00321                                       const MeasVar& z,
00322                                       const StateVar& s)
00323 {
00324   bool result = true;
00325 
00326   // Only makes sense if there is a system model?
00327   // Bug, not completely true, but should do for now...
00328   if (sysmodel != NULL)
00329     {
00330       result = result && this->StaticResampleStep();
00331       result = result && this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
00332 
00333     }
00334   // Updating the weights only makes sense using a measurement model
00335   if (measmodel != NULL)
00336     {
00337       result = result && this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s);
00338       result = result && this->DynamicResampleStep();
00339     }
00340 
00341   return result;
00342 }
00343 
00344 template <typename SV, typename MV> bool
00345 ParticleFilter<SV,MV>::Resample()
00346 {
00347   int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
00348   // #define __PARTICLEFILTER_DEBUG__
00349 #ifdef __PARTICLEFILTER_DEBUG__
00350   cout << "PARTICLEFILTER: resampling now" << endl;
00351   _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00352   for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00353     {
00354       cout << "PF: Old samples:\n";
00355       cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00356     }
00357 #endif // __PARTICLEFILTER_DEBUG
00358   switch(_resampleScheme)
00359     {
00360     case MULTINOMIAL_RS:
00361       {
00362         (dynamic_cast<MCPdf<SV> *>(this->_post))->SampleFrom(_new_samples_unweighted, NumSamples,RIPLEY,NULL);
00363         break;
00364       }
00365     case SYSTEMATIC_RS:{break;}
00366     case STRATIFIED_RS:{break;}
00367     case RESIDUAL_RS:{break;}
00368     default:
00369       {
00370         cerr << "Sampling method not supported" << endl;
00371         break;
00372       }
00373     }
00374   bool result = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples_unweighted);
00375 #ifdef __PARTICLEFILTER_DEBUG__
00376   cout << "PARTICLEFILTER: after resampling" << endl;
00377   _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00378   for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00379     {
00380       cout << "PF: New samples:\n";
00381       cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00382     }
00383 #endif // __PARTICLEFILTER_DEBUG
00384 
00385   return result;
00386 }
00387 
00388 
00389 template<typename SV, typename MV> MCPdf<SV> *
00390 ParticleFilter<SV,MV>::PostGet()
00391 {
00392   return (MCPdf<SV>*)Filter<SV,MV>::PostGet();
00393 }


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