00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "EKparticlefilter.h"
00020
00021 #define MeasModel MeasurementModel
00022 #define CV ColumnVector
00023 namespace BFL
00024 {
00025
00026 EKParticleFilter::EKParticleFilter(MCPdf<CV> * prior,
00027 int resampleperiod,
00028 double resamplethreshold,
00029 int resamplescheme)
00030 : ParticleFilter<ColumnVector,ColumnVector>(prior,NULL,
00031 resampleperiod,
00032 resamplethreshold,
00033 resamplescheme)
00034 , _dimension(prior->DimensionGet())
00035 , _num_samples(prior->NumSamplesGet())
00036 {
00037 this->_proposal_depends_on_meas = true;
00038 _proposal = new EKFProposalDensity(NULL,NULL);
00039
00040 _sampleCov.assign(_num_samples,prior->CovarianceGet());
00041 _tmpCov.assign(_num_samples,prior->CovarianceGet());
00042 _old_samples.assign(_num_samples,WeightedSample<ColumnVector>(_dimension));
00043 _result_samples.assign(_num_samples,WeightedSample<ColumnVector>(_dimension));
00044 _unif_samples.assign(_num_samples,0.0);
00045 _CumPDF.assign(_num_samples,0.0);
00046 _x_old.resize(_num_samples);
00047 _sample.DimensionSet(_dimension);
00048 }
00049
00050 EKParticleFilter::~EKParticleFilter(){
00051 delete _proposal;
00052 }
00053
00054
00055
00056 bool
00057 EKParticleFilter::UpdateInternal(SystemModel<CV>* const sysmodel,
00058 const CV& u,
00059 MeasModel<CV,CV>* const measmodel,
00060 const CV& z,
00061 const CV& s)
00062 {
00063 bool result = true;
00064 dynamic_cast<FilterProposalDensity *>(_proposal)->SystemModelSet(dynamic_cast<AnalyticSystemModelGaussianUncertainty *>(sysmodel));
00065 dynamic_cast<FilterProposalDensity *>(_proposal)->MeasurementModelSet(dynamic_cast<AnalyticMeasurementModelGaussianUncertainty *>(measmodel));
00066
00067 this->StaticResampleStep();
00068 result = this->ProposalStepInternal(sysmodel,u,measmodel,z,s) && result;
00069 result = this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s) && result;
00070 this->DynamicResampleStep();
00071
00072 return result;
00073 }
00074
00075 bool
00076 EKParticleFilter::Resample()
00077 {
00078
00079
00080
00081 _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
00082 int numsamples = _old_samples.size();
00083 for ( int i = 0; i < numsamples ; i++) _unif_samples[i] = runif();
00084 _unif_samples[numsamples-1] = pow(_unif_samples[numsamples-1], double (1.0/numsamples));
00085 for ( int i = numsamples-2; i >= 0 ; i--){
00086 _unif_samples[i] = pow(_unif_samples[i],double (1.0/(i+1))) * _unif_samples[i+1];}
00087
00088 unsigned int index = 0;
00089 _oit = _old_samples.begin();
00090 _CumPDF = (dynamic_cast<MCPdf<CV> *>(this->_post))->CumulativePDFGet();
00091 _CumPDFit = _CumPDF.begin();
00092 _rit = _result_samples.begin();
00093 _cov_it = _sampleCov.begin(); _tmpCovit = _tmpCov.begin();
00094
00095 for ( int i = 0; i < numsamples ; i++)
00096 {
00097 while ( _unif_samples[i] > *_CumPDFit )
00098 {
00099 assert(index <= (unsigned int)numsamples);
00100 index++; _oit++; _CumPDFit++; _cov_it++;
00101
00102 }
00103 _oit--; _cov_it--;
00104 *(_rit) = *(_oit);
00105 *_tmpCovit = *_cov_it;
00106 _oit++; _cov_it++;
00107
00108 _rit++; _tmpCovit++;
00109 }
00110
00111
00112 _sampleCov = _tmpCov;
00113 return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_result_samples);
00114 }
00115
00116 bool
00117 EKParticleFilter::ProposalStepInternal(SystemModel<CV>* const sysmodel,
00118 const CV& u,
00119 MeasModel<CV,CV>* const measmodel,
00120 const CV& z,
00121 const CV& s)
00122 {
00123 _old_samples = (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesGet();
00124
00125 _ns_it = _new_samples.begin();
00126 _cov_it = _sampleCov.begin();
00127
00128 for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
00129 {
00130 _x_old = _os_it->ValueGet();
00131
00132
00133 dynamic_cast<FilterProposalDensity *>(this->_proposal)->SampleCovSet(*_cov_it);
00134
00135 _proposal->ConditionalArgumentSet(0,_x_old);
00136
00137 if (!sysmodel->SystemWithoutInputs())
00138 {
00139 _proposal->ConditionalArgumentSet(1,u);
00140 if (this->_proposal_depends_on_meas)
00141 {
00142 _proposal->ConditionalArgumentSet(2,z);
00143 if (!measmodel->SystemWithoutSensorParams())
00144 _proposal->ConditionalArgumentSet(3,s);
00145 }
00146 }
00147 else
00148 {
00149 if (this->_proposal_depends_on_meas)
00150 {
00151 _proposal->ConditionalArgumentSet(1,z);
00152 if (!measmodel->SystemWithoutSensorParams())
00153 _proposal->ConditionalArgumentSet(2,s);
00154 }
00155 }
00156
00157 _proposal->SampleFrom(_sample, DEFAULT,NULL);
00158
00159 _ns_it->ValueSet(_sample.ValueGet());
00160 _ns_it->WeightSet(_os_it->WeightGet());
00161 _ns_it++;
00162
00163
00164 *_cov_it = _proposal->CovarianceGet();
00165 _cov_it++;
00166
00167 }
00168
00169 (this->_timestep)++;
00170
00171 return (dynamic_cast<MCPdf<CV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00172
00173 }
00174 }