Bayesian Filtering Library
Generated from SVN r
|
00001 // Copyright (C) 2009 Tinne De Laet <first dot last at gmail dot com> 00002 // $Id: mixtureParticlefilter.h 2009-02-03 tdelaet $ 00003 00004 /*************************************************************************** 00005 * This library is free software; you can redistribute it and/or * 00006 * modify it under the terms of the GNU General Public * 00007 * License as published by the Free Software Foundation; * 00008 * version 2 of the License. * 00009 * * 00010 * As a special exception, you may use this file as part of a free * 00011 * software library without restriction. Specifically, if other files * 00012 * instantiate templates or use macros or inline functions from this * 00013 * file, or you compile this file and link it with other files to * 00014 * produce an executable, this file does not by itself cause the * 00015 * resulting executable to be covered by the GNU General Public * 00016 * License. This exception does not however invalidate any other * 00017 * reasons why the executable file might be covered by the GNU General * 00018 * Public License. * 00019 * * 00020 * This library is distributed in the hope that it will be useful, * 00021 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00022 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 00023 * Lesser General Public License for more details. * 00024 * * 00025 * You should have received a copy of the GNU General Public * 00026 * License along with this library; if not, write to the Free Software * 00027 * Foundation, Inc., 59 Temple Place, * 00028 * Suite 330, Boston, MA 02111-1307 USA * 00029 * * 00030 ***************************************************************************/ 00031 00032 #ifndef __MIXTURE_PARTICLE_FILTER__ 00033 #define __MIXTURE_PARTICLE_FILTER__ 00034 00035 #include "filter.h" 00036 #include "../pdf/conditionalpdf.h" 00037 #include "../pdf/mcpdf.h" 00038 #include "../pdf/mixture.h" 00039 00040 // RS stands for Resample Scheme 00041 // TODO: Work this better out 00042 #define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL 00043 #define MULTINOMIAL_RS 0 00044 /* Carpenter, Clifford and Fearnhead: 00045 Efficient implementation of particle 00046 filters for non-linear systems. 00047 00048 See eg. 00049 00050 @Article{ gordon93, 00051 author = {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.}, 00052 title = {Novel approach to nonlinear/non-Gaussian state estimation}, 00053 journal = {IEE Proceedings-F}, 00054 year = {1993}, 00055 volume = {140}, 00056 number = {2}, 00057 pages = {107--113}, 00058 annote = {Multinomial Sampling}} 00059 */ 00060 00061 #define SYSTEMATIC_RS 1 00062 // A lot of possible systematic approaches to resampling are described 00063 // in literature. The goal is to reduce the Monte Carlo Variance of 00064 // Particle filters. One example is 00065 /* 00066 @Article{ carpenter99-improved, 00067 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.}, 00068 title = {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems}, 00069 journal = {Radar, Sonar and Navigation, IEE Proceedings -}, 00070 year = {1999}, 00071 volume = {146}, 00072 number = {1}, 00073 pages = {2--7}, 00074 month = {February}, 00075 annote = {Describes systematic resampling, variance reduction} 00076 } 00077 */ 00078 00079 // KG TODO Check if systematic resampling = deterministic resampling 00080 // as described by kitagawa 00081 00082 #define STRATIFIED_RS 2 00083 // Generate N samples not uniformly on [0,1] but generate 1 sample 00084 // uniformly in for each "stratum" u_j ~ U(j-1/N,j/N) 00085 // Variance reduction! 00086 /* 00087 @Article{ kitagawa96, 00088 author = {Kitagawa, G.}, 00089 title = {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models }, 00090 journal = {Journal of Computational and Graphical Statistics}, 00091 year = {1996}, 00092 volume = {5}, 00093 number = {1}, 00094 pages = {1--25}, 00095 annote = {describes deterministic and stratified resampling} 00096 } 00097 */ 00098 00099 #define RESIDUAL_RS 3 00100 // sample "deterministically" the integer part of N \times w_j , the 00101 // perform multinomial sampling for the resulting N - \sum [N \times 00102 // w_j] where [] denotes the integer part. 00103 /* 00104 See eg. p.19 00105 @Article{ liuchen98, 00106 author = {Liu, J. S. and Chen, R.}, 00107 title = {Sequential {M}onte {C}arlo methods for dynamic systems}, 00108 journal = {Journal of the American Statistical Association}, 00109 year = {1998}, 00110 volume = {93}, 00111 pages = {1032--1044}} 00112 */ 00113 00114 #define MINIMUM_VARIANCE_RS 4 00115 /* 00116 See eg. 00117 @TechReport{ carpenter99, 00118 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.}, 00119 title = {Building robust simulation-based filters for evolving data sets}, 00120 institution = {Department of Statistics, University of Oxford}, 00121 year = {99}, 00122 note = {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}} 00123 */ 00124 00125 namespace BFL 00126 { 00127 00129 00153 template <typename StateVar, typename MeasVar> class MixtureParticleFilter 00154 : public Filter<StateVar,MeasVar> 00155 { 00156 protected: 00157 virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel, 00158 const StateVar& u, 00159 MeasurementModel<MeasVar,StateVar>* const measmodel, 00160 const MeasVar& z, 00161 const StateVar& s); 00162 00164 00168 ConditionalPdf<StateVar,StateVar> * _proposal; 00169 00171 WeightedSample<StateVar> _sample; 00173 vector<vector<WeightedSample<StateVar> > > _old_samplesVec; 00175 vector<vector<WeightedSample<StateVar> > > _new_samplesVec; 00177 vector< vector<Sample<StateVar> > > _new_samples_unweightedVec; 00179 typename vector<WeightedSample<StateVar> >::iterator _os_it; 00181 typename vector<WeightedSample<StateVar> >::iterator _ns_it; 00183 vector<Probability> _newMixtureWeights; 00185 vector<Probability> _sumWeights; 00186 00188 00191 int _resamplePeriod; 00192 00194 double _resampleThreshold; 00195 00197 int _resampleScheme; 00198 00200 bool _dynamicResampling; 00201 00203 bool _proposal_depends_on_meas; 00204 00206 bool _created_post; 00207 00209 //Mixture Pdf. 00213 int _maintainMixturePeriod; 00214 00216 00224 virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel, 00225 const StateVar & u, 00226 MeasurementModel<MeasVar,StateVar> * const measmodel, 00227 const MeasVar & z, 00228 const StateVar & s); 00229 00231 00240 virtual bool ProposalStepInternalOne(int component, SystemModel<StateVar> * const sysmodel, 00241 const StateVar & u, 00242 MeasurementModel<MeasVar,StateVar> * const measmodel, 00243 const MeasVar & z, 00244 const StateVar & s); 00245 00247 00253 virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel, 00254 const StateVar & u, 00255 MeasurementModel<MeasVar,StateVar> * const measmodel, 00256 const MeasVar & z, 00257 const StateVar & s); 00258 00260 00267 virtual bool UpdateWeightsInternalOne(int component, SystemModel<StateVar> * const sysmodel, 00268 const StateVar & u, 00269 MeasurementModel<MeasVar,StateVar> * const measmodel, 00270 const MeasVar & z, 00271 const StateVar & s); 00272 00274 virtual bool DynamicResampleStep(); 00275 00277 00280 virtual bool DynamicResampleStepOne(int component); 00281 00283 00285 virtual bool StaticResampleStep(); 00286 00288 virtual bool Resample(); 00289 00291 00293 virtual bool ResampleOne(int component); 00294 00296 00298 virtual bool MaintainMixtureStep(); 00299 00301 virtual bool MaintainMixture(); 00302 00303 00304 public: 00306 00318 MixtureParticleFilter(Mixture<StateVar> * prior, 00319 ConditionalPdf<StateVar,StateVar> * proposal, 00320 int resampleperiod = 0, 00321 double resamplethreshold = 0, 00322 int resamplescheme = DEFAULT_RS, 00323 int maintainMixturePeriod = 1 ); 00324 00325 00327 00340 MixtureParticleFilter(Mixture<StateVar> * prior, 00341 Mixture<StateVar> * post, 00342 ConditionalPdf<StateVar,StateVar> * proposal, 00343 int resampleperiod = 0, 00344 double resamplethreshold = 0, 00345 int resamplescheme = DEFAULT_RS, 00346 int maintainMixturePeriod = 1 ); 00347 00349 virtual ~MixtureParticleFilter(); 00351 00353 MixtureParticleFilter(const MixtureParticleFilter<StateVar,MeasVar> & filt); 00354 00356 virtual void Reset(Mixture<StateVar> * prior); 00357 00359 00364 virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf); 00365 00367 00369 ConditionalPdf<StateVar,StateVar> * ProposalGet(); 00370 00371 // implement virtual function 00372 virtual Mixture<StateVar> * PostGet(); 00373 }; 00374 00375 #include "mixtureParticleFilter.cpp" 00376 00377 } // End namespace BFL 00378 00379 #endif // __MIXTURE_PARTICLE_FILTER__