Bayesian Filtering Library Generated from SVN r
particlefilter.h
1// $Id$
2
3 /***************************************************************************
4 * This library is free software; you can redistribute it and/or *
5 * modify it under the terms of the GNU General Public *
6 * License as published by the Free Software Foundation; *
7 * version 2 of the License. *
8 * *
9 * As a special exception, you may use this file as part of a free *
10 * software library without restriction. Specifically, if other files *
11 * instantiate templates or use macros or inline functions from this *
12 * file, or you compile this file and link it with other files to *
13 * produce an executable, this file does not by itself cause the *
14 * resulting executable to be covered by the GNU General Public *
15 * License. This exception does not however invalidate any other *
16 * reasons why the executable file might be covered by the GNU General *
17 * Public License. *
18 * *
19 * This library is distributed in the hope that it will be useful, *
20 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
21 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
22 * Lesser General Public License for more details. *
23 * *
24 * You should have received a copy of the GNU General Public *
25 * License along with this library; if not, write to the Free Software *
26 * Foundation, Inc., 51 Franklin Street, Fifth Floor, *
27 * Boston, MA 02110-1301 USA *
28 * *
29 ***************************************************************************/
30
31#ifndef __PARTICLE_FILTER__
32#define __PARTICLE_FILTER__
33
34#include "filter.h"
35#include "../pdf/conditionalpdf.h"
36#include "../pdf/mcpdf.h"
37
38// RS stands for Resample Scheme
39// TODO: Work this better out
40#define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
41#define MULTINOMIAL_RS 0
42/* Carpenter, Clifford and Fearnhead:
43 Efficient implementation of particle
44 filters for non-linear systems.
46 See eg.
47
48 @Article{ gordon93,
49 author = {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.},
50 title = {Novel approach to nonlinear/non-Gaussian state estimation},
51 journal = {IEE Proceedings-F},
52 year = {1993},
53 volume = {140},
54 number = {2},
55 pages = {107--113},
56 annote = {Multinomial Sampling}}
57*/
59#define SYSTEMATIC_RS 1
60// A lot of possible systematic approaches to resampling are described
61// in literature. The goal is to reduce the Monte Carlo Variance of
62// Particle filters. One example is
63/*
64@Article{ carpenter99-improved,
65 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.},
66 title = {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems},
67 journal = {Radar, Sonar and Navigation, IEE Proceedings -},
68 year = {1999},
69 volume = {146},
70 number = {1},
71 pages = {2--7},
72 month = {February},
73 annote = {Describes systematic resampling, variance reduction}
74}
75*/
77// KG TODO Check if systematic resampling = deterministic resampling
78// as described by kitagawa
79
80#define STRATIFIED_RS 2
81// Generate N samples not uniformly on [0,1] but generate 1 sample
82// uniformly in for each "stratum" u_j ~ U(j-1/N,j/N)
83// Variance reduction!
84/*
85 @Article{ kitagawa96,
86 author = {Kitagawa, G.},
87 title = {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models },
88 journal = {Journal of Computational and Graphical Statistics},
89 year = {1996},
90 volume = {5},
91 number = {1},
92 pages = {1--25},
93 annote = {describes deterministic and stratified resampling}
94 }
95*/
97#define RESIDUAL_RS 3
98// sample "deterministically" the integer part of N \times w_j , the
99// perform multinomial sampling for the resulting N - \sum [N \times
100// w_j] where [] denotes the integer part.
102 See eg. p.19
103 @Article{ liuchen98,
104 author = {Liu, J. S. and Chen, R.},
105 title = {Sequential {M}onte {C}arlo methods for dynamic systems},
106 journal = {Journal of the American Statistical Association},
107 year = {1998},
108 volume = {93},
109 pages = {1032--1044}}
110*/
112#define MINIMUM_VARIANCE_RS 4
114 See eg.
115 @TechReport{ carpenter99,
116 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.},
117 title = {Building robust simulation-based filters for evolving data sets},
118 institution = {Department of Statistics, University of Oxford},
119 year = {99},
120 note = {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}}
123namespace BFL
125
159 template <typename StateVar, typename MeasVar> class ParticleFilter
160 : public Filter<StateVar,MeasVar>
161 {
162 protected:
163 virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
164 const StateVar& u,
166 const MeasVar& z,
167 const StateVar& s);
170
175
177 WeightedSample<StateVar> _sample;
179 vector<WeightedSample<StateVar> > _old_samples;
181 vector<WeightedSample<StateVar> > _new_samples;
183 vector<Sample<StateVar> > _new_samples_unweighted;
185 typename vector<WeightedSample<StateVar> >::iterator _os_it;
187 typename vector<WeightedSample<StateVar> >::iterator _ns_it;
188
203
206
209
211
219 virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
220 const StateVar & u,
221 MeasurementModel<MeasVar,StateVar> * const measmodel,
222 const MeasVar & z,
223 const StateVar & s);
224
226
232 virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
233 const StateVar & u,
235 const MeasVar & z,
236 const StateVar & s);
237
239
241 virtual bool DynamicResampleStep();
242
246 virtual bool StaticResampleStep();
249 virtual bool Resample();
251 public:
264 ParticleFilter(MCPdf<StateVar> * prior,
266 int resampleperiod = 0,
267 double resamplethreshold = 0,
268 int resamplescheme = DEFAULT_RS);
269
284 ParticleFilter(MCPdf<StateVar> * prior,
285 MCPdf<StateVar> * post,
287 int resampleperiod = 0,
288 double resamplethreshold = 0,
289 int resamplescheme = DEFAULT_RS);
290
294
297
299
305
307
310
311 // implement virtual function
312 virtual MCPdf<StateVar> * PostGet();
313 };
314
315#include "particlefilter.cpp"
316
317} // End namespace BFL
319#endif // __PARTICLE_FILTER__
Abstract Class representing conditional Pdfs P(x | ...)
Abstract class representing an interface for Bayesian Filters.
Definition filter.h:78
Virtual Class representing all particle filters.
virtual bool Resample()
Actual Resampling happens here;.
ConditionalPdf< StateVar, StateVar > * _proposal
Pointer to the Proposal Density.
ParticleFilter(MCPdf< StateVar > *prior, MCPdf< StateVar > *post, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS)
Constructor.
vector< WeightedSample< StateVar > > _old_samples
While updating store list of old samples.
bool _dynamicResampling
Dynamic resampling or fixed period resampling?
double _resampleThreshold
Threshold used when dynamic resampling.
ParticleFilter(const ParticleFilter< StateVar, MeasVar > &filt)
Copy Constructor.
virtual bool UpdateInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Actual implementation of Update, varies along filters.
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
vector< WeightedSample< StateVar > > _new_samples
While updating store list of new samples.
virtual MCPdf< StateVar > * PostGet()
Get Posterior density.
int _resamplePeriod
Number of timestep between resampling from the Posterior Pdf.
virtual bool ProposalStepInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Proposal step.
virtual void ProposalSet(ConditionalPdf< StateVar, StateVar > *const cpdf)
Set the proposal density.
vector< Sample< StateVar > > _new_samples_unweighted
While resampling.
virtual bool DynamicResampleStep()
Resample if necessary.
ConditionalPdf< StateVar, StateVar > * ProposalGet()
Get a pointer to the proposal density.
virtual bool UpdateWeightsInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Update Weights.
virtual bool StaticResampleStep()
Resample if wanted.
ParticleFilter(MCPdf< StateVar > *prior, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS)
Constructor.
bool _proposal_depends_on_meas
Proposal depends on last measurement?
int _resampleScheme
Which resample algorithm (see top of particle.h for #defines)
vector< WeightedSample< StateVar > >::iterator _os_it
Iterator for old list of samples.
virtual ~ParticleFilter()
Destructor.
WeightedSample< StateVar > _sample
While updating use sample<StateVar>
bool _created_post
created own post