particlesmoother.cpp
Go to the documentation of this file.
1 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
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 
18 #include "particlesmoother.h"
19 #include "../pdf/mcpdf.h"
20 
21 #define SV StateVar
22 #define MV MeasVar
23 
24 template <typename SV>
26  : BackwardFilter<SV>(prior)
27 {
28  this->_post = new MCPdf<SV>(*prior);
29  (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
30 
31  // Initialise lists of samples
32  _old_samples = (prior->ListOfSamplesGet());
35 }
36 
37 
38 template <typename SV>
40 {
41  delete this->_post;
42 }
43 
44 template <typename SV> bool
46 {
47  cout << "update started" << endl;
48  SysUpdate(sysmodel,u,filtered_post);
49  cout << "update fininshed" << endl;
50  return true;
51 }
52 
53 template <typename SV> void
54 ParticleSmoother<SV>::SysUpdate(SystemModel<StateVar>* const sysmodel, const StateVar& u, Pdf<StateVar>* const filtered_post)
55 {
56  // Get all samples from the filtered posterior
57  _filtered_samples = (dynamic_cast<MCPdf<SV> *>(filtered_post))->ListOfSamplesGet();
58  // Get all samples from the current post through proposal density
59  _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
60  int N = (dynamic_cast<MCPdf<SV> *>(filtered_post))->NumSamplesGet();
61  int M = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
62 
63  vector<double> gamma(M);
64  Matrix prob(M,N);
65  int i = 1;
66  for (_os_it = _old_samples.begin(); _os_it !=_old_samples.end() ; _os_it++)
67  {
68  const SV& x_smo = _os_it->ValueGet();
69  double gamma_loc = 0.0; //correction factor for weight of new particle
70  int j=1;
71  for ( _fs_it=_filtered_samples.begin(); _fs_it != _filtered_samples.end() ; _fs_it++)
72  {
73  const SV& x_fil = _fs_it->ValueGet();
74  // calculate prediction probabilities
75  if (!sysmodel->SystemWithoutInputs()){
76  prob(i,j) = sysmodel->ProbabilityGet(x_smo,x_fil,u);
77  }
78  else{
79  prob(i,j) = sysmodel->ProbabilityGet(x_smo,x_fil);
80  }
81  gamma_loc = gamma_loc + _fs_it->WeightGet() * prob(i,j);
82  // calculate correction factors
83  gamma[j-1] = gamma_loc;
84  j++;
85  }
86  i++;
87  }
88  //cout << "probabilities " << prob << endl;
89  // make copy of filtered sample list => new smoothed samples
91  // new weights for filtered samples
92  i = 1 ;
93  double tot_weight; // sum of weights needed to normalize
94  vector<double> weight (_new_samples.size()); // vector of weights of particles
95  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
96  {
97  double delta = 0.0;
98  int j=1;
99  for (_os_it = _old_samples.begin(); _os_it !=_old_samples.end() ; _os_it++)
100  {
101  if (gamma[j-1]!=0){
102  delta = delta + _os_it->WeightGet() * prob(i,j) / gamma[j-1];
103  }
104  else{}
105  j++;
106  }
107  // cout << "probability for i " << i << " is " << prob.columnCopy(i) << endl;
108  // cout << "Weight: " << _ns_it->WeightGet() << " * " << delta << endl;
109  weight[i-1] =_ns_it->WeightGet()*delta;
110  tot_weight = tot_weight + weight[i-1];
111  i++;
112  }
113  // Normalize the weights
114  i = 1;
115  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
116  {
117  _ns_it->WeightSet(weight[i-1]/tot_weight);
118  i++;
119  }
120 
121  // Update the list of samples
122  (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(_new_samples);
123 
124 }
vector< WeightedSample< StateVar > >::iterator _ns_it
Iterator for new list of samples.
bool SystemWithoutInputs() const
Has the system inputs or not.
Definition: systemmodel.cpp:84
Class PDF: Virtual Base class representing Probability Density Functions.
Definition: pdf.h:53
vector< WeightedSample< StateVar > > _filtered_samples
While updating store list of filtered samples.
#define SV
virtual bool UpdateInternal(SystemModel< StateVar > *const sysmodel, const StateVar &u, Pdf< StateVar > *const filtered_post)
Actual implementation of Update, varies along filters.
vector< WeightedSample< StateVar > >::iterator _os_it
Iterator for old list of samples.
Virtual Baseclass representing all bayesian backward filters.
vector< WeightedSample< StateVar > > _new_samples
While updating store list of new samples.
#define StateVar
Definition: asirfilter.h:22
virtual void SysUpdate(SystemModel< StateVar > *const sysmodel, const StateVar &u, Pdf< StateVar > *const filtered_post)
Class representing a particle backward filter.
const vector< WeightedSample< T > > & ListOfSamplesGet() const
Get the list of weighted samples.
virtual ~ParticleSmoother()
Destructor.
Monte Carlo Pdf: Sample based implementation of Pdf.
Definition: mcpdf.h:49
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 _fs_it
Iterator for list of filtered samples.
Pdf< StateVar > * _post
Pointer to the Posterior Pdf.
bool ListOfSamplesSet(const vector< WeightedSample< T > > &list_of_samples)
Set the list of weighted samples.
vector< WeightedSample< StateVar > > _old_samples
While updating store list of old samples.


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