EKparticlefilter.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00002 //               2008 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 //
00018 
00019 #include "EKparticlefilter.h"
00020 
00021 #define MeasModel MeasurementModel
00022 #define CV ColumnVector
00023 namespace BFL
00024 {
00025 
00026   EKParticleFilter::EKParticleFilter(MCPdf<CV> * prior,
00027                                      int resampleperiod,
00028                                      double resamplethreshold,
00029                                      int resamplescheme)
00030     : ParticleFilter<ColumnVector,ColumnVector>(prior,NULL,
00031                                                 resampleperiod,
00032                                                 resamplethreshold,
00033                                                 resamplescheme)
00034     , _dimension(prior->DimensionGet())
00035     , _num_samples(prior->NumSamplesGet())
00036   {
00037     this->_proposal_depends_on_meas = true;
00038     _proposal = new EKFProposalDensity(NULL,NULL);
00039 
00040     _sampleCov.assign(_num_samples,prior->CovarianceGet());
00041     _tmpCov.assign(_num_samples,prior->CovarianceGet());
00042     _old_samples.assign(_num_samples,WeightedSample<ColumnVector>(_dimension));
00043     _result_samples.assign(_num_samples,WeightedSample<ColumnVector>(_dimension));
00044     _unif_samples.assign(_num_samples,0.0);
00045     _CumPDF.assign(_num_samples,0.0);
00046     _x_old.resize(_num_samples);
00047     _sample.DimensionSet(_dimension);
00048   }
00049 
00050   EKParticleFilter::~EKParticleFilter(){
00051     delete _proposal;
00052   }
00053 
00054   // KG: 20040702:  This method will be exactly the same for.  Extra
00055   // class between?
00056   bool
00057   EKParticleFilter::UpdateInternal(SystemModel<CV>* const sysmodel,
00058                                    const CV& u,
00059                                    MeasModel<CV,CV>* const measmodel,
00060                                    const CV& z,
00061                                    const CV& s)
00062   {
00063     bool result = true;
00064     dynamic_cast<FilterProposalDensity *>(_proposal)->SystemModelSet(dynamic_cast<AnalyticSystemModelGaussianUncertainty *>(sysmodel));
00065     dynamic_cast<FilterProposalDensity *>(_proposal)->MeasurementModelSet(dynamic_cast<AnalyticMeasurementModelGaussianUncertainty *>(measmodel));
00066     // Proposalstep is redefined here...
00067     this->StaticResampleStep();
00068     result = this->ProposalStepInternal(sysmodel,u,measmodel,z,s) && result;
00069     result = this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s) && result;
00070     this->DynamicResampleStep();
00071 
00072     return result;
00073   }
00074 
00075   bool
00076   EKParticleFilter::Resample()
00077   {
00078     // They're should be a cleaner solution then doubling the code
00079     // from mcpdf.h, doesn't it??
00080     // ONLY RIPLEY SAMPLING SUPPORTED FOR NOW!
00081     _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
00082     int numsamples = _old_samples.size();
00083     for ( int i = 0; i < numsamples ; i++) _unif_samples[i] = runif();
00084     _unif_samples[numsamples-1] = pow(_unif_samples[numsamples-1], double (1.0/numsamples));
00085     for ( int i = numsamples-2; i >= 0 ; i--){
00086                 _unif_samples[i] = pow(_unif_samples[i],double (1.0/(i+1))) * _unif_samples[i+1];}
00087 
00088     unsigned int index = 0;
00089     _oit = _old_samples.begin();
00090     _CumPDF = (dynamic_cast<MCPdf<CV> *>(this->_post))->CumulativePDFGet();
00091     _CumPDFit = _CumPDF.begin();
00092     _rit = _result_samples.begin();
00093     _cov_it = _sampleCov.begin(); _tmpCovit = _tmpCov.begin();
00094 
00095     for ( int i = 0; i < numsamples ; i++)
00096       {
00097         while ( _unif_samples[i] > *_CumPDFit )
00098           {
00099             assert(index <= (unsigned int)numsamples);
00100             index++; _oit++; _CumPDFit++; _cov_it++;
00101 
00102           }
00103         _oit--; _cov_it--;
00104         *(_rit) = *(_oit);
00105     *_tmpCovit = *_cov_it;
00106         _oit++; _cov_it++;
00107 
00108         _rit++; _tmpCovit++;
00109       }
00110 
00111     // Update lists
00112     _sampleCov = _tmpCov;
00113     return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_result_samples);
00114   }
00115 
00116   bool
00117   EKParticleFilter::ProposalStepInternal(SystemModel<CV>* const sysmodel,
00118                                          const CV& u,
00119                                          MeasModel<CV,CV>* const measmodel,
00120                                          const CV& z,
00121                                          const CV& s)
00122   {
00123     _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
00124 
00125     _ns_it = _new_samples.begin();
00126     _cov_it = _sampleCov.begin();
00127 
00128     for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
00129       {
00130         _x_old = _os_it->ValueGet();
00131 
00132         // Set sample Covariance
00133         dynamic_cast<FilterProposalDensity *>(this->_proposal)->SampleCovSet(*_cov_it);
00134 
00135         _proposal->ConditionalArgumentSet(0,_x_old);
00136 
00137         if (!sysmodel->SystemWithoutInputs())
00138           {
00139             _proposal->ConditionalArgumentSet(1,u);
00140             if (this->_proposal_depends_on_meas)
00141               {
00142                 _proposal->ConditionalArgumentSet(2,z);
00143                 if (!measmodel->SystemWithoutSensorParams())
00144                   _proposal->ConditionalArgumentSet(3,s);
00145               }
00146           }
00147         else // System without inputs
00148           {
00149             if (this->_proposal_depends_on_meas)
00150               {
00151                 _proposal->ConditionalArgumentSet(1,z);
00152                 if (!measmodel->SystemWithoutSensorParams())
00153                   _proposal->ConditionalArgumentSet(2,s);
00154               }
00155           }
00156         // Bug, make sampling method a parameter!
00157         _proposal->SampleFrom(_sample, DEFAULT,NULL);
00158 
00159         _ns_it->ValueSet(_sample.ValueGet());
00160         _ns_it->WeightSet(_os_it->WeightGet());
00161         _ns_it++;
00162 
00163         // Update Covariances here
00164         *_cov_it = _proposal->CovarianceGet();
00165         _cov_it++;
00166 
00167       }
00168 
00169     (this->_timestep)++;
00170     // Update the list of samples
00171     return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00172 
00173   }
00174 }


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