particlesmoother.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00002 //
00003 // This program is free software; you can redistribute it and/or modify
00004 // it under the terms of the GNU Lesser General Public License as published by
00005 // the Free Software Foundation; either version 2.1 of the License, or
00006 // (at your option) any later version.
00007 //
00008 // This program is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011 // GNU Lesser General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU Lesser General Public License
00014 // along with this program; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
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   // Initialise lists of samples
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     // Get all samples from the filtered posterior
00057     _filtered_samples = (dynamic_cast<MCPdf<SV> *>(filtered_post))->ListOfSamplesGet();
00058     // Get all samples from the current post through proposal density
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; //correction factor for weight of new particle
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               // calculate prediction probabilities
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               // calculate correction factors
00083               gamma[j-1] = gamma_loc;
00084               j++;
00085              }
00086       i++;
00087       }
00088       //cout << "probabilities " << prob << endl;
00089       // make copy of filtered sample list => new smoothed samples
00090       _new_samples = _filtered_samples;
00091       // new weights for filtered samples
00092       i = 1 ;
00093       double tot_weight; // sum of weights needed to normalize
00094       vector<double> weight (_new_samples.size()); // vector of weights of particles
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            // cout << "probability for i " << i << " is " << prob.columnCopy(i) << endl;
00108            // cout << "Weight: " << _ns_it->WeightGet() << " * " << delta << endl;
00109           weight[i-1] =_ns_it->WeightGet()*delta;
00110           tot_weight = tot_weight + weight[i-1];
00111           i++;
00112         }
00113     // Normalize the weights
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     // Update the list of samples
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 Thu Feb 11 2016 22:31:57