• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版19 (tree)
时间2010-05-11 20:56:59
作者(del#42041)

Log Message

implemented resampling

更改概述

差异

--- test/test_filter.cc (revision 18)
+++ test/test_filter.cc (revision 19)
@@ -82,6 +82,8 @@
8282 a.set_robserve_density_func(func4);
8383 cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308));
8484 a.compute_likelihood();
85+ a.resampling();
86+ cut_assert_equal_string("(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)",a.predict_particles_toString().c_str());
8587 }
8688 }
8789 int main()
--- src/filter.cc (revision 18)
+++ src/filter.cc (revision 19)
@@ -121,16 +121,40 @@
121121 }
122122 return(true);
123123 }
124+Particles<double> Filter::get_particles()
125+{
126+ return(x);
127+}
124128 bool Filter::resampling()
125129 {
126130 double alphasum = 0.0;
127- for(vector<double>::iterator i=0;i!=alpha.end();++i)
131+ for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i)
128132 {
129- alphasum += alpha[i];
133+ alphasum += (*i);
130134 }
131135 double m = (double)x.size();
136+ Particles<double> f(number, dimension);
132137 for(int j=0;j<x.size();++j)
133138 {
134- double u = (j-0.5)/m;
139+ double u = ((j+1)-0.5)/m, a1=0.0, a2=0.0;
140+ int sample_i = 0;
141+ for(int i=1;i<((int)m-2);++i)
142+ {
143+ int l;
144+ for(l=0;l<i;++l)
145+ {
146+ a1+=alpha[l];
147+ }
148+ a2 = a1 + alpha[l];
149+//cout<<a1/alphasum<<"<"<<u<<"<"<<a2/alphasum<<"?"<<endl;
150+ if(a1/alphasum<u && a2/alphasum>=u)
151+ {
152+ sample_i = i;
153+ break;
154+ }
155+ }
156+ f[j] = x[sample_i];
135157 }
158+ x = f;
159+ return(true);
136160 }
\ No newline at end of file
--- src/filter.h (revision 18)
+++ src/filter.h (revision 19)
@@ -29,6 +29,7 @@
2929 bool createInitialParticles();
3030 bool create_system_noise();
3131 int dump_predict_particles();
32+ Particles<double> get_particles();
3233 string predict_particles_toString();
3334 string system_noise_toString();
3435 bool set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v));
Show on old repository browser