src/filter.cc

   1 #include <filter.h>
   2
   3 Filter::Filter()
   4 {
   5   number = -1;
   6   dimension = -1;
   7   state_func = NULL;
   8   mtrand.seed(time(NULL));
   9 }
  10 Filter::Filter(double seed)
  11 {
  12   number = -1;
  13   dimension = -1;
  14   state_func = NULL;
  15   mtrand.seed(seed);
  16 }
  17 Filter::~Filter()
  18 {
  19 }
  20
  21 bool Filter::setNumber(int n)
  22 {
  23   number = n;
  24   alpha.resize(n);
  25   return(true);
  26 }
  27 bool Filter::setDimension(int d)
  28 {
  29   dimension = d;
  30   return(true);
  31 }
  32 bool Filter::createInitialParticles()
  33 {
  34   if(number<0||dimension<0) return(false);
  35   x.resize(number, dimension);
  36   v.resize(number, dimension);
  37   return(true);
  38 }
  39 bool Filter::createInitialParticles(double d)
  40 {
  41   if(number<0||dimension<0) return(false);
  42   x.resize(number, dimension, d);
  43   v.resize(number, dimension);
  44   return(true);
  45 }
  46 bool Filter::create_system_noise(double mean, double dist)
  47 {
  48   for(int i=0;i<v.size();++i)
  49   {
  50     for(int j=0;j<v[i].size();++j)
  51     {
  52       v[i][j] = mtrand.randNorm(mean, dist);
  53     }
  54   }
  55   return(true);
  56 }
  57 int Filter::dump_predict_particles()
  58 {
  59   x.dump_particles();
  60   return(0);
  61 }
  62 int Filter::dump_System_Noise()
  63 {
  64   v.dump_particles();
  65   return(0);
  66 }
  67 Particles<double> Filter::get_predict_particles()
  68 {
  69   return(x);
  70 }
  71 Particles<double> Filter::get_system_noise()
  72 {
  73   return(v);
  74 }
  75 bool Filter::set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v))
  76 {
  77   state_func = func;
  78   return(true);
  79 }
  80 bool Filter::set_robserve_func(double (*func)(Particles<double> &p, Particle<double> &q))
  81 {
  82   robserve_func = func;
  83   return(true);
  84 }
  85 bool Filter::set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particle<double> &q))
  86 {
  87   robserve_jacobian_func = func;
  88   return(true);
  89 }
  90 bool Filter::set_robserve_density_func(double (*func)(double w))
  91 {
  92   robserve_density_func = func;
  93   return(true);
  94 }
  95 Filter & Filter::get_next_state()
  96 {
  97   (*state_func)(x, v);
  98   return(*this);
  99 }
 100 double Filter::get_observed_noise(int i)
 101 {
 102   return((*robserve_func)(y, x[i]));
 103 }
 104 double Filter::get_robserved_density_value(double w)
 105 {
 106   return((*robserve_density_func)(w));
 107 }
 108
 109 bool Filter::set_observed_data(Particles<double> &p)
 110 {
 111   y = p;
 112   return(true);
 113 }
 114 bool Filter::compute_likelihood()
 115 {
 116   for(unsigned int i=0;i<alpha.size();++i)
 117   {
 118 //cout<<"y="<<y[0].toString()<<" x="<<x[i].toString()<<endl;
 119 //cout<<"alpha="<<(*robserve_density_func)((*robserve_func)(y, x[i]))
 120 //             <<"*"<<(*robserve_jacobian_func)(y, x[i])<<endl;
 121     alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]))
 122                 *(*robserve_jacobian_func)(y, x[i]);
 123   }
 124   return(true);
 125 }
 126 Particles<double> Filter::get_particles()
 127 {
 128   return(x);
 129 }
 130 bool Filter::resampling()
 131 {
 132   double alphasum = 0.0;
 133   int k=0;
 134   for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i)
 135   {
 136 //cout<<"alpha["<<k<<"]="<<(*i)<<endl;
 137     ++k;
 138     alphasum += (*i);
 139   }
 140 //cout<<"alpha sum="<<alphasum<<endl;
 141   if(alphasum*alphasum<DBL_EPSILON)
 142     cout<<"Regularize Constant is too small="<<alphasum<<endl;
 143   double m = (double)x.size();
 144   Particles<double> f(number, dimension);
 145   vector<double> psuma(x.size() + 1);
 146   psuma[0] = 0.0;
 147   for(int i=1;i<x.size();++i)
 148   {
 149     int l;
 150     psuma[i]=0.0;
 151     for(l=0;l<i;++l)
 152     {
 153       psuma[i]+=alpha[l];
 154     }
 155   }
 156   psuma[x.size()-1] = psuma[x.size()-2]+alpha[x.size()-1];
 157   for(int j=0;j<x.size();++j)
 158   {
 159     double u = ((j+1)-0.5)/m;
 160 //cout<<"u="<<u<<endl;
 161     int sample_i = -1;
 162 //    for(int i=1;i<((int)m-2);++i)
 163     for(int i=1;i<=x.size();++i)
 164     {
 165       double a1 = psuma[i-1], a2 = psuma[i];
 166 //cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl;
 167 //cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl;
 168 //cout<<(u<=a2/alphasum)<<"?"<<endl;
 169 //cout<<(a1/alphasum<u)<<endl;
 170       if((a1/alphasum<u) && (a2/alphasum>=u))
 171       {
 172         sample_i = i - 1;
 173         break;
 174       }
 175     }
 176 //cout<<"sample="<<(sample_i+0)<<endl;
 177     if(sample_i>=0)
 178     {
 179       f[j] = x[sample_i+0];
 180     }
 181   }
 182   x = f;
 183   return(true);
 184 }