particlefilter.cpp
Go to the documentation of this file.
1 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
2 //
3 // This program is free software; you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation; either version 2.1 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16 //
17 // $Id$
18 
19 #include "particlefilter.h"
20 #include "../pdf/mcpdf.h"
21 
22 #define SV StateVar
23 #define MV MeasVar
24 #define MeasModel MeasurementModel
25 
26 #define STATE_AND_MEAS_VAR_DIFFERENT
27 
28 template <typename SV, typename MV>
29 ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
30  ConditionalPdf<SV,SV> * proposal,
31  int resampleperiod,
32  double resamplethreshold,
33  int resamplescheme)
34  : Filter<SV,MV>(prior)
35  , _proposal(proposal)
36  , _sample(WeightedSample<SV>(prior->DimensionGet()))
37  , _resampleScheme(resamplescheme)
38  , _created_post(true)
39 {
40  /* Initialize Post, at time = 0, post = prior
41  To be more clean, this should be done in the filter base class,
42  but this is impossible because of the pure virtuals...
43  */
44  this->_post = new MCPdf<SV>(prior->NumSamplesGet(),prior->DimensionGet());
45  // Post is equal to prior at timetep 1
46  /* Note: Dirty cast should be avoided by not demanding an MCPdf as
47  prior and just sample from the prior instead :-(
48  */
49  bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
50  assert(ret);
51 
52  // Initialise lists of samples
53  _old_samples = (prior->ListOfSamplesGet());
55 
56 
57  // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
58  assert(!(resampleperiod == 0 && resamplethreshold == 0));
59  assert(!(resampleperiod != 0 && resamplethreshold != 0));
60 
61  // dynamic resampling
62  if (resampleperiod == 0)
63  _dynamicResampling = true;
64  // fixed period resampling
65  else
66  _dynamicResampling = false;
67  _resamplePeriod = resampleperiod;
68  _resampleThreshold = resamplethreshold;
69 }
70 
71 
72 
73 template <typename SV, typename MV>
75  MCPdf<SV> * post,
76  ConditionalPdf<SV,SV> * proposal,
77  int resampleperiod,
78  double resamplethreshold,
79  int resamplescheme)
80  : Filter<SV,MV>(prior),
81  _proposal(proposal),
82  _resampleScheme(resamplescheme),
83  _created_post(false)
84 {
85  this->_post = post;
86  // Post is equal to prior at timestep 1
87  /* Note: Dirty cast should be avoided by not demanding an MCPdf as
88  prior and just sample from the prior instead :-(
89  */
90  bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
91  assert(ret);
92 
93  // Initialise lists of samples
94  _old_samples = (prior->ListOfSamplesGet());
96 
97  // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
98  assert(!(resampleperiod == 0 && resamplethreshold == 0));
99  assert(!(resampleperiod != 0 && resamplethreshold != 0));
100 
101  // dynamic resampling
102  if (resampleperiod == 0)
103  _dynamicResampling = true;
104  // fixed period resampling
105  else
106  _dynamicResampling = false;
107 
108  _resamplePeriod = resampleperiod;
109  _resampleThreshold = resamplethreshold;
110 }
111 
112 
113 
114 
115 template <typename SV, typename MV>
117 {
118  if (_created_post)
119  delete this->_post;
120 }
121 
122 template <typename SV, typename MV>
124  : Filter<SV,MV>(filter),
125  _created_post(true)
126 {
127  // Copy constructor of MCPdf
128  // Probably a bug...
129  this->_post = new MCPdf<SV>(dynamic_cast<MCPdf<SV> *>(filter._post));
130 }
131 
132 template <typename SV, typename MV> void
134 {
135  _proposal = cpdf;
136 }
137 
138 template <typename SV, typename MV> ConditionalPdf<SV,SV> *
140 {
141  return _proposal;
142 }
143 
144 template <typename SV, typename MV> bool
146  const SV & u,
147  MeasurementModel<MV,SV> * const measmodel,
148  const MV & z,
149  const SV & s)
150 {
151  // Get all samples from the current post through proposal density
152  _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
153 
154  _ns_it = _new_samples.begin();
155  for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
156  {
157  const SV& x_old = _os_it->ValueGet();
159 
160  if (!sysmodel->SystemWithoutInputs())
161  {
163  if (this->_proposal_depends_on_meas)
164  {
165  #ifndef STATE_AND_MEAS_VAR_DIFFERENT
167  if (!measmodel->SystemWithoutSensorParams())
169  #endif
170  }
171 
172  }
173  else // System without inputs
174  {
175  if (this->_proposal_depends_on_meas)
176  {
177  #ifndef STATE_AND_MEAS_VAR_DIFFERENT
179  if (!measmodel->SystemWithoutSensorParams())
181  #endif
182 
183  }
184  }
185  // Bug, make sampling method a parameter!
187  _ns_it->ValueSet(_sample.ValueGet());
188  _ns_it->WeightSet(_os_it->WeightGet());
189  _ns_it++;
190  }
191 
192  (this->_timestep)++;
193 
194  // Update the list of samples
195  return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
196 
197 }
198 
199 
200 template <typename SV, typename MV> bool
202  const SV & u,
203  MeasurementModel<MV,SV> * const measmodel,
204  const MV & z,
205  const SV & s)
206 {
207  Probability weightfactor = 1;
208 
209  // Update the weights
210  // Same remarks as for the system update!
211  _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
212  _os_it = _old_samples.begin();
213 
214  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
215  {
216  const SV& x_new = _ns_it->ValueGet();
217  const SV& x_old = _os_it->ValueGet();
218 
219  if (sysmodel == NULL)
220  {
221  if (measmodel->SystemWithoutSensorParams() == true)
222  weightfactor = measmodel->ProbabilityGet(z,x_new);
223  else
224  weightfactor = measmodel->ProbabilityGet(z,x_new,s);
225  }
226  else // We do have a system model
227  {
229  if (measmodel->SystemWithoutSensorParams() == true)
230  {
231  weightfactor = measmodel->ProbabilityGet(z,x_new);
232  if (sysmodel->SystemWithoutInputs() == false)
233  {
235  if (this->_proposal_depends_on_meas){
236  #ifndef STATE_AND_MEAS_VAR_DIFFERENT
238  #endif
239  }
240 
241  if (_proposal->ProbabilityGet(x_new) != 0)
242  weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,x_old,u) / _proposal->ProbabilityGet(x_new) );
243  else weightfactor = 0;
244  }
245  else // we do have a system without inputs
246  {
247  if (this->_proposal_depends_on_meas){
248  #ifndef STATE_AND_MEAS_VAR_DIFFERENT
250  #endif
251  }
252  if ( _proposal->ProbabilityGet(x_new) != 0)
253  weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,_os_it->ValueGet()) / _proposal->ProbabilityGet(x_new) );
254  else weightfactor = 0;
255  }
256  }
257  else // System with sensor Parameters
258  {
259  weightfactor = measmodel->ProbabilityGet(z,x_new,s);
260  }
261  }
262  _ns_it->WeightSet(_ns_it->WeightGet() * weightfactor);
263 
264  _os_it++;
265  }
266  // Update the sample list of post the SumofWeights of the pdf
267  return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
268 
269 }
270 
271 template <typename SV, typename MV> bool
273 {
274  // Resampling?
275  bool resampling = false;
276  double sum_sq_weigths = 0.0;
277 
278  // Resampling if necessary
279  if ( this->_dynamicResampling)
280  {
281  // Check if sum of 1 / \sum{(wi_normalised)^2} < threshold
282  // This is the criterion proposed by Liu
283  // BUG foresee other methods of approximation/calculating
284  // effective sample size. Let the user implement this in !
285  _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
286  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
287  {
288  sum_sq_weigths += pow(_ns_it->WeightGet(),2);
289  }
290  if ((1.0 / sum_sq_weigths) < _resampleThreshold)
291  {
292  // #define __RESAMPLE_DEBUG__
293 #ifdef __RESAMPLE_DEBUG__
294  cout << "resampling now: " << this->_timestep
295  << "\tN_eff: " << (1.0 / sum_sq_weigths) << endl;
296 #endif // __RESAMPLE_DEBUG__
297  resampling = true;
298  }
299  }
300  if (resampling == true)
301  return this->Resample();
302  else
303  return true;
304 }
305 
306 
307 template <typename SV, typename MV> bool
309 {
310  // Resampling if necessary
311  if ( (!this->_dynamicResampling) && (((this->_timestep) % _resamplePeriod) == 0) && (this->_timestep != 0))
312  return this->Resample();
313  return true;
314 }
315 
316 
317 template <typename SV, typename MV> bool
319  const StateVar& u,
320  MeasurementModel<MeasVar,StateVar>* const measmodel,
321  const MeasVar& z,
322  const StateVar& s)
323 {
324  bool result = true;
325 
326  // Only makes sense if there is a system model?
327  // Bug, not completely true, but should do for now...
328  if (sysmodel != NULL)
329  {
330  result = result && this->StaticResampleStep();
331  result = result && this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
332 
333  }
334  // Updating the weights only makes sense using a measurement model
335  if (measmodel != NULL)
336  {
337  result = result && this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s);
338  result = result && this->DynamicResampleStep();
339  }
340 
341  return result;
342 }
343 
344 template <typename SV, typename MV> bool
346 {
347  int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
348  // #define __PARTICLEFILTER_DEBUG__
349 #ifdef __PARTICLEFILTER_DEBUG__
350  cout << "PARTICLEFILTER: resampling now" << endl;
351  _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
352  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
353  {
354  cout << "PF: Old samples:\n";
355  cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
356  }
357 #endif // __PARTICLEFILTER_DEBUG
358  switch(_resampleScheme)
359  {
360  case MULTINOMIAL_RS:
361  {
362  (dynamic_cast<MCPdf<SV> *>(this->_post))->SampleFrom(_new_samples_unweighted, NumSamples,RIPLEY,NULL);
363  break;
364  }
365  case SYSTEMATIC_RS:{break;}
366  case STRATIFIED_RS:{break;}
367  case RESIDUAL_RS:{break;}
368  default:
369  {
370  cerr << "Sampling method not supported" << endl;
371  break;
372  }
373  }
374  bool result = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples_unweighted);
375 #ifdef __PARTICLEFILTER_DEBUG__
376  cout << "PARTICLEFILTER: after resampling" << endl;
377  _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
378  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
379  {
380  cout << "PF: New samples:\n";
381  cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
382  }
383 #endif // __PARTICLEFILTER_DEBUG
384 
385  return result;
386 }
387 
388 
389 template<typename SV, typename MV> MCPdf<SV> *
391 {
393 }
#define STRATIFIED_RS
#define RESIDUAL_RS
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:77
bool SystemWithoutInputs() const
Has the system inputs or not.
Definition: systemmodel.cpp:84
#define MULTINOMIAL_RS
virtual MCPdf< StateVar > * PostGet()
Get Posterior density.
ParticleFilter(MCPdf< StateVar > *prior, ConditionalPdf< StateVar, StateVar > *proposal, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS)
Constructor.
#define DEFAULT
double _resampleThreshold
Threshold used when dynamic resampling.
Filter(Pdf< StateVar > *prior)
Constructor.
Definition: filter.h:27
Pdf< StateVar > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
Virtual Class representing all particle filters.
ConditionalPdf< StateVar, StateVar > * ProposalGet()
Get a pointer to the proposal density.
virtual bool DynamicResampleStep()
Resample if necessary.
virtual void ConditionalArgumentSet(unsigned int n_argument, const CondArg &argument)
Set the n-th argument of the list.
#define StateVar
Definition: asirfilter.h:22
WeightedSample< StateVar > _sample
While updating use sample<StateVar>
bool _proposal_depends_on_meas
Proposal depends on last measurement?
vector< WeightedSample< StateVar > >::iterator _os_it
Iterator for old list of samples.
virtual Pdf< StateVar > * PostGet()
Get Posterior density.
Definition: filter.cpp:126
Probability ProbabilityGet(const MeasVar &z, const StateVar &x, const StateVar &s)
Get the probability of a certain measurement.
virtual bool Resample()
Actual Resampling happens here;.
int _resampleScheme
Which resample algorithm (see top of particle.h for #defines)
Abstract Class representing conditional Pdfs P(x | ...)
#define MV
vector< WeightedSample< StateVar > > _old_samples
While updating store list of old samples.
int _timestep
Represents the current timestep of the filter.
Definition: filter.h:100
vector< WeightedSample< StateVar > > _new_samples
While updating store list of new samples.
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.
#define SYSTEMATIC_RS
virtual Probability ProbabilityGet(const T &input) const
Get the probability of a certain argument.
Monte Carlo Pdf: Sample based implementation of Pdf.
Definition: mcpdf.h:49
virtual void ProposalSet(ConditionalPdf< StateVar, StateVar > *const cpdf)
Set the proposal density.
virtual bool ProposalStepInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Proposal step.
#define SV
virtual bool StaticResampleStep()
Resample if wanted.
#define RIPLEY
#define MeasVar
Definition: asirfilter.h:23
virtual ~ParticleFilter()
Destructor.
bool _created_post
created own post
Probability ProbabilityGet(const T &x_k, const T &x_kminusone, const T &u)
Get the probability of arriving in a next state.
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
bool _dynamicResampling
Dynamic resampling or fixed period 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< Sample< StateVar > > _new_samples_unweighted
While resampling.
int _resamplePeriod
Number of timestep between resampling from the Posterior Pdf.
ConditionalPdf< StateVar, StateVar > * _proposal
Pointer to the Proposal Density.
Class representing a probability (a double between 0 and 1)
Definition: bfl_constants.h:39
virtual bool SampleFrom(vector< Sample< T > > &list_samples, const unsigned int num_samples, int method=DEFAULT, void *args=NULL) const
Draw multiple samples from the Pdf (overloaded)
T & ValueGet()
Get the value of the Sample.
Definition: asirfilter.h:132
bool SystemWithoutSensorParams() const
Number of Conditional Arguments.


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