8 mtrand.seed(time(NULL));
10 Filter::Filter(double seed)
21 bool Filter::setNumber(int n)
27 bool Filter::setDimension(int d)
32 bool Filter::createInitialParticles()
34 if(number<0||dimension<0) return(false);
35 x.resize(number, dimension);
36 v.resize(number, dimension);
39 bool Filter::createInitialParticles(double d)
41 if(number<0||dimension<0) return(false);
42 x.resize(number, dimension, d);
43 v.resize(number, dimension);
46 bool Filter::create_system_noise(double mean, double dist)
48 for(int i=0;i<v.size();++i)
50 for(int j=0;j<v[i].size();++j)
52 v[i][j] = mtrand.randNorm(mean, dist);
57 int Filter::dump_predict_particles()
62 int Filter::dump_System_Noise()
67 Particles<double> Filter::get_predict_particles()
71 Particles<double> Filter::get_system_noise()
75 bool Filter::set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v))
80 bool Filter::set_robserve_func(double (*func)(Particles<double> &p, Particle<double> &q))
85 bool Filter::set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particle<double> &q))
87 robserve_jacobian_func = func;
90 bool Filter::set_robserve_density_func(double (*func)(double w))
92 robserve_density_func = func;
95 Filter & Filter::get_next_state()
100 double Filter::get_observed_noise(int i)
102 return((*robserve_func)(y, x[i]));
104 double Filter::get_robserved_density_value(double w)
106 return((*robserve_density_func)(w));
109 bool Filter::set_observed_data(Particles<double> &p)
114 bool Filter::compute_likelihood()
116 for(unsigned int i=0;i<alpha.size();++i)
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]);
126 Particles<double> Filter::get_particles()
130 bool Filter::resampling()
132 double alphasum = 0.0;
134 for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i)
136 //cout<<"alpha["<<k<<"]="<<(*i)<<endl;
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);
147 for(int i=1;i<x.size();++i)
156 psuma[x.size()-1] = psuma[x.size()-2]+alpha[x.size()-1];
157 for(int j=0;j<x.size();++j)
159 double u = ((j+1)-0.5)/m;
160 //cout<<"u="<<u<<endl;
162 // for(int i=1;i<((int)m-2);++i)
163 for(int i=1;i<=x.size();++i)
165 double a1=0.0, a2=0.0;
168 //cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl;
169 //cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl;
170 //cout<<(u<=a2/alphasum)<<"?"<<endl;
171 //cout<<(a1/alphasum<u)<<endl;
172 if((a1/alphasum<u) && (a2/alphasum>=u))
178 //cout<<"sample="<<(sample_i+0)<<endl;
181 f[j] = x[sample_i+0];