Bayesian Filtering Library Generated from SVN r
mixtureParticleFilter.h
1// Copyright (C) 2009 Tinne De Laet <first dot last at gmail dot com>
2// $Id: mixtureParticlefilter.h 2009-02-03 tdelaet $
3
4 /***************************************************************************
5 * This library is free software; you can redistribute it and/or *
6 * modify it under the terms of the GNU General Public *
7 * License as published by the Free Software Foundation; *
8 * version 2 of the License. *
9 * *
10 * As a special exception, you may use this file as part of a free *
11 * software library without restriction. Specifically, if other files *
12 * instantiate templates or use macros or inline functions from this *
13 * file, or you compile this file and link it with other files to *
14 * produce an executable, this file does not by itself cause the *
15 * resulting executable to be covered by the GNU General Public *
16 * License. This exception does not however invalidate any other *
17 * reasons why the executable file might be covered by the GNU General *
18 * Public License. *
19 * *
20 * This library is distributed in the hope that it will be useful, *
21 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
22 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
23 * Lesser General Public License for more details. *
24 * *
25 * You should have received a copy of the GNU General Public *
26 * License along with this library; if not, write to the Free Software *
27 * Foundation, Inc., 51 Franklin Street, Fifth Floor, *
28 * Boston, MA 02110-1301 USA *
29 * *
30 ***************************************************************************/
31
32#ifndef __MIXTURE_PARTICLE_FILTER__
33#define __MIXTURE_PARTICLE_FILTER__
34
35#include "filter.h"
36#include "../pdf/conditionalpdf.h"
37#include "../pdf/mcpdf.h"
38#include "../pdf/mixture.h"
39
40// RS stands for Resample Scheme
41// TODO: Work this better out
42#define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
43#define MULTINOMIAL_RS 0
44/* Carpenter, Clifford and Fearnhead:
45 Efficient implementation of particle
46 filters for non-linear systems.
47
48 See eg.
49
50 @Article{ gordon93,
51 author = {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.},
52 title = {Novel approach to nonlinear/non-Gaussian state estimation},
53 journal = {IEE Proceedings-F},
54 year = {1993},
55 volume = {140},
56 number = {2},
57 pages = {107--113},
58 annote = {Multinomial Sampling}}
59*/
60
61#define SYSTEMATIC_RS 1
62// A lot of possible systematic approaches to resampling are described
63// in literature. The goal is to reduce the Monte Carlo Variance of
64// Particle filters. One example is
65/*
66@Article{ carpenter99-improved,
67 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.},
68 title = {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems},
69 journal = {Radar, Sonar and Navigation, IEE Proceedings -},
70 year = {1999},
71 volume = {146},
72 number = {1},
73 pages = {2--7},
74 month = {February},
75 annote = {Describes systematic resampling, variance reduction}
76}
77*/
78
79// KG TODO Check if systematic resampling = deterministic resampling
80// as described by kitagawa
81
82#define STRATIFIED_RS 2
83// Generate N samples not uniformly on [0,1] but generate 1 sample
84// uniformly in for each "stratum" u_j ~ U(j-1/N,j/N)
85// Variance reduction!
86/*
87 @Article{ kitagawa96,
88 author = {Kitagawa, G.},
89 title = {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models },
90 journal = {Journal of Computational and Graphical Statistics},
91 year = {1996},
92 volume = {5},
93 number = {1},
94 pages = {1--25},
95 annote = {describes deterministic and stratified resampling}
96 }
97*/
98
99#define RESIDUAL_RS 3
100// sample "deterministically" the integer part of N \times w_j , the
101// perform multinomial sampling for the resulting N - \sum [N \times
102// w_j] where [] denotes the integer part.
103/*
104 See eg. p.19
105 @Article{ liuchen98,
106 author = {Liu, J. S. and Chen, R.},
107 title = {Sequential {M}onte {C}arlo methods for dynamic systems},
108 journal = {Journal of the American Statistical Association},
109 year = {1998},
110 volume = {93},
111 pages = {1032--1044}}
112*/
113
114#define MINIMUM_VARIANCE_RS 4
115/*
116 See eg.
117 @TechReport{ carpenter99,
118 author = {Carpenter, J. and Clifford, P. and Fearnhead, P.},
119 title = {Building robust simulation-based filters for evolving data sets},
120 institution = {Department of Statistics, University of Oxford},
121 year = {99},
122 note = {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}}
123*/
124
125namespace BFL
126{
127
129
153 template <typename StateVar, typename MeasVar> class MixtureParticleFilter
154 : public Filter<StateVar,MeasVar>
155 {
156 protected:
157 virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
158 const StateVar& u,
159 MeasurementModel<MeasVar,StateVar>* const measmodel,
160 const MeasVar& z,
161 const StateVar& s);
162
164
169
173 vector<vector<WeightedSample<StateVar> > > _old_samplesVec;
175 vector<vector<WeightedSample<StateVar> > > _new_samplesVec;
177 vector< vector<Sample<StateVar> > > _new_samples_unweightedVec;
179 typename vector<WeightedSample<StateVar> >::iterator _os_it;
181 typename vector<WeightedSample<StateVar> >::iterator _ns_it;
183 vector<Probability> _newMixtureWeights;
185 vector<Probability> _sumWeights;
186
188
192
195
198
201
204
207
209 //Mixture Pdf.
214
216
224 virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
225 const StateVar & u,
226 MeasurementModel<MeasVar,StateVar> * const measmodel,
227 const MeasVar & z,
228 const StateVar & s);
229
231
240 virtual bool ProposalStepInternalOne(int component, SystemModel<StateVar> * const sysmodel,
241 const StateVar & u,
242 MeasurementModel<MeasVar,StateVar> * const measmodel,
243 const MeasVar & z,
244 const StateVar & s);
245
247
253 virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
254 const StateVar & u,
255 MeasurementModel<MeasVar,StateVar> * const measmodel,
256 const MeasVar & z,
257 const StateVar & s);
258
260
267 virtual bool UpdateWeightsInternalOne(int component, SystemModel<StateVar> * const sysmodel,
268 const StateVar & u,
269 MeasurementModel<MeasVar,StateVar> * const measmodel,
270 const MeasVar & z,
271 const StateVar & s);
272
274 virtual bool DynamicResampleStep();
275
277
280 virtual bool DynamicResampleStepOne(int component);
281
283
285 virtual bool StaticResampleStep();
286
288 virtual bool Resample();
289
291
293 virtual bool ResampleOne(int component);
294
296
298 virtual bool MaintainMixtureStep();
299
301 virtual bool MaintainMixture();
302
303
304 public:
306
320 int resampleperiod = 0,
321 double resamplethreshold = 0,
322 int resamplescheme = DEFAULT_RS,
323 int maintainMixturePeriod = 1 );
324
325
327
341 Mixture<StateVar> * post,
343 int resampleperiod = 0,
344 double resamplethreshold = 0,
345 int resamplescheme = DEFAULT_RS,
346 int maintainMixturePeriod = 1 );
347
351
354
356 virtual void Reset(Mixture<StateVar> * prior);
357
359
365
367
370
371 // implement virtual function
373 };
374
375#include "mixtureParticleFilter.cpp"
376
377} // End namespace BFL
378
379#endif // __MIXTURE_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 Mixture particle filters.
virtual void Reset(Mixture< StateVar > *prior)
Reset Filter.
virtual bool UpdateWeightsInternalOne(int component, SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Update Weights for one component.
ConditionalPdf< StateVar, StateVar > * _proposal
Pointer to the Proposal Density.
virtual bool DynamicResampleStep()
Resample if necessary.
MixtureParticleFilter(Mixture< StateVar > *prior, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS, int maintainMixturePeriod=1)
Constructor.
vector< vector< WeightedSample< StateVar > > > _old_samplesVec
While updating store list of old samples.
MixtureParticleFilter(Mixture< StateVar > *prior, Mixture< StateVar > *post, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS, int maintainMixturePeriod=1)
Constructor.
virtual bool DynamicResampleStepOne(int component)
Resampling for one component.
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.
virtual bool StaticResampleStep()
Resample if wanted.
virtual Mixture< StateVar > * PostGet()
Get Posterior density.
bool _dynamicResampling
Dynamic resampling or fixed period resampling?
double _resampleThreshold
Threshold used when dynamic resampling.
virtual bool UpdateWeightsInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Update Weights.
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
virtual ~MixtureParticleFilter()
Destructor.
int _resamplePeriod
Number of timestep between resampling from the Posterior Pdf.
virtual bool ResampleOne(int component)
Actual Resampling for one component;.
int _maintainMixturePeriod
Number of timestep between mixture maintainance of the Posterior.
ConditionalPdf< StateVar, StateVar > * ProposalGet()
Get a pointer to the proposal density.
vector< vector< Sample< StateVar > > > _new_samples_unweightedVec
While resampling.
virtual void ProposalSet(ConditionalPdf< StateVar, StateVar > *const cpdf)
Set the proposal density.
vector< Probability > _sumWeights
Vector containing the sum of weights during update step.
virtual bool ProposalStepInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Proposal step.
virtual bool MaintainMixture()
Actual mixture maintainance happens here;.
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 bool Resample()
Actual Resampling happens here;.
vector< vector< WeightedSample< StateVar > > > _new_samplesVec
While updating store list of new samples.
MixtureParticleFilter(const MixtureParticleFilter< StateVar, MeasVar > &filt)
Copy Constructor.
virtual bool MaintainMixtureStep()
Maintain Mixture if wanted.
WeightedSample< StateVar > _sample
While updating use sample<StateVar>
bool _created_post
created own post
virtual bool ProposalStepInternalOne(int component, SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Proposal step for one component.
vector< Probability > _newMixtureWeights
Vector containing the new mixture weights during update step.
Class representing a mixture of PDFs, the mixture can contain different.
Definition: mixture.h:49