mixtureParticleFilter.h
Go to the documentation of this file.
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., 59 Temple Place, *
28  * Suite 330, Boston, MA 02111-1307 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 
125 namespace 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 
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 
209  //Mixture Pdf.
214 
216 
224  virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
225  const StateVar & u,
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 
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();
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 
349  virtual ~MixtureParticleFilter();
351 
354 
356  virtual void Reset(Mixture<StateVar> * prior);
357 
359 
364  virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf);
365 
367 
370 
371  // implement virtual function
372  virtual Mixture<StateVar> * PostGet();
373  };
374 
375 #include "mixtureParticleFilter.cpp"
376 
377 } // End namespace BFL
379 #endif // __MIXTURE_PARTICLE_FILTER__
virtual ~MixtureParticleFilter()
Destructor.
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:77
vector< vector< WeightedSample< StateVar > > > _new_samplesVec
While updating store list of new samples.
Virtual Class representing all Mixture particle filters.
double _resampleThreshold
Threshold used when dynamic resampling.
virtual bool ProposalStepInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Proposal step.
vector< WeightedSample< StateVar > >::iterator _os_it
Iterator for old list of samples.
#define DEFAULT_RS
ConditionalPdf< StateVar, StateVar > * _proposal
Pointer to the Proposal Density.
virtual void Reset(Mixture< StateVar > *prior)
Reset Filter.
int _resampleScheme
Which resample algorithm (see top of particle.h for #defines)
int _maintainMixturePeriod
Number of timestep between mixture maintainance of the Posterior.
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.
WeightedSample< StateVar > _sample
While updating use sample<StateVar>
vector< vector< Sample< StateVar > > > _new_samples_unweightedVec
While resampling.
virtual Mixture< StateVar > * PostGet()
Get Posterior density.
ConditionalPdf< StateVar, StateVar > * ProposalGet()
Get a pointer to the proposal density.
MixtureParticleFilter(Mixture< StateVar > *prior, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS, int maintainMixturePeriod=1)
Constructor.
virtual bool UpdateWeightsInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Update Weights.
#define StateVar
Definition: asirfilter.h:22
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
virtual bool Resample()
Actual Resampling happens here;.
virtual bool MaintainMixture()
Actual mixture maintainance happens here;.
virtual void ProposalSet(ConditionalPdf< StateVar, StateVar > *const cpdf)
Set the proposal density.
virtual bool ResampleOne(int component)
Actual Resampling for one component;.
Class representing a mixture of PDFs, the mixture can contain different.
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.
virtual bool MaintainMixtureStep()
Maintain Mixture if wanted.
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.
#define MeasVar
Definition: asirfilter.h:23
virtual bool DynamicResampleStepOne(int component)
Resampling for one component.
virtual bool DynamicResampleStep()
Resample if necessary.
virtual bool StaticResampleStep()
Resample if wanted.
bool _proposal_depends_on_meas
Proposal depends on last measurement?
vector< Probability > _newMixtureWeights
Vector containing the new mixture weights during update step.
vector< Probability > _sumWeights
Vector containing the sum of weights during update step.
vector< vector< WeightedSample< StateVar > > > _old_samplesVec
While updating store list of old samples.
int _resamplePeriod
Number of timestep between resampling from the Posterior Pdf.
bool _created_post
created own post
bool _dynamicResampling
Dynamic resampling or fixed period resampling?


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 28 2022 21:56:33