asirfilter.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
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 // $Id: asirfilter.cpp 29495 2008-08-13 12:57:49Z tdelaet $
00018 
00019 #include "asirfilter.h"
00020 #include "../sample/weightedsample.h"
00021 
00022 #define StateVar SVar
00023 #define MeasVar MVar
00024 
00025 template <typename SVar, typename MVar>
00026 ASIRFilter<SVar,MVar>::ASIRFilter(MCPdf<SVar> * prior,
00027                                   int resampleperiod,
00028                                   double resamplethreshold,
00029                                   int resamplescheme)
00030   : ParticleFilter<SVar,MVar>(prior,NULL,resampleperiod,
00031                               resamplethreshold, resamplescheme)
00032 {
00033   /* Well euhm, technically speaking it does, but we solve this
00034      software-matically in another way (by reusing the
00035      ProposalStepInternal from particlefilter.cpp).  This is because
00036      otherwise we would have to create a hybrid proposal density,
00037      which requires far more programming.
00038   */
00039   this->_proposal_depends_on_meas = false;
00040 }
00041 
00042 
00043 template <typename SVar, typename MVar>
00044 ASIRFilter<SVar,MVar>::~ASIRFilter(){}
00045 
00046 
00047 template <typename SVar, typename MVar> void
00048 ASIRFilter<SVar,MVar>::UpdateInternal(SystemModel<SVar>* const sysmodel,
00049                                       const SVar& u,
00050                                       MeasurementModel<MVar,SVar>* const measmodel,
00051                                       const MVar& z,
00052                                       const SVar& s)
00053 {
00054   if (sysmodel != NULL)
00055     {
00056       this->ProposalSet(sysmodel->SystemPdfGet());
00057     }
00058 
00059   /* The following code differs from standard SIR filter */
00060   this->_old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00061   int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
00062 
00063   static SV x_old;
00064   static vector<SV> mu(NumSamples);
00065   typename vector<SV>::iterator mu_it = mu.begin();
00066   static  Probability weightfactor;
00067   static Sample<SV> sample;
00068 
00069   // Only if there's a system model (for now, probably not general
00070   // enough)
00071   if (sysmodel != NULL)
00072     {
00073       // Step 1:  Calculate beta values and adapt weights
00074       for ( this->_os_it=this->_old_samples.begin();
00075             this->_os_it != this->_old_samples.end() ;
00076             this->_os_it++)
00077         {
00078           // Since the proposal is equal to the SystemPdf of the model
00079           // here, simulating the systemmodel will do fine
00080           x_old = this->_os_it->ValueGet();
00081           this->_proposal->ConditionalArgumentSet(0,x_old);
00082 
00083           if (!sysmodel->SystemWithoutInputs())
00084             {
00085               this->_proposal->ConditionalArgumentSet(1,u);
00086               if (this->_proposal_depends_on_meas)
00087                 {
00088                   this->_proposal->ConditionalArgumentSet(2,z);
00089                   if (!measmodel->SystemWithoutSensorParams())
00090                     this->_proposal->ConditionalArgumentSet(3,s);
00091                 }
00092             }
00093           else // System without inputs
00094             {
00095               if (this->_proposal_depends_on_meas)
00096                 {
00097                   this->_proposal->ConditionalArgumentSet(1,z);
00098                   if (!measmodel->SystemWithoutSensorParams())
00099                     this->_proposal->ConditionalArgumentSet(2,s);
00100                 }
00101             }
00102           // Get Expected Value of of this particles
00103           *mu_it = this->_proposal->ExpectedValueGet();
00104           // Calculate likelihood of this particle
00105           if (!measmodel->SystemWithoutSensorParams())
00106             weightfactor = measmodel->ProbabilityGet(z,*mu_it,s);
00107           else
00108             weightfactor = measmodel->ProbabilityGet(z,*mu_it);
00109 
00110           // Set new weight
00111           this->_os_it->WeightSet(this->_os_it->WeightGet() * weightfactor);
00112           mu_it++;
00113         }
00114       // Update list of samples
00115       (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_old_samples);
00116 
00117       // Step 2: Sample discrete index k (in O (N) ops)
00118       this->Resample();
00119 
00120       // Step 3: Proposal step
00121       this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
00122     }
00123 
00124   // Step 4: Update the weights
00125   if (measmodel != NULL)
00126     {
00127       this->_new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00128 
00129       static SV x_new;
00130       mu_it=mu.begin();
00131       for ( this->_ns_it=this->_new_samples.begin();
00132             this->_ns_it != this->_new_samples.end() ;
00133             this->_ns_it++)
00134         {
00135           x_new = this->_ns_it->ValueGet();
00136           if (measmodel->SystemWithoutSensorParams() == true)
00137             {
00138               if (measmodel->ProbabilityGet(z,*mu_it) != 0)
00139                 weightfactor = measmodel->ProbabilityGet(z,x_new) / measmodel->ProbabilityGet(z,*mu_it);
00140               else weightfactor = 0;
00141             }
00142           else // System with sensor Parameters
00143             if (measmodel->ProbabilityGet(z,*mu_it,s) != 0)
00144               weightfactor = measmodel->ProbabilityGet(z,x_new,s) / measmodel->ProbabilityGet(z,*mu_it,s);
00145             else weightfactor = 0;
00146           this->_ns_it->WeightSet(this->_ns_it->WeightGet() * weightfactor);
00147           mu_it++;
00148         }
00149       // Update the sample list of post the SumofWeights of the pdf
00150       (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_new_samples);
00151 
00152       // Resample
00153       this->ResampleStep();
00154     }
00155 }


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 Fri Aug 28 2015 10:10:21