EKparticlefilter.cpp
Go to the documentation of this file.
1 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
2 // 2008 Tinne De Laet <first dot last at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18 
19 #include "EKparticlefilter.h"
20 
21 #define MeasModel MeasurementModel
22 #define CV ColumnVector
23 namespace BFL
24 {
25 
27  int resampleperiod,
28  double resamplethreshold,
29  int resamplescheme)
30  : ParticleFilter<ColumnVector,ColumnVector>(prior,NULL,
31  resampleperiod,
32  resamplethreshold,
33  resamplescheme)
34  , _dimension(prior->DimensionGet())
35  , _num_samples(prior->NumSamplesGet())
36  {
37  this->_proposal_depends_on_meas = true;
38  _proposal = new EKFProposalDensity(NULL,NULL);
39 
40  _sampleCov.assign(_num_samples,prior->CovarianceGet());
41  _tmpCov.assign(_num_samples,prior->CovarianceGet());
44  _unif_samples.assign(_num_samples,0.0);
45  _CumPDF.assign(_num_samples,0.0);
46  _x_old.resize(_num_samples);
48  }
49 
51  delete _proposal;
52  }
53 
54  // KG: 20040702: This method will be exactly the same for. Extra
55  // class between?
56  bool
58  const CV& u,
59  MeasModel<CV,CV>* const measmodel,
60  const CV& z,
61  const CV& s)
62  {
63  bool result = true;
64  dynamic_cast<FilterProposalDensity *>(_proposal)->SystemModelSet(dynamic_cast<AnalyticSystemModelGaussianUncertainty *>(sysmodel));
65  dynamic_cast<FilterProposalDensity *>(_proposal)->MeasurementModelSet(dynamic_cast<AnalyticMeasurementModelGaussianUncertainty *>(measmodel));
66  // Proposalstep is redefined here...
67  this->StaticResampleStep();
68  result = this->ProposalStepInternal(sysmodel,u,measmodel,z,s) && result;
69  result = this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s) && result;
70  this->DynamicResampleStep();
71 
72  return result;
73  }
74 
75  bool
77  {
78  // They're should be a cleaner solution then doubling the code
79  // from mcpdf.h, doesn't it??
80  // ONLY RIPLEY SAMPLING SUPPORTED FOR NOW!
81  _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
82  int numsamples = _old_samples.size();
83  for ( int i = 0; i < numsamples ; i++) _unif_samples[i] = runif();
84  _unif_samples[numsamples-1] = pow(_unif_samples[numsamples-1], double (1.0/numsamples));
85  for ( int i = numsamples-2; i >= 0 ; i--){
86  _unif_samples[i] = pow(_unif_samples[i],double (1.0/(i+1))) * _unif_samples[i+1];}
87 
88  unsigned int index = 0;
89  _oit = _old_samples.begin();
90  _CumPDF = (dynamic_cast<MCPdf<CV> *>(this->_post))->CumulativePDFGet();
91  _CumPDFit = _CumPDF.begin();
92  _rit = _result_samples.begin();
93  _cov_it = _sampleCov.begin(); _tmpCovit = _tmpCov.begin();
94 
95  for ( int i = 0; i < numsamples ; i++)
96  {
97  while ( _unif_samples[i] > *_CumPDFit )
98  {
99  assert(index <= (unsigned int)numsamples);
100  index++; _oit++; _CumPDFit++; _cov_it++;
101 
102  }
103  _oit--; _cov_it--;
104  *(_rit) = *(_oit);
105  *_tmpCovit = *_cov_it;
106  _oit++; _cov_it++;
107 
108  _rit++; _tmpCovit++;
109  }
110 
111  // Update lists
113  return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_result_samples);
114  }
115 
116  bool
118  const CV& u,
119  MeasModel<CV,CV>* const measmodel,
120  const CV& z,
121  const CV& s)
122  {
123  _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
124 
125  _ns_it = _new_samples.begin();
126  _cov_it = _sampleCov.begin();
127 
128  for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
129  {
130  _x_old = _os_it->ValueGet();
131 
132  // Set sample Covariance
133  dynamic_cast<FilterProposalDensity *>(this->_proposal)->SampleCovSet(*_cov_it);
134 
136 
137  if (!sysmodel->SystemWithoutInputs())
138  {
140  if (this->_proposal_depends_on_meas)
141  {
143  if (!measmodel->SystemWithoutSensorParams())
145  }
146  }
147  else // System without inputs
148  {
149  if (this->_proposal_depends_on_meas)
150  {
152  if (!measmodel->SystemWithoutSensorParams())
154  }
155  }
156  // Bug, make sampling method a parameter!
158 
159  _ns_it->ValueSet(_sample.ValueGet());
160  _ns_it->WeightSet(_os_it->WeightGet());
161  _ns_it++;
162 
163  // Update Covariances here
165  _cov_it++;
166 
167  }
168 
169  (this->_timestep)++;
170  // Update the list of samples
171  return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
172 
173  }
174 }
std::vector< WeightedSample< ColumnVector > > _old_samples
bool SystemWithoutInputs() const
Has the system inputs or not.
Definition: systemmodel.cpp:84
#define DEFAULT
Proposal Density for non-linear systems with additive Gaussian Noise (using a EKF Filter) ...
virtual bool UpdateInternal(SystemModel< ColumnVector > *const sysmodel, const ColumnVector &u, MeasurementModel< ColumnVector, ColumnVector > *const measmodel, const ColumnVector &z, const ColumnVector &s)
Actual implementation of Update, varies along filters.
std::vector< WeightedSample< ColumnVector > >::iterator _oit
std::vector< double > _unif_samples
EKParticleFilter(MCPdf< ColumnVector > *prior, int resampleperiod=0, double resamplethreshold=0, int resamplescheme=DEFAULT_RS)
Constructor.
std::vector< SymmetricMatrix > _tmpCov
void DimensionSet(unsigned int dim)
Pdf< ColumnVector > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
Virtual Class representing all particle filters.
virtual ~EKParticleFilter()
Destructor.
virtual bool DynamicResampleStep()
Resample if necessary.
virtual bool Resample()
Resample also redefined for the same reasons...
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< ColumnVector > >::iterator _os_it
Iterator for old list of samples.
std::vector< SymmetricMatrix >::iterator _cov_it
int _timestep
Represents the current timestep of the filter.
Definition: filter.h:100
vector< WeightedSample< ColumnVector > > _new_samples
While updating store list of new samples.
std::vector< double > _CumPDF
Monte Carlo Pdf: Sample based implementation of Pdf.
Definition: mcpdf.h:49
std::vector< double >::const_iterator _CumPDFit
MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
Proposal Density for non-linear systems with additive Gaussian Noise (using a (analytic) Filter) ...
T & ValueGet()
Get the value of the Sample.
virtual bool StaticResampleStep()
Resample if wanted.
Sample< ColumnVector > _sample
vector< WeightedSample< ColumnVector > >::iterator _ns_it
Iterator for new list of samples.
virtual bool UpdateWeightsInternal(SystemModel< ColumnVector > *const sysmodel, const ColumnVector &u, MeasurementModel< ColumnVector, ColumnVector > *const measmodel, const ColumnVector &z, const ColumnVector &s)
Update Weights.
double runif()
ConditionalPdf< ColumnVector, ColumnVector > * _proposal
Pointer to the Proposal Density.
std::vector< WeightedSample< ColumnVector > > _result_samples
virtual bool ProposalStepInternal(SystemModel< ColumnVector > *const sysmodel, const ColumnVector &u, MeasurementModel< ColumnVector, ColumnVector > *const measmodel, const ColumnVector &z, const ColumnVector &s)
std::vector< WeightedSample< ColumnVector > >::iterator _rit
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)
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
std::vector< SymmetricMatrix > _sampleCov
Sample Covariances for use with EKF Proposal density.
#define CV
std::vector< SymmetricMatrix >::iterator _tmpCovit


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