optimalimportancefilter.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: optimalimportancefilter.cpp 29495 2008-08-13 12:57:49Z tdelaet $
00018 
00019 #include "optimalimportancefilter.h"
00020 
00021 #define SV StateVar
00022 #define MV MeasVar
00023 #define MeasModel MeasurementModel
00024 #define OptImpFilter OptimalImportanceFilter
00025 
00026 template <typename SV, typename MV> void
00027 OptimalImportanceFilter::ConstructProposal(SystemModel<SV>* const sysmodel,
00028                                 MeasModel<MV,SV>* const measmodel)
00029 {
00030 
00031   // DIRTY IMPLEMENTATION
00032   if (_proposal == NULL) // First time, no memory allocated yet
00033     {
00034       // Allocate necessary memory
00035       _proposal = new ConditionalGaussian(sysmodel->SystemPdfGet());
00036     }
00037   else // Check if we really need to allocate memory
00038     if (_proposal->DimensionGet())
00039       ;
00040 }
00041 
00042 template <typename SV, typename MV>
00043 OptimalImportanceFilter<SV,MV>::OptimalImportanceFilter(MCPdf<SV> * prior,
00044                                   int resampleperiod,
00045                                   double resamplethreshold,
00046                                   int resamplescheme)
00047   : ParticleFilter<SV,MV>(prior, NULL, resampleperiod,
00048                           resamplethreshold,
00049                           resamplescheme)
00050 {};
00051 
00052 
00053 template <typename SV, typename MV>
00054 OptimalImportanceFilter<SV,MV>::~OptimalImportanceFilter()
00055 {
00056   delete _proposal;
00057 };
00058 
00059 template <typename SV, typename MV>
00060 OptimalImportanceFilter<SV,MV>::OptimalImportanceFilter(const OptimalImportanceFilter & filter)
00061   : ParticleFilter<SV,MV>(filter){}
00062 
00063 template <typename SV, typename MV> void
00064 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel,
00065                             const SV& u,
00066                             MeasModel<MV,SV>* const measmodel,
00067                             const MV& z,
00068                             const SV& s)
00069 {
00070   // Change here
00071   this->ProposalSet();
00072   this->ProposalStepInput(sysmodel,u);
00073   this->MeasUpdate(measmodel,z,s);
00074 };
00075 
00076 template <typename SV, typename MV> void
00077 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel,
00078                             MeasModel<MV,SV>* const measmodel,
00079                             const MV& z,
00080                             const SV& s)
00081 {
00082   this->ProposalSet();
00083   this->ProposalStep(sysmodel);
00084   this->MeasUpdate(measmodel,z,s);
00085 };
00086 
00087 template <typename SV, typename MV> void
00088 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel,
00089                             MeasModel<MV,SV>* const measmodel,
00090                             const MV& z)
00091 {
00092   this->ProposalSet();
00093   this->ProposalStep(sysmodel);
00094   this->MeasUpdate(measmodel,z);
00095 };
00096 
00097 template <typename SV, typename MV> void
00098 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel,
00099                             const SV& u,
00100                             MeasModel<MV,SV>* const measmodel,
00101                             const MV& z)
00102 {
00103   this->ProposalSet();
00104   this->ProposalStepInput(sysmodel,u);
00105   this->MeasUpdate(measmodel,z);
00106 };
00107 
00109 template <typename SV, typename MV> void
00110 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel,
00111                             const SV& u)
00112 {
00113   this->ProposalSet();
00114   this->ProposalStepInput(sysmodel,u);
00115 }
00116 
00117 template <typename SV, typename MV> void
00118 OptimalImportanceFilter<SV,MV>::Update(SystemModel<SV>* const sysmodel)
00119 {
00120   this->ProposalSet();
00121   this->ProposalStep(sysmodel);
00122 }
00123 
00125 template <typename SV, typename MV> void
00126 OptimalImportanceFilter<SV,MV>::Update(MeasModel<MV,SV>* const measmodel,
00127                             const MV& z,
00128                             const SV& s)
00129 {
00130   this->MeasUpdate(measmodel,z,s);
00131 }
00132 
00133 template <typename SV, typename MV> void
00134 OptimalImportanceFilter<SV,MV>::Update(MeasModel<MV,SV>* const measmodel,
00135                             const MV& z)
00136 {
00137   this->MeasUpdate(measmodel,z);
00138 }
00139 


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