EKparticlefilter.h
Go to the documentation of this file.
00001 // $ Id: $
00002 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00003 //               2008 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 //
00019 
00020 #ifndef __EK_PARTICLE_FILTER__
00021 #define __EK_PARTICLE_FILTER__
00022 
00023 #include "particlefilter.h"
00024 #include "../pdf/EKF_proposaldensity.h"
00025 #include "../pdf/mcpdf.h"
00026 #include <list>
00027 
00028 namespace BFL
00029 {
00031 
00035   class EKParticleFilter
00036     : public ParticleFilter<ColumnVector,ColumnVector>
00037     {
00038     protected:
00040       std::vector<SymmetricMatrix> _sampleCov;
00041       std::vector<SymmetricMatrix>::iterator _cov_it;
00042 
00043       std::vector<SymmetricMatrix> _tmpCov;
00044       std::vector<SymmetricMatrix>::iterator _tmpCovit;
00045 
00046       // helper variables for resampleing to prevent memory allocation on the heap
00047       const int _dimension;
00048       const int _num_samples;
00049       std::vector<WeightedSample<ColumnVector> > _old_samples;
00050       std::vector<WeightedSample<ColumnVector> >::iterator _oit;
00051       std::vector<WeightedSample<ColumnVector> > _result_samples;
00052       std::vector<WeightedSample<ColumnVector> >::iterator _rit;
00053       std::vector<double> _unif_samples;
00054       std::vector<double> _CumPDF;
00055       std::vector<double>::const_iterator _CumPDFit;
00056       ColumnVector _x_old;
00057       Sample<ColumnVector> _sample;
00058 
00059       virtual bool UpdateInternal(SystemModel<ColumnVector>* const sysmodel,
00060                                   const ColumnVector& u,
00061                                   MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00062                                   const ColumnVector& z,
00063                                   const ColumnVector& s);
00064 
00067       virtual bool ProposalStepInternal(SystemModel<ColumnVector> * const sysmodel,
00068                                         const ColumnVector & u,
00069                                         MeasurementModel<ColumnVector,ColumnVector> * const measmodel,
00070                                         const ColumnVector & z,
00071                                         const ColumnVector & s);
00072 
00074       virtual bool Resample();
00075 
00076     public:
00078 
00087       EKParticleFilter(MCPdf<ColumnVector> * prior,
00088                        int resampleperiod = 0,
00089                        double resamplethreshold = 0,
00090                        int resamplescheme = DEFAULT_RS);
00091 
00093       virtual ~EKParticleFilter();
00094     };
00095 
00096 } // End namespace BFL
00097 
00098 #endif // __EK_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 Fri Aug 28 2015 10:10:21