asirfilter.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 "asirfilter.h"
20 #include "../sample/weightedsample.h"
21 
22 #define StateVar SVar
23 #define MeasVar MVar
24 
25 template <typename SVar, typename MVar>
27  int resampleperiod,
28  double resamplethreshold,
29  int resamplescheme)
30  : ParticleFilter<SVar,MVar>(prior,NULL,resampleperiod,
31  resamplethreshold, resamplescheme)
32 {
33  /* Well euhm, technically speaking it does, but we solve this
34  software-matically in another way (by reusing the
35  ProposalStepInternal from particlefilter.cpp). This is because
36  otherwise we would have to create a hybrid proposal density,
37  which requires far more programming.
38  */
39  this->_proposal_depends_on_meas = false;
40 }
41 
42 
43 template <typename SVar, typename MVar>
45 
46 
47 template <typename SVar, typename MVar> void
49  const SVar& u,
50  MeasurementModel<MVar,SVar>* const measmodel,
51  const MVar& z,
52  const SVar& s)
53 {
54  if (sysmodel != NULL)
55  {
56  this->ProposalSet(sysmodel->SystemPdfGet());
57  }
58 
59  /* The following code differs from standard SIR filter */
60  this->_old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
61  int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
62 
63  static SV x_old;
64  static vector<SV> mu(NumSamples);
65  typename vector<SV>::iterator mu_it = mu.begin();
66  static Probability weightfactor;
67  static Sample<SV> sample;
68 
69  // Only if there's a system model (for now, probably not general
70  // enough)
71  if (sysmodel != NULL)
72  {
73  // Step 1: Calculate beta values and adapt weights
74  for ( this->_os_it=this->_old_samples.begin();
75  this->_os_it != this->_old_samples.end() ;
76  this->_os_it++)
77  {
78  // Since the proposal is equal to the SystemPdf of the model
79  // here, simulating the systemmodel will do fine
80  x_old = this->_os_it->ValueGet();
81  this->_proposal->ConditionalArgumentSet(0,x_old);
82 
83  if (!sysmodel->SystemWithoutInputs())
84  {
86  if (this->_proposal_depends_on_meas)
87  {
89  if (!measmodel->SystemWithoutSensorParams())
91  }
92  }
93  else // System without inputs
94  {
95  if (this->_proposal_depends_on_meas)
96  {
98  if (!measmodel->SystemWithoutSensorParams())
100  }
101  }
102  // Get Expected Value of of this particles
103  *mu_it = this->_proposal->ExpectedValueGet();
104  // Calculate likelihood of this particle
105  if (!measmodel->SystemWithoutSensorParams())
106  weightfactor = measmodel->ProbabilityGet(z,*mu_it,s);
107  else
108  weightfactor = measmodel->ProbabilityGet(z,*mu_it);
109 
110  // Set new weight
111  this->_os_it->WeightSet(this->_os_it->WeightGet() * weightfactor);
112  mu_it++;
113  }
114  // Update list of samples
115  (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_old_samples);
116 
117  // Step 2: Sample discrete index k (in O (N) ops)
118  this->Resample();
119 
120  // Step 3: Proposal step
121  this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
122  }
123 
124  // Step 4: Update the weights
125  if (measmodel != NULL)
126  {
127  this->_new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
128 
129  static SV x_new;
130  mu_it=mu.begin();
131  for ( this->_ns_it=this->_new_samples.begin();
132  this->_ns_it != this->_new_samples.end() ;
133  this->_ns_it++)
134  {
135  x_new = this->_ns_it->ValueGet();
136  if (measmodel->SystemWithoutSensorParams() == true)
137  {
138  if (measmodel->ProbabilityGet(z,*mu_it) != 0)
139  weightfactor = measmodel->ProbabilityGet(z,x_new) / measmodel->ProbabilityGet(z,*mu_it);
140  else weightfactor = 0;
141  }
142  else // System with sensor Parameters
143  if (measmodel->ProbabilityGet(z,*mu_it,s) != 0)
144  weightfactor = measmodel->ProbabilityGet(z,x_new,s) / measmodel->ProbabilityGet(z,*mu_it,s);
145  else weightfactor = 0;
146  this->_ns_it->WeightSet(this->_ns_it->WeightGet() * weightfactor);
147  mu_it++;
148  }
149  // Update the sample list of post the SumofWeights of the pdf
150  (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_new_samples);
151 
152  // Resample
153  this->ResampleStep();
154  }
155 }
#define SV
bool SystemWithoutInputs() const
Has the system inputs or not.
Definition: systemmodel.cpp:84
virtual ~ASIRFilter()
Destructor.
Definition: asirfilter.cpp:44
ASIR: Auxiliary Particle Filter.
Definition: asirfilter.h:79
Pdf< StateVar > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
Virtual Class representing all particle filters.
virtual void ConditionalArgumentSet(unsigned int n_argument, const CondArg &argument)
Set the n-th argument of the list.
bool _proposal_depends_on_meas
Proposal depends on last measurement?
vector< WeightedSample< StateVar > >::iterator _os_it
Iterator for old list of samples.
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;.
vector< WeightedSample< StateVar > > _old_samples
While updating store list of old samples.
ConditionalPdf< T, T > * SystemPdfGet()
Get the SystemPDF.
Definition: systemmodel.cpp:91
vector< WeightedSample< StateVar > > _new_samples
While updating store list of new samples.
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.
virtual void UpdateInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Actual implementation of updateinternal.
Definition: asirfilter.cpp:48
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
ConditionalPdf< StateVar, StateVar > * _proposal
Pointer to the Proposal Density.
Class representing a probability (a double between 0 and 1)
Definition: bfl_constants.h:39
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