mixtureParticleFilter.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2009 Tinne De Laet <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: mixtureParticleFilter.cpp 2009-02-02 tdelaet $
00018 
00019 #include "mixtureParticleFilter.h"
00020 #include "../pdf/mixture.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 MixtureParticleFilter<SV,MV>::MixtureParticleFilter(Mixture<SV> * prior,
00030                                       ConditionalPdf<SV,SV> * proposal,
00031                                       int resampleperiod,
00032                                       double resamplethreshold,
00033                                       int resamplescheme,
00034                       int maintainMixturePeriod)
00035   : Filter<SV,MV>(prior)
00036   , _proposal(proposal)
00037   , _sample(WeightedSample<SV>(prior->DimensionGet()))
00038   , _resampleScheme(resamplescheme)
00039   , _created_post(true)
00040   , _newMixtureWeights(prior->NumComponentsGet())
00041   , _sumWeights(prior->NumComponentsGet())
00042   , _old_samplesVec(prior->NumComponentsGet())
00043   , _new_samplesVec(prior->NumComponentsGet())
00044   , _new_samples_unweightedVec(prior->NumComponentsGet())
00045   , _maintainMixturePeriod(maintainMixturePeriod)
00046 {
00047   /* Initialize Post, at time = 0, post = prior
00048      To be more clean, this should be done in the filter base class,
00049      but this is impossible because of the pure virtuals...
00050   */
00051   // Post is equal to prior at timetep 1
00052   this->_post = prior->Clone();
00053 
00054   // Initialise vector of lists of samples
00055   for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00056   {
00057     _old_samplesVec[i] = (dynamic_cast<MCPdf<SV> *>(prior->ComponentGet(i))->ListOfSamplesGet());
00058   }
00059   _new_samplesVec = _old_samplesVec;
00060 
00061 
00062 
00063   // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
00064   assert(!(resampleperiod == 0 && resamplethreshold == 0));
00065   assert(!(resampleperiod != 0 && resamplethreshold != 0));
00066 
00067   // dynamic resampling
00068   if (resampleperiod == 0)
00069    _dynamicResampling = true;
00070   // fixed period resampling
00071   else
00072     _dynamicResampling = false;
00073   _resamplePeriod = resampleperiod;
00074   _resampleThreshold = resamplethreshold;
00075 }
00076 
00077 
00078 
00079 template <typename SV, typename MV>
00080 MixtureParticleFilter<SV,MV>::MixtureParticleFilter(Mixture<SV> * prior,
00081                                       Mixture<SV> * post,
00082                                       ConditionalPdf<SV,SV> * proposal,
00083                                       int resampleperiod,
00084                                       double resamplethreshold,
00085                                       int resamplescheme,
00086                       int maintainMixturePeriod)
00087   : Filter<SV,MV>(prior)
00088   ,  _proposal(proposal)
00089   ,  _resampleScheme(resamplescheme)
00090   ,  _created_post(false)
00091   , _newMixtureWeights(prior->NumComponentsGet())
00092   , _sumWeights(prior->NumComponentsGet())
00093   , _old_samplesVec(prior->NumComponentsGet())
00094   , _new_samplesVec(prior->NumComponentsGet())
00095   , _new_samples_unweightedVec(prior->NumComponentsGet())
00096   , _maintainMixturePeriod(maintainMixturePeriod)
00097 {
00098   this->_post = post;
00099   // Post is equal to prior at timestep 1
00100   /* Note: Dirty cast should be avoided by not demanding an MCPdf as
00101      prior and just sample from the prior instead  :-(
00102   */
00103   bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00104   assert(ret);
00105   for(int i =0 ; i < prior.NumComponentsGet() ; i++)
00106   {
00107     bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post->ComponentGet(i)))->ListOfSamplesSet(prior->ComponentGet(i)->ListOfSamplesGet());
00108   }
00109 
00110   // Initialise vector of lists of samples
00111   for(int i =0 ; i < prior.NumComponentsGet() ; i++)
00112   {
00113     _old_samplesVec[i] = (prior.ComponentGet(i)->ListOfSamplesGet());
00114   }
00115   _new_samplesVec = _old_samplesVec;
00116 
00117   // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
00118   assert(!(resampleperiod == 0 && resamplethreshold == 0));
00119   assert(!(resampleperiod != 0 && resamplethreshold != 0));
00120 
00121   // dynamic resampling
00122   if (resampleperiod == 0)
00123    _dynamicResampling = true;
00124   // fixed period resampling
00125   else
00126     _dynamicResampling = false;
00127 
00128   _resamplePeriod = resampleperiod;
00129   _resampleThreshold = resamplethreshold;
00130 }
00131 
00132 
00133 
00134 
00135 template <typename SV, typename MV>
00136 MixtureParticleFilter<SV,MV>::~MixtureParticleFilter()
00137 {
00138   if (_created_post)
00139     delete this->_post;
00140 }
00141 
00142 template <typename SV, typename MV>
00143 MixtureParticleFilter<SV,MV>::MixtureParticleFilter(const MixtureParticleFilter<SV,MV> & filter)
00144   : Filter<SV,MV>(filter)
00145   , _created_post(true)
00146 {
00147   // Clone the Mixture posterior of filter 
00148   this->_post = filter.PostGet().Clone();
00149   this->_newMixtureWeights.resize(this->_post->NumComponentsGet());
00150   this->_sumWeights.resize(this->_post->NumComponentsGet());
00151 }
00152 
00153 template <typename SV, typename MV> void
00154 MixtureParticleFilter<SV,MV>::Reset(Mixture<SV> * prior)
00155 {
00156     this->_prior = prior;
00157     delete this->_post;
00158     this->_post = prior->Clone();
00159     this->_newMixtureWeights.resize(dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet());
00160     this->_sumWeights.resize(dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet());
00161 }
00162 
00163 template <typename SV, typename MV> void
00164 MixtureParticleFilter<SV,MV>::ProposalSet(ConditionalPdf<SV,SV> * const cpdf)
00165 {
00166   _proposal = cpdf;
00167 }
00168 
00169 template <typename SV, typename MV> ConditionalPdf<SV,SV> *
00170 MixtureParticleFilter<SV,MV>::ProposalGet()
00171 {
00172   return _proposal;
00173 }
00174 
00175 // Proposal step can be executed for each component in the mixture seperately
00176 template <typename SV, typename MV> bool
00177 MixtureParticleFilter<SV,MV>::ProposalStepInternal(SystemModel<SV> * const sysmodel,
00178                                             const SV & u,
00179                                             MeasurementModel<MV,SV> * const measmodel,
00180                                             const MV & z,
00181                                             const SV & s)
00182 {
00183     bool result = true;
00184     for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00185     {
00186         result = result && this->ProposalStepInternalOne(i,sysmodel,u,measmodel,z,s);
00187     }
00188     return result;
00189 }
00190 
00191 // Proposal step for one component
00192 template <typename SV, typename MV> bool
00193 MixtureParticleFilter<SV,MV>::ProposalStepInternalOne(int component, SystemModel<SV> * const sysmodel,
00194                                             const SV & u,
00195                                             MeasurementModel<MV,SV> * const measmodel,
00196                                             const MV & z,
00197                                             const SV & s)
00198 {
00199   // Get all samples from the current post through proposal density
00200   _old_samplesVec[component]= (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet();
00201 
00202   _ns_it = _new_samplesVec[component].begin();
00203   for ( _os_it=_old_samplesVec[component].begin(); _os_it != _old_samplesVec[component].end() ; _os_it++)
00204     {
00205       const SV& x_old = _os_it->ValueGet();
00206       _proposal->ConditionalArgumentSet(0,x_old);
00207 
00208       if (!sysmodel->SystemWithoutInputs())
00209         {
00210           _proposal->ConditionalArgumentSet(1,u);
00211           if (this->_proposal_depends_on_meas)
00212             {
00213           #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00214               _proposal->ConditionalArgumentSet(2,z);
00215               if (!measmodel->SystemWithoutSensorParams())
00216                 _proposal->ConditionalArgumentSet(3,s);
00217               #endif
00218             }
00219 
00220         }
00221       else // System without inputs
00222         {
00223           if (this->_proposal_depends_on_meas)
00224             {
00225           #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00226               _proposal->ConditionalArgumentSet(1,z);
00227               if (!measmodel->SystemWithoutSensorParams())
00228                 _proposal->ConditionalArgumentSet(2,s);
00229               #endif
00230 
00231             }
00232         }
00233       // Bug, make sampling method a parameter!
00234       _proposal->SampleFrom(_sample, DEFAULT,NULL);
00235       _ns_it->ValueSet(_sample.ValueGet());
00236       _ns_it->WeightSet(_os_it->WeightGet());
00237       _ns_it++;
00238     }
00239 
00240   (this->_timestep)++;
00241 
00242   // Update the list of samples
00243   return (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesUpdate(_new_samplesVec[component]);
00244 
00245 }
00246 
00247 template <typename SV, typename MV> bool
00248 MixtureParticleFilter<SV,MV>::UpdateWeightsInternal(SystemModel<SV> * const sysmodel,
00249                                              const SV & u,
00250                                              MeasurementModel<MV,SV> * const measmodel,
00251                                              const MV & z,
00252                                              const SV & s)
00253 {
00254   // TODO: first calculate new weights of mixture components
00255     bool result = true;
00256     for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00257     {
00258         this->UpdateWeightsInternalOne(i,sysmodel,u,measmodel,z,s);
00259     }
00260     for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00261     {
00262         // calculate the new unnormalized mixture weights:
00263         // new_weight = old_weight * _sumWeights[i]
00264         // _sumWeights[i] is the sum of the particle weights of the i'th component
00265         // MPCdf and is an approximation of the i-th component likelihood
00266         _newMixtureWeights[i] = dynamic_cast<Mixture<SV> *>(this->_post)->WeightGet(i) * _sumWeights[i];
00267     }
00268     // Update the mixture weights
00269     result = result && dynamic_cast<Mixture<SV> * >(this->_post)->WeightsSet(_newMixtureWeights); // this function automatically takes care of normalization
00270 
00271     return result;
00272 }
00273 
00274 template <typename SV, typename MV> bool
00275 MixtureParticleFilter<SV,MV>::UpdateWeightsInternalOne(int component,SystemModel<SV> * const sysmodel,
00276                                              const SV & u,
00277                                              MeasurementModel<MV,SV> * const measmodel,
00278                                              const MV & z,
00279                                              const SV & s)
00280 {
00281   if(component < 0 || component > dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet())
00282   {
00283         cerr<< "MixtureParticleFilter::UpdateWeightsInternalOne called with invalid component number " << endl;
00284         return false;
00285   } 
00286   _sumWeights[component] = 0.0; 
00287   Probability weightfactor = 1;
00288   // Update the weights
00289   // Same remarks as for the system update!
00290   _new_samplesVec[component] = (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet();
00291   _os_it = _old_samplesVec[component].begin();
00292 
00293   for ( _ns_it=_new_samplesVec[component].begin(); _ns_it != _new_samplesVec[component].end() ; _ns_it++)
00294     {
00295       const SV& x_new = _ns_it->ValueGet();
00296       const SV& x_old = _os_it->ValueGet();
00297 
00298       if (sysmodel == NULL)
00299         {
00300           if (measmodel->SystemWithoutSensorParams() == true)
00301       {
00302             weightfactor = measmodel->ProbabilityGet(z,x_new);
00303      }
00304           else
00305             weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00306         }
00307       else // We do have a system model
00308         {
00309       _proposal->ConditionalArgumentSet(0,x_old);
00310           if (measmodel->SystemWithoutSensorParams() == true)
00311             {
00312               weightfactor = measmodel->ProbabilityGet(z,x_new);
00313               if (sysmodel->SystemWithoutInputs() == false)
00314                 {
00315                   _proposal->ConditionalArgumentSet(1,u);
00316                   if (this->_proposal_depends_on_meas){
00317                     #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00318                     _proposal->ConditionalArgumentSet(2,z);
00319                     #endif
00320                   }
00321 
00322                   if (_proposal->ProbabilityGet(x_new) != 0)
00323                     weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,x_old,u) / _proposal->ProbabilityGet(x_new) );
00324                   else weightfactor = 0;
00325                 }
00326               else // we do have a system without inputs
00327                 {
00328                   if (this->_proposal_depends_on_meas){
00329                     #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00330                     _proposal->ConditionalArgumentSet(1,z);
00331                     #endif
00332                   }
00333                   if ( _proposal->ProbabilityGet(x_new) != 0)
00334                     weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,_os_it->ValueGet()) / _proposal->ProbabilityGet(x_new) );
00335                   else weightfactor = 0;
00336                 }
00337             }
00338           else // System with sensor Parameters
00339             {
00340               weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00341             }
00342         }
00343       double new_weight = _ns_it->WeightGet() * weightfactor;
00344       _ns_it->WeightSet(new_weight);
00345       // add the new weight to the _sumWeights of this component
00346       _sumWeights[component] = _sumWeights[component] + new_weight; 
00347 
00348       _os_it++;
00349     }
00350   // Update the sample list of post the SumofWeights of the pdf
00351   // Update the mixture PDfs
00352   return (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesUpdate(_new_samplesVec[component]);
00353 }
00354 
00355 template <typename SV, typename MV> bool
00356 MixtureParticleFilter<SV,MV>::DynamicResampleStep()
00357 {
00358     bool result = true;
00359     // Independent resampling for different components
00360     for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00361     {
00362         result = result && this->DynamicResampleStepOne(i);
00363     }
00364     return result;
00365 }
00366 
00367 template <typename SV, typename MV> bool
00368 MixtureParticleFilter<SV,MV>::DynamicResampleStepOne(int component)
00369 {
00370   // Resampling?
00371   bool resampling = false;
00372   double sum_sq_weigths = 0.0;
00373 
00374   // Resampling if necessary
00375   if ( this->_dynamicResampling)
00376     {
00377       // Check if sum of 1 / \sum{(wi_normalised)^2} < threshold
00378       // This is the criterion proposed by Liu
00379       // BUG  foresee other methods of approximation/calculating
00380       // effective sample size.  Let the user implement this in !
00381       _new_samplesVec[component] = (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet();
00382       for ( _ns_it=_new_samplesVec[component].begin(); _ns_it != _new_samplesVec[component].end() ; _ns_it++)
00383         {
00384           sum_sq_weigths += pow(_ns_it->WeightGet(),2);
00385         }
00386       if ((1.0 / sum_sq_weigths) < _resampleThreshold)
00387         {
00388           // #define __RESAMPLE_DEBUG__
00389 #ifdef __RESAMPLE_DEBUG__
00390           cout << "resampling now: " << this->_timestep
00391                << "\tN_eff: " << (1.0 / sum_sq_weigths) << endl;
00392 #endif // __RESAMPLE_DEBUG__
00393           resampling = true;
00394         }
00395     }
00396   if (resampling == true)
00397     return this->ResampleOne(component);
00398   else
00399     return true;
00400 }
00401 
00402 
00403 template <typename SV, typename MV> bool
00404 MixtureParticleFilter<SV,MV>::StaticResampleStep()
00405 {
00406   // Resampling if necessary
00407   if ( (!this->_dynamicResampling) &&  (((this->_timestep) % _resamplePeriod) == 0) && (this->_timestep != 0))
00408     return this->Resample();
00409   return true;
00410 }
00411 
00412 
00413 template <typename SV, typename MV> bool
00414 MixtureParticleFilter<SV,MV>::UpdateInternal(SystemModel<StateVar>* const sysmodel,
00415                                       const StateVar& u,
00416                                       MeasurementModel<MeasVar,StateVar>* const measmodel,
00417                                       const MeasVar& z,
00418                                       const StateVar& s)
00419 {
00420   bool result = true;
00421 
00422   // Only makes sense if there is a system model?
00423   // Bug, not completely true, but should do for now...
00424   if (sysmodel != NULL)
00425     {
00426       result = result && this->StaticResampleStep();
00427       result = result && this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
00428 
00429     }
00430   // Updating the weights only makes sense using a measurement model
00431   if (measmodel != NULL)
00432     {
00433       result = result && this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s);
00434       result = result && this->DynamicResampleStep();
00435     }
00436 
00437   // Mixture Computation: recompute mixture representation to take into account
00438   // possibly varying number of modes.
00439   result = result && this->MaintainMixture();
00440 
00441   return result;
00442 }
00443 
00444 template <typename SV, typename MV> bool
00445 MixtureParticleFilter<SV,MV>::Resample()
00446 {
00447     bool result = true;
00448     // Independent resampling for different components
00449     for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++)
00450     {
00451         result = result && this->ResampleOne(i);
00452     }
00453     return result;
00454 }
00455 
00456 template <typename SV, typename MV> bool
00457 MixtureParticleFilter<SV,MV>::ResampleOne(int component)
00458 {
00459   int NumSamples = (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->NumSamplesGet();
00460   // #define __MixtureParticleFilter_DEBUG__
00461 #ifdef __MixtureParticleFilter_DEBUG__
00462   cout << "MixtureParticleFilter: resampling now" << endl;
00463   _new_samplesVec[component]= (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet();
00464   for ( _ns_it=_new_samplesVec[component].begin(); _ns_it != _new_samplesVec[component].end() ; _ns_it++)
00465     {
00466       cout << "PF: Old samples:\n";
00467       cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00468     }
00469 #endif // __MixtureParticleFilter_DEBUG
00470   switch(_resampleScheme)
00471     {
00472     case MULTINOMIAL_RS:
00473       {
00474         (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->SampleFrom(_new_samples_unweightedVec[component], NumSamples,RIPLEY,NULL);
00475         break;
00476       }
00477     case SYSTEMATIC_RS:{break;}
00478     case STRATIFIED_RS:{break;}
00479     case RESIDUAL_RS:{break;}
00480     default:
00481       {
00482         cerr << "Sampling method not supported" << endl;
00483         break;
00484       }
00485     }
00486   bool result = (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesUpdate(_new_samples_unweightedVec[component]);
00487 #ifdef __MixtureParticleFilter_DEBUG__
00488   cout << "MixtureParticleFilter: after resampling" << endl;
00489   _new_samplesVec[component]= (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet();
00490   for ( _ns_it=_new_samplesVec[component].begin(); _ns_it != _new_samplesVec[component].end() ; _ns_it++)
00491     {
00492       cout << "PF: New samples:\n";
00493       cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00494     }
00495 #endif // __MixtureParticleFilter_DEBUG
00496 
00497   return result;
00498 }
00499 
00500 
00501 template<typename SV, typename MV> Mixture<SV> *
00502 MixtureParticleFilter<SV,MV>::PostGet()
00503 {
00504   return (Mixture<SV>*)Filter<SV,MV>::PostGet();
00505 }
00506 
00507 template <typename SV, typename MV> bool
00508 MixtureParticleFilter<SV,MV>::MaintainMixtureStep()
00509 {
00510   // Resampling if necessary
00511   if ( (((this->_timestep) % _maintainMixturePeriod) == 0) && (this->_timestep != 0))
00512     return this->MaintainMixture();
00513   return true;
00514 }
00515 
00516 template <typename SV, typename MV> bool
00517 MixtureParticleFilter<SV,MV>::MaintainMixture()
00518 {
00519   // Default method doesn't take care of Mixture Maintainance
00520   return true;
00521 }


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 Mon Feb 11 2019 03:45:12