00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef __MIXTURE_PARTICLE_FILTER__
00033 #define __MIXTURE_PARTICLE_FILTER__
00034
00035 #include "filter.h"
00036 #include "../pdf/conditionalpdf.h"
00037 #include "../pdf/mcpdf.h"
00038 #include "../pdf/mixture.h"
00039
00040
00041
00042 #define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
00043 #define MULTINOMIAL_RS 0
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 #define SYSTEMATIC_RS 1
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 #define STRATIFIED_RS 2
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 #define RESIDUAL_RS 3
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 #define MINIMUM_VARIANCE_RS 4
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 namespace BFL
00126 {
00127
00129
00153 template <typename StateVar, typename MeasVar> class MixtureParticleFilter
00154 : public Filter<StateVar,MeasVar>
00155 {
00156 protected:
00157 virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
00158 const StateVar& u,
00159 MeasurementModel<MeasVar,StateVar>* const measmodel,
00160 const MeasVar& z,
00161 const StateVar& s);
00162
00164
00168 ConditionalPdf<StateVar,StateVar> * _proposal;
00169
00171 WeightedSample<StateVar> _sample;
00173 vector<vector<WeightedSample<StateVar> > > _old_samplesVec;
00175 vector<vector<WeightedSample<StateVar> > > _new_samplesVec;
00177 vector< vector<Sample<StateVar> > > _new_samples_unweightedVec;
00179 typename vector<WeightedSample<StateVar> >::iterator _os_it;
00181 typename vector<WeightedSample<StateVar> >::iterator _ns_it;
00183 vector<Probability> _newMixtureWeights;
00185 vector<Probability> _sumWeights;
00186
00188
00191 int _resamplePeriod;
00192
00194 double _resampleThreshold;
00195
00197 int _resampleScheme;
00198
00200 bool _dynamicResampling;
00201
00203 bool _proposal_depends_on_meas;
00204
00206 bool _created_post;
00207
00209
00213 int _maintainMixturePeriod;
00214
00216
00224 virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
00225 const StateVar & u,
00226 MeasurementModel<MeasVar,StateVar> * const measmodel,
00227 const MeasVar & z,
00228 const StateVar & s);
00229
00231
00240 virtual bool ProposalStepInternalOne(int component, SystemModel<StateVar> * const sysmodel,
00241 const StateVar & u,
00242 MeasurementModel<MeasVar,StateVar> * const measmodel,
00243 const MeasVar & z,
00244 const StateVar & s);
00245
00247
00253 virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
00254 const StateVar & u,
00255 MeasurementModel<MeasVar,StateVar> * const measmodel,
00256 const MeasVar & z,
00257 const StateVar & s);
00258
00260
00267 virtual bool UpdateWeightsInternalOne(int component, SystemModel<StateVar> * const sysmodel,
00268 const StateVar & u,
00269 MeasurementModel<MeasVar,StateVar> * const measmodel,
00270 const MeasVar & z,
00271 const StateVar & s);
00272
00274 virtual bool DynamicResampleStep();
00275
00277
00280 virtual bool DynamicResampleStepOne(int component);
00281
00283
00285 virtual bool StaticResampleStep();
00286
00288 virtual bool Resample();
00289
00291
00293 virtual bool ResampleOne(int component);
00294
00296
00298 virtual bool MaintainMixtureStep();
00299
00301 virtual bool MaintainMixture();
00302
00303
00304 public:
00306
00318 MixtureParticleFilter(Mixture<StateVar> * prior,
00319 ConditionalPdf<StateVar,StateVar> * proposal,
00320 int resampleperiod = 0,
00321 double resamplethreshold = 0,
00322 int resamplescheme = DEFAULT_RS,
00323 int maintainMixturePeriod = 1 );
00324
00325
00327
00340 MixtureParticleFilter(Mixture<StateVar> * prior,
00341 Mixture<StateVar> * post,
00342 ConditionalPdf<StateVar,StateVar> * proposal,
00343 int resampleperiod = 0,
00344 double resamplethreshold = 0,
00345 int resamplescheme = DEFAULT_RS,
00346 int maintainMixturePeriod = 1 );
00347
00349 virtual ~MixtureParticleFilter();
00351
00353 MixtureParticleFilter(const MixtureParticleFilter<StateVar,MeasVar> & filt);
00354
00356 virtual void Reset(Mixture<StateVar> * prior);
00357
00359
00364 virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf);
00365
00367
00369 ConditionalPdf<StateVar,StateVar> * ProposalGet();
00370
00371
00372 virtual Mixture<StateVar> * PostGet();
00373 };
00374
00375 #include "mixtureParticleFilter.cpp"
00376
00377 }
00378
00379 #endif // __MIXTURE_PARTICLE_FILTER__