Bayesian Filtering Library
Generated from SVN r
|
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__