00001 #ifndef PARTICLEFILTER_H
00002 #define PARTICLEFILTER_H
00003 #include <stdlib.h>
00004 #include <float.h>
00005 #include <sys/types.h>
00006 #include<vector>
00007 #include<utility>
00008 #include<cmath>
00009 #include<utils/gvalues.h>
00010
00011
00022 typedef std::pair<uint,uint> UIntPair;
00023
00024 template <class OutputIterator, class Iterator>
00025 double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
00026
00027
00028 double lmax=-DBL_MAX;
00029 for (Iterator it=begin; it!=end; it++){
00030 lmax=lmax>((double)(*it))? lmax: (double)(*it);
00031 }
00032
00033 for (Iterator it=begin; it!=end; it++){
00034 *out=exp((double)(*it)-lmax);
00035 out++;
00036 }
00037 return lmax;
00038 }
00039
00040 template <class OutputIterator, class Iterator, class Numeric>
00041 void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
00042
00043 for (Iterator it=begin; it!=end; it++){
00044 *out=log((Numeric)(*it))-lmax;
00045 out++;
00046 }
00047 return lmax;
00048 }
00049
00050 template <class WeightVector>
00051 void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
00052 double cweight=0;
00053
00054
00055 unsigned int n=0;
00056 for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
00057 cweight+=(double)*it;
00058 n++;
00059 }
00060
00061 if (nparticles>0)
00062 n=nparticles;
00063
00064
00065 double interval=cweight/n;
00066
00067
00068 double target=interval*::drand48();
00069
00070
00071 cweight=0;
00072 indexes.resize(n);
00073
00074 n=0;
00075 unsigned int i=0;
00076 for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
00077 cweight+=(double)* it;
00078 while(cweight>target){
00079 indexes[n++]=i;
00080 target+=interval;
00081 }
00082 }
00083 }
00084
00085 template <typename Vector>
00086 void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
00087 assert(indexes.size()==particles.size());
00088 dest.resize(particles.size());
00089 unsigned int i=0;
00090 for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
00091 dest[i]=particles[*it];
00092 i++;
00093 }
00094 }
00095
00096
00097 template <class Iterator>
00098 double neff(const Iterator& begin, const Iterator& end){
00099 double sum=0;
00100 for (Iterator it=begin; it!=end; ++it){
00101 sum+=*it;
00102 }
00103 double cum=0;
00104 for (Iterator it=begin; it!=end; ++it){
00105 double w=*it/sum;
00106 cum+=w*w;
00107 }
00108 return 1./cum;
00109 }
00110
00111 template <class Iterator>
00112 void normalize(const Iterator& begin, const Iterator& end){
00113 double sum=0;
00114 for (Iterator it=begin; it!=end; ++it){
00115 sum+=*it;
00116 }
00117 for (Iterator it=begin; it!=end; ++it){
00118 *it=*it/sum;
00119 }
00120 }
00121
00122 template <class OutputIterator, class Iterator>
00123 void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
00124 unsigned int current=0;
00125 unsigned int count=0;
00126 for (Iterator it=begin; it!=end; it++){
00127 if (it==begin){
00128 current=*it;
00129 count=1;
00130 continue;
00131 }
00132 if (((uint)*it) ==current)
00133 count++;
00134 if (((uint)*it)!=current){
00135 *out=std::make_pair(current,count);
00136 out++;
00137 current=*it;
00138 count=1;
00139 }
00140 }
00141 if (count>0)
00142 *out=std::make_pair(current,count);
00143 out++;
00144 }
00145
00146
00147 template <class Particle, class Numeric>
00148 struct uniform_resampler{
00149 std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
00150 std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
00151 Numeric neff(const std::vector<Particle> & particles) const;
00152 };
00153
00154
00155 template <class Particle, class Numeric>
00156 std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
00157 Numeric cweight=0;
00158
00159
00160 unsigned int n=0;
00161 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00162 cweight+=(Numeric)*it;
00163 n++;
00164 }
00165
00166 if (nparticles>0)
00167 n=nparticles;
00168
00169
00170 Numeric interval=cweight/n;
00171
00172
00173 Numeric target=interval*::drand48();
00174
00175
00176 cweight=0;
00177 std::vector<unsigned int> indexes(n);
00178 n=0;
00179 unsigned int i=0;
00180 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
00181 cweight+=(Numeric)* it;
00182 while(cweight>target){
00183 indexes[n++]=i;
00184 target+=interval;
00185 }
00186 }
00187 return indexes;
00188 }
00189
00190 template <class Particle, class Numeric>
00191 std::vector<Particle> uniform_resampler<Particle,Numeric>::resample
00192 (const typename std::vector<Particle>& particles, int nparticles) const{
00193 Numeric cweight=0;
00194
00195
00196 unsigned int n=0;
00197 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00198 cweight+=(Numeric)*it;
00199 n++;
00200 }
00201
00202 if (nparticles>0)
00203 n=nparticles;
00204
00205
00206 double uw=1./n;
00207
00208
00209 Numeric interval=cweight/n;
00210
00211
00212 Numeric target=interval*::drand48();
00213
00214
00215 cweight=0;
00216 std::vector<Particle> resampled;
00217 n=0;
00218 unsigned int i=0;
00219 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
00220 cweight+=(Numeric)*it;
00221 while(cweight>target){
00222 resampled.push_back(*it);
00223 resampled.back().setWeight(uw);
00224 target+=interval;
00225 }
00226 }
00227 return resampled;
00228 }
00229
00230 template <class Particle, class Numeric>
00231 Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
00232 double cum=0;
00233 double sum=0;
00234 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00235 Numeric w=(Numeric)*it;
00236 cum+=w*w;
00237 sum+=w;
00238 }
00239 return sum*sum/cum;
00240 }
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262 template <class Particle, class EvolutionModel>
00263 struct evolver{
00264 EvolutionModel evolutionModel;
00265 void evolve(std::vector<Particle>& particles);
00266 void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
00267 };
00268
00269 template <class Particle, class EvolutionModel>
00270 void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& particles){
00271 for (typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
00272 *it=evolutionModel.evolve(*it);
00273 }
00274 }
00275
00276 template <class Particle, class EvolutionModel>
00277 void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& dest, const std::vector<Particle>& src){
00278 dest.clear();
00279 for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
00280 dest.push_back(evolutionModel.evolve(*it));
00281 }
00282
00283
00284 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00285 struct auxiliary_evolver{
00286 EvolutionModel evolutionModel;
00287 QualificationModel qualificationModel;
00288 LikelyhoodModel likelyhoodModel;
00289 void evolve(std::vector<Particle>& particles);
00290 void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
00291 };
00292
00293 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00294 void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
00295 (std::vector<Particle>&particles){
00296 std::vector<Numeric> observationWeights(particles.size());
00297 unsigned int i=0;
00298 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
00299 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
00300 }
00301 uniform_resampler<Numeric, Numeric> resampler;
00302 std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
00303 for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
00304 Particle & particle=particles[*it];
00305 particle=evolutionModel.evolve(particle);
00306 particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
00307 }
00308 }
00309
00310 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00311 void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
00312 (std::vector<Particle>& dest, const std::vector<Particle>& src){
00313 dest.clear();
00314 std::vector<Numeric> observationWeights(src.size());
00315 unsigned int i=0;
00316 for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
00317 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
00318 }
00319 uniform_resampler<Numeric, Numeric> resampler;
00320 std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
00321 for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
00322 Particle & particle=src[*it];
00323 dest.push_back(evolutionModel.evolve(particle));
00324 dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it];
00325 }
00326 }
00327
00328
00329 #endif