Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "particlesmoother.h"
00019 #include "../pdf/mcpdf.h"
00020
00021 #define SV StateVar
00022 #define MV MeasVar
00023
00024 template <typename SV>
00025 ParticleSmoother<SV>::ParticleSmoother(MCPdf<SV> * prior)
00026 : BackwardFilter<SV>(prior)
00027 {
00028 this->_post = new MCPdf<SV>(*prior);
00029 (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00030
00031
00032 _old_samples = (prior->ListOfSamplesGet());
00033 _new_samples = _old_samples;
00034 _filtered_samples = _old_samples;
00035 }
00036
00037
00038 template <typename SV>
00039 ParticleSmoother<SV>::~ParticleSmoother()
00040 {
00041 delete this->_post;
00042 }
00043
00044 template <typename SV> bool
00045 ParticleSmoother<SV>::UpdateInternal(SystemModel<StateVar>* const sysmodel, const StateVar& u,Pdf<StateVar>* const filtered_post)
00046 {
00047 cout << "update started" << endl;
00048 SysUpdate(sysmodel,u,filtered_post);
00049 cout << "update fininshed" << endl;
00050 return true;
00051 }
00052
00053 template <typename SV> void
00054 ParticleSmoother<SV>::SysUpdate(SystemModel<StateVar>* const sysmodel, const StateVar& u, Pdf<StateVar>* const filtered_post)
00055 {
00056
00057 _filtered_samples = (dynamic_cast<MCPdf<SV> *>(filtered_post))->ListOfSamplesGet();
00058
00059 _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00060 int N = (dynamic_cast<MCPdf<SV> *>(filtered_post))->NumSamplesGet();
00061 int M = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
00062
00063 vector<double> gamma(M);
00064 Matrix prob(M,N);
00065 int i = 1;
00066 for (_os_it = _old_samples.begin(); _os_it !=_old_samples.end() ; _os_it++)
00067 {
00068 const SV& x_smo = _os_it->ValueGet();
00069 double gamma_loc = 0.0;
00070 int j=1;
00071 for ( _fs_it=_filtered_samples.begin(); _fs_it != _filtered_samples.end() ; _fs_it++)
00072 {
00073 const SV& x_fil = _fs_it->ValueGet();
00074
00075 if (!sysmodel->SystemWithoutInputs()){
00076 prob(i,j) = sysmodel->ProbabilityGet(x_smo,x_fil,u);
00077 }
00078 else{
00079 prob(i,j) = sysmodel->ProbabilityGet(x_smo,x_fil);
00080 }
00081 gamma_loc = gamma_loc + _fs_it->WeightGet() * prob(i,j);
00082
00083 gamma[j-1] = gamma_loc;
00084 j++;
00085 }
00086 i++;
00087 }
00088
00089
00090 _new_samples = _filtered_samples;
00091
00092 i = 1 ;
00093 double tot_weight;
00094 vector<double> weight (_new_samples.size());
00095 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00096 {
00097 double delta = 0.0;
00098 int j=1;
00099 for (_os_it = _old_samples.begin(); _os_it !=_old_samples.end() ; _os_it++)
00100 {
00101 if (gamma[j-1]!=0){
00102 delta = delta + _os_it->WeightGet() * prob(i,j) / gamma[j-1];
00103 }
00104 else{}
00105 j++;
00106 }
00107
00108
00109 weight[i-1] =_ns_it->WeightGet()*delta;
00110 tot_weight = tot_weight + weight[i-1];
00111 i++;
00112 }
00113
00114 i = 1;
00115 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00116 {
00117 _ns_it->WeightSet(weight[i-1]/tot_weight);
00118 i++;
00119 }
00120
00121
00122 (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(_new_samples);
00123
00124 }
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 Sun Oct 5 2014 22:29:53