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
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
00034
00035
00036
00037
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
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
00070
00071 if (sysmodel != NULL)
00072 {
00073
00074 for ( this->_os_it=this->_old_samples.begin();
00075 this->_os_it != this->_old_samples.end() ;
00076 this->_os_it++)
00077 {
00078
00079
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
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
00103 *mu_it = this->_proposal->ExpectedValueGet();
00104
00105 if (!measmodel->SystemWithoutSensorParams())
00106 weightfactor = measmodel->ProbabilityGet(z,*mu_it,s);
00107 else
00108 weightfactor = measmodel->ProbabilityGet(z,*mu_it);
00109
00110
00111 this->_os_it->WeightSet(this->_os_it->WeightGet() * weightfactor);
00112 mu_it++;
00113 }
00114
00115 (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_old_samples);
00116
00117
00118 this->Resample();
00119
00120
00121 this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
00122 }
00123
00124
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
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
00150 (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(this->_new_samples);
00151
00152
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 Mon Feb 11 2019 03:45:12