00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "particlefilter.h"
00020 #include "../pdf/mcpdf.h"
00021
00022 #define SV StateVar
00023 #define MV MeasVar
00024 #define MeasModel MeasurementModel
00025
00026 #define STATE_AND_MEAS_VAR_DIFFERENT
00027
00028 template <typename SV, typename MV>
00029 ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
00030 ConditionalPdf<SV,SV> * proposal,
00031 int resampleperiod,
00032 double resamplethreshold,
00033 int resamplescheme)
00034 : Filter<SV,MV>(prior)
00035 , _proposal(proposal)
00036 , _sample(WeightedSample<SV>(prior->DimensionGet()))
00037 , _resampleScheme(resamplescheme)
00038 , _created_post(true)
00039 {
00040
00041
00042
00043
00044 this->_post = new MCPdf<SV>(prior->NumSamplesGet(),prior->DimensionGet());
00045
00046
00047
00048
00049 bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00050 assert(ret);
00051
00052
00053 _old_samples = (prior->ListOfSamplesGet());
00054 _new_samples = _old_samples;
00055
00056
00057
00058 assert(!(resampleperiod == 0 && resamplethreshold == 0));
00059 assert(!(resampleperiod != 0 && resamplethreshold != 0));
00060
00061
00062 if (resampleperiod == 0)
00063 _dynamicResampling = true;
00064
00065 else
00066 _dynamicResampling = false;
00067 _resamplePeriod = resampleperiod;
00068 _resampleThreshold = resamplethreshold;
00069 }
00070
00071
00072
00073 template <typename SV, typename MV>
00074 ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
00075 MCPdf<SV> * post,
00076 ConditionalPdf<SV,SV> * proposal,
00077 int resampleperiod,
00078 double resamplethreshold,
00079 int resamplescheme)
00080 : Filter<SV,MV>(prior),
00081 _proposal(proposal),
00082 _resampleScheme(resamplescheme),
00083 _created_post(false)
00084 {
00085 this->_post = post;
00086
00087
00088
00089
00090 bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
00091 assert(ret);
00092
00093
00094 _old_samples = (prior->ListOfSamplesGet());
00095 _new_samples = _old_samples;
00096
00097
00098 assert(!(resampleperiod == 0 && resamplethreshold == 0));
00099 assert(!(resampleperiod != 0 && resamplethreshold != 0));
00100
00101
00102 if (resampleperiod == 0)
00103 _dynamicResampling = true;
00104
00105 else
00106 _dynamicResampling = false;
00107
00108 _resamplePeriod = resampleperiod;
00109 _resampleThreshold = resamplethreshold;
00110 }
00111
00112
00113
00114
00115 template <typename SV, typename MV>
00116 ParticleFilter<SV,MV>::~ParticleFilter()
00117 {
00118 if (_created_post)
00119 delete this->_post;
00120 }
00121
00122 template <typename SV, typename MV>
00123 ParticleFilter<SV,MV>::ParticleFilter(const ParticleFilter<SV,MV> & filter)
00124 : Filter<SV,MV>(filter),
00125 _created_post(true)
00126 {
00127
00128
00129 this->_post = new MCPdf<SV>(dynamic_cast<MCPdf<SV> *>(filter._post));
00130 }
00131
00132 template <typename SV, typename MV> void
00133 ParticleFilter<SV,MV>::ProposalSet(ConditionalPdf<SV,SV> * const cpdf)
00134 {
00135 _proposal = cpdf;
00136 }
00137
00138 template <typename SV, typename MV> ConditionalPdf<SV,SV> *
00139 ParticleFilter<SV,MV>::ProposalGet()
00140 {
00141 return _proposal;
00142 }
00143
00144 template <typename SV, typename MV> bool
00145 ParticleFilter<SV,MV>::ProposalStepInternal(SystemModel<SV> * const sysmodel,
00146 const SV & u,
00147 MeasurementModel<MV,SV> * const measmodel,
00148 const MV & z,
00149 const SV & s)
00150 {
00151
00152 _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00153
00154 _ns_it = _new_samples.begin();
00155 for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
00156 {
00157 const SV& x_old = _os_it->ValueGet();
00158 _proposal->ConditionalArgumentSet(0,x_old);
00159
00160 if (!sysmodel->SystemWithoutInputs())
00161 {
00162 _proposal->ConditionalArgumentSet(1,u);
00163 if (this->_proposal_depends_on_meas)
00164 {
00165 #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00166 _proposal->ConditionalArgumentSet(2,z);
00167 if (!measmodel->SystemWithoutSensorParams())
00168 _proposal->ConditionalArgumentSet(3,s);
00169 #endif
00170 }
00171
00172 }
00173 else
00174 {
00175 if (this->_proposal_depends_on_meas)
00176 {
00177 #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00178 _proposal->ConditionalArgumentSet(1,z);
00179 if (!measmodel->SystemWithoutSensorParams())
00180 _proposal->ConditionalArgumentSet(2,s);
00181 #endif
00182
00183 }
00184 }
00185
00186 _proposal->SampleFrom(_sample, DEFAULT,NULL);
00187 _ns_it->ValueSet(_sample.ValueGet());
00188 _ns_it->WeightSet(_os_it->WeightGet());
00189 _ns_it++;
00190 }
00191
00192 (this->_timestep)++;
00193
00194
00195 return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00196
00197 }
00198
00199
00200 template <typename SV, typename MV> bool
00201 ParticleFilter<SV,MV>::UpdateWeightsInternal(SystemModel<SV> * const sysmodel,
00202 const SV & u,
00203 MeasurementModel<MV,SV> * const measmodel,
00204 const MV & z,
00205 const SV & s)
00206 {
00207 Probability weightfactor = 1;
00208
00209
00210
00211 _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00212 _os_it = _old_samples.begin();
00213
00214 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00215 {
00216 const SV& x_new = _ns_it->ValueGet();
00217 const SV& x_old = _os_it->ValueGet();
00218
00219 if (sysmodel == NULL)
00220 {
00221 if (measmodel->SystemWithoutSensorParams() == true)
00222 weightfactor = measmodel->ProbabilityGet(z,x_new);
00223 else
00224 weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00225 }
00226 else
00227 {
00228 _proposal->ConditionalArgumentSet(0,x_old);
00229 if (measmodel->SystemWithoutSensorParams() == true)
00230 {
00231 weightfactor = measmodel->ProbabilityGet(z,x_new);
00232 if (sysmodel->SystemWithoutInputs() == false)
00233 {
00234 _proposal->ConditionalArgumentSet(1,u);
00235 if (this->_proposal_depends_on_meas){
00236 #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00237 _proposal->ConditionalArgumentSet(2,z);
00238 #endif
00239 }
00240
00241 if (_proposal->ProbabilityGet(x_new) != 0)
00242 weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,x_old,u) / _proposal->ProbabilityGet(x_new) );
00243 else weightfactor = 0;
00244 }
00245 else
00246 {
00247 if (this->_proposal_depends_on_meas){
00248 #ifndef STATE_AND_MEAS_VAR_DIFFERENT
00249 _proposal->ConditionalArgumentSet(1,z);
00250 #endif
00251 }
00252 if ( _proposal->ProbabilityGet(x_new) != 0)
00253 weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,_os_it->ValueGet()) / _proposal->ProbabilityGet(x_new) );
00254 else weightfactor = 0;
00255 }
00256 }
00257 else
00258 {
00259 weightfactor = measmodel->ProbabilityGet(z,x_new,s);
00260 }
00261 }
00262 _ns_it->WeightSet(_ns_it->WeightGet() * weightfactor);
00263
00264 _os_it++;
00265 }
00266
00267 return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);
00268
00269 }
00270
00271 template <typename SV, typename MV> bool
00272 ParticleFilter<SV,MV>::DynamicResampleStep()
00273 {
00274
00275 bool resampling = false;
00276 double sum_sq_weigths = 0.0;
00277
00278
00279 if ( this->_dynamicResampling)
00280 {
00281
00282
00283
00284
00285 _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00286 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00287 {
00288 sum_sq_weigths += pow(_ns_it->WeightGet(),2);
00289 }
00290 if ((1.0 / sum_sq_weigths) < _resampleThreshold)
00291 {
00292
00293 #ifdef __RESAMPLE_DEBUG__
00294 cout << "resampling now: " << this->_timestep
00295 << "\tN_eff: " << (1.0 / sum_sq_weigths) << endl;
00296 #endif // __RESAMPLE_DEBUG__
00297 resampling = true;
00298 }
00299 }
00300 if (resampling == true)
00301 return this->Resample();
00302 else
00303 return true;
00304 }
00305
00306
00307 template <typename SV, typename MV> bool
00308 ParticleFilter<SV,MV>::StaticResampleStep()
00309 {
00310
00311 if ( (!this->_dynamicResampling) && (((this->_timestep) % _resamplePeriod) == 0) && (this->_timestep != 0))
00312 return this->Resample();
00313 return true;
00314 }
00315
00316
00317 template <typename SV, typename MV> bool
00318 ParticleFilter<SV,MV>::UpdateInternal(SystemModel<StateVar>* const sysmodel,
00319 const StateVar& u,
00320 MeasurementModel<MeasVar,StateVar>* const measmodel,
00321 const MeasVar& z,
00322 const StateVar& s)
00323 {
00324 bool result = true;
00325
00326
00327
00328 if (sysmodel != NULL)
00329 {
00330 result = result && this->StaticResampleStep();
00331 result = result && this->ProposalStepInternal(sysmodel,u,measmodel,z,s);
00332
00333 }
00334
00335 if (measmodel != NULL)
00336 {
00337 result = result && this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s);
00338 result = result && this->DynamicResampleStep();
00339 }
00340
00341 return result;
00342 }
00343
00344 template <typename SV, typename MV> bool
00345 ParticleFilter<SV,MV>::Resample()
00346 {
00347 int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
00348
00349 #ifdef __PARTICLEFILTER_DEBUG__
00350 cout << "PARTICLEFILTER: resampling now" << endl;
00351 _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00352 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00353 {
00354 cout << "PF: Old samples:\n";
00355 cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00356 }
00357 #endif // __PARTICLEFILTER_DEBUG
00358 switch(_resampleScheme)
00359 {
00360 case MULTINOMIAL_RS:
00361 {
00362 (dynamic_cast<MCPdf<SV> *>(this->_post))->SampleFrom(_new_samples_unweighted, NumSamples,RIPLEY,NULL);
00363 break;
00364 }
00365 case SYSTEMATIC_RS:{break;}
00366 case STRATIFIED_RS:{break;}
00367 case RESIDUAL_RS:{break;}
00368 default:
00369 {
00370 cerr << "Sampling method not supported" << endl;
00371 break;
00372 }
00373 }
00374 bool result = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples_unweighted);
00375 #ifdef __PARTICLEFILTER_DEBUG__
00376 cout << "PARTICLEFILTER: after resampling" << endl;
00377 _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
00378 for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
00379 {
00380 cout << "PF: New samples:\n";
00381 cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
00382 }
00383 #endif // __PARTICLEFILTER_DEBUG
00384
00385 return result;
00386 }
00387
00388
00389 template<typename SV, typename MV> MCPdf<SV> *
00390 ParticleFilter<SV,MV>::PostGet()
00391 {
00392 return (MCPdf<SV>*)Filter<SV,MV>::PostGet();
00393 }