Bayesian Filtering Library Generated from SVN r
|
00001 // $Id: particlefilter.h 29830 2009-01-14 15:10:41Z kgadeyne $ 00002 00003 /*************************************************************************** 00004 * This library is free software; you can redistribute it and/or * 00005 * modify it under the terms of the GNU General Public * 00006 * License as published by the Free Software Foundation; * 00007 * version 2 of the License. * 00008 * * 00009 * As a special exception, you may use this file as part of a free * 00010 * software library without restriction. Specifically, if other files * 00011 * instantiate templates or use macros or inline functions from this * 00012 * file, or you compile this file and link it with other files to * 00013 * produce an executable, this file does not by itself cause the * 00014 * resulting executable to be covered by the GNU General Public * 00015 * License. This exception does not however invalidate any other * 00016 * reasons why the executable file might be covered by the GNU General * 00017 * Public License. * 00018 * * 00019 * This library is distributed in the hope that it will be useful, * 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 00022 * Lesser General Public License for more details. * 00023 * * 00024 * You should have received a copy of the GNU General Public * 00025 * License along with this library; if not, write to the Free Software * 00026 * Foundation, Inc., 59 Temple Place, * 00027 * Suite 330, Boston, MA 02111-1307 USA * 00028 * * 00029 ***************************************************************************/ 00030 00031 #ifndef __PARTICLE_FILTER__ 00032 #define __PARTICLE_FILTER__ 00033 00034 #include "filter.h" 00035 #include "../pdf/conditionalpdf.h" 00036 #include "../pdf/mcpdf.h" 00037 00038 // RS stands for Resample Scheme 00039 // TODO: Work this better out 00040 #define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL 00041 #define MULTINOMIAL_RS 0 00042 /* Carpenter, Clifford and Fearnhead: 00043 Efficient implementation of particle 00044 filters for non-linear systems. 00045 00046 See eg. 00047 00048 @Article{ gordon93, 00049 author = {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.}, 00050 title = {Novel approach to nonlinear/non-Gaussian state estimation}, 00051 journal = {IEE Proceedings-F}, 00052 year = {1993}, 00053 volume = {140}, 00054 number = {2}, 00055 pages = {107--113}, 00056 annote = {Multinomial Sampling}} 00057 */ 00058 00059 #define SYSTEMATIC_RS 1 00060 // A lot of possible systematic approaches to resampling are described 00061 // in literature. The goal is to reduce the Monte Carlo Variance of 00062 // Particle filters. One example is 00063 /* 00064 @Article{ carpenter99-improved, 00065 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.}, 00066 title = {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems}, 00067 journal = {Radar, Sonar and Navigation, IEE Proceedings -}, 00068 year = {1999}, 00069 volume = {146}, 00070 number = {1}, 00071 pages = {2--7}, 00072 month = {February}, 00073 annote = {Describes systematic resampling, variance reduction} 00074 } 00075 */ 00076 00077 // KG TODO Check if systematic resampling = deterministic resampling 00078 // as described by kitagawa 00079 00080 #define STRATIFIED_RS 2 00081 // Generate N samples not uniformly on [0,1] but generate 1 sample 00082 // uniformly in for each "stratum" u_j ~ U(j-1/N,j/N) 00083 // Variance reduction! 00084 /* 00085 @Article{ kitagawa96, 00086 author = {Kitagawa, G.}, 00087 title = {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models }, 00088 journal = {Journal of Computational and Graphical Statistics}, 00089 year = {1996}, 00090 volume = {5}, 00091 number = {1}, 00092 pages = {1--25}, 00093 annote = {describes deterministic and stratified resampling} 00094 } 00095 */ 00096 00097 #define RESIDUAL_RS 3 00098 // sample "deterministically" the integer part of N \times w_j , the 00099 // perform multinomial sampling for the resulting N - \sum [N \times 00100 // w_j] where [] denotes the integer part. 00101 /* 00102 See eg. p.19 00103 @Article{ liuchen98, 00104 author = {Liu, J. S. and Chen, R.}, 00105 title = {Sequential {M}onte {C}arlo methods for dynamic systems}, 00106 journal = {Journal of the American Statistical Association}, 00107 year = {1998}, 00108 volume = {93}, 00109 pages = {1032--1044}} 00110 */ 00111 00112 #define MINIMUM_VARIANCE_RS 4 00113 /* 00114 See eg. 00115 @TechReport{ carpenter99, 00116 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.}, 00117 title = {Building robust simulation-based filters for evolving data sets}, 00118 institution = {Department of Statistics, University of Oxford}, 00119 year = {99}, 00120 note = {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}} 00121 */ 00122 00123 namespace BFL 00124 { 00125 00127 00159 template <typename StateVar, typename MeasVar> class ParticleFilter 00160 : public Filter<StateVar,MeasVar> 00161 { 00162 protected: 00163 virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel, 00164 const StateVar& u, 00165 MeasurementModel<MeasVar,StateVar>* const measmodel, 00166 const MeasVar& z, 00167 const StateVar& s); 00168 00170 00174 ConditionalPdf<StateVar,StateVar> * _proposal; 00175 00177 WeightedSample<StateVar> _sample; 00179 vector<WeightedSample<StateVar> > _old_samples; 00181 vector<WeightedSample<StateVar> > _new_samples; 00183 vector<Sample<StateVar> > _new_samples_unweighted; 00185 typename vector<WeightedSample<StateVar> >::iterator _os_it; 00187 typename vector<WeightedSample<StateVar> >::iterator _ns_it; 00188 00190 00193 int _resamplePeriod; 00194 00196 double _resampleThreshold; 00197 00199 int _resampleScheme; 00200 00202 bool _dynamicResampling; 00203 00205 bool _proposal_depends_on_meas; 00206 00208 bool _created_post; 00209 00211 00219 virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel, 00220 const StateVar & u, 00221 MeasurementModel<MeasVar,StateVar> * const measmodel, 00222 const MeasVar & z, 00223 const StateVar & s); 00224 00226 00232 virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel, 00233 const StateVar & u, 00234 MeasurementModel<MeasVar,StateVar> * const measmodel, 00235 const MeasVar & z, 00236 const StateVar & s); 00237 00239 00241 virtual bool DynamicResampleStep(); 00242 00244 00246 virtual bool StaticResampleStep(); 00247 00249 virtual bool Resample(); 00250 00251 public: 00253 00264 ParticleFilter(MCPdf<StateVar> * prior, 00265 ConditionalPdf<StateVar,StateVar> * proposal, 00266 int resampleperiod = 0, 00267 double resamplethreshold = 0, 00268 int resamplescheme = DEFAULT_RS); 00269 00270 00272 00284 ParticleFilter(MCPdf<StateVar> * prior, 00285 MCPdf<StateVar> * post, 00286 ConditionalPdf<StateVar,StateVar> * proposal, 00287 int resampleperiod = 0, 00288 double resamplethreshold = 0, 00289 int resamplescheme = DEFAULT_RS); 00290 00292 virtual ~ParticleFilter(); 00294 00296 ParticleFilter(const ParticleFilter<StateVar,MeasVar> & filt); 00297 00299 00304 virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf); 00305 00307 00309 ConditionalPdf<StateVar,StateVar> * ProposalGet(); 00310 00311 // implement virtual function 00312 virtual MCPdf<StateVar> * PostGet(); 00313 }; 00314 00315 #include "particlefilter.cpp" 00316 00317 } // End namespace BFL 00318 00319 #endif // __PARTICLE_FILTER__