• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版26 (tree)
时间2010-06-08 19:48:37
作者(del#42041)

Log Message

now sample/step.cc works fine

更改概述

差异

--- sample/step.cc (revision 25)
+++ sample/step.cc (revision 26)
@@ -6,6 +6,7 @@
66 {
77 for(int j=0;j<p[i].size();++j)
88 {
9+//cout<< p[i][j]<<"+"<<v[i][j]<<endl;
910 p[i][j]= p[i][j]+v[i][j];
1011 }
1112 }
@@ -17,7 +18,17 @@
1718
1819 // cout<<"robserve function"<<endl;
1920 // cout<<"noise="<<(p[0][0]-q[0])<<endl;
20- return(p[0][0]-q[0]);
21+// return(p[0][0]-q[0]);
22+ double err = 0.0, n=0.0;
23+ for(int i=0;i<p.size();++i)
24+ {
25+ for(int j=0;j<p[i].size();++j)
26+ {
27+ err += p[i][j] - q[j];
28+ n += 1.0;
29+ }
30+ }
31+ return(err/n);
2132 }
2233 double func3(Particles<double> &p, Particle<double> &q)
2334 {
@@ -27,7 +38,7 @@
2738 double func4(double w)
2839 {
2940 // cout<<"robserve density function"<<endl;
30- double rho = 0.08;
41+ double rho = 1.51;
3142 double r = (1.0/sqrt(2.0*M_PI*rho*rho)*exp(-w*w/(2.0*rho*rho)));
3243 return(r);
3344 }
@@ -36,7 +47,7 @@
3647 {
3748 Filter track;
3849 MTRand mtrand;
39-#define NUM 5
50+#define NUM 1000
4051 mtrand.seed(time(NULL));
4152 track.setNumber(NUM);
4253 track.setDimension(1);
@@ -51,11 +62,26 @@
5162 {
5263 track.create_system_noise();
5364 track.get_next_state(); // predict
65+ x = track.get_particles();
66+ double xp_=0.0;
67+ for(int j=0;j<x.size();++j)
68+ {
69+ xp_ += x[j][0];
70+ }
71+//cout<<xp_/x.size()<<endl;
72+
5473 t = i<25?1:i<50?0:i<75?-1:1;
5574 for(int j=0;j<x.size();++j)
5675 {
57- y[j][0] = t + mtrand.rand()*0.00;
76+ y[j][0] = t + (mtrand.rand()-0.5)*8.0;
5877 }
78+ double y_=0.0;
79+ for(int j=0;j<x.size();++j)
80+ {
81+ y_ += y[j][0];
82+ }
83+//cout<<"observed="<<y_/x.size()<<endl;
84+//cout<<y_/x.size()<<endl;
5985
6086 //cout<<y[0][0]<<endl;
6187 track.set_observed_data(y);
@@ -63,17 +89,17 @@
6389 x = track.get_particles();
6490 for(int j=0;j<x.size();++j)
6591 {
66- cout<<x[j][0]<<endl;
92+// cout<<"["<<j<<"]"<<x[j][0]<<endl;
6793 }
6894 track.resampling();
69-cout<<endl;
95+//cout<<"--resampled--"<<endl;
7096 x = track.get_particles();
7197 for(int j=0;j<x.size();++j)
7298 {
73- cout<<x[j][0]<<endl;
99+// cout<<"["<<j<<"]"<<x[j][0]<<endl;
74100 }
75-cout<<endl;
76-cout<<endl;
101+//cout<<endl;
102+//cout<<endl;
77103
78104 double x_=0.0;
79105 for(int j=0;j<x.size();++j)
@@ -80,6 +106,6 @@
80106 {
81107 x_ += x[j][0];
82108 }
83-cout<<x_/x.size()<<endl<<endl;
109+cout<<x_/x.size()<<endl;
84110 }
85111 }
\ No newline at end of file
--- src/filter.cc (revision 25)
+++ src/filter.cc (revision 26)
@@ -50,7 +50,8 @@
5050 {
5151 for(int j=0;j<v[i].size();++j)
5252 {
53- v[i][j] = (mtrand.randNorm()-0.5)*0.8; // ToDO: functionize
53+ v[i][j] = mtrand.randNorm(0.0, 1.2); // ToDO: functionize
54+//cout<<"noise="<<v[i][j]<<endl;
5455 }
5556 }
5657 return(true);
@@ -131,9 +132,11 @@
131132 bool Filter::resampling()
132133 {
133134 double alphasum = 0.0;
135+ int k=0;
134136 for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i)
135137 {
136-cout<<"alpha="<<(*i)<<endl;
138+//cout<<"alpha["<<k<<"]="<<(*i)<<endl;
139+ ++k;
137140 alphasum += (*i);
138141 }
139142 //cout<<"alpha sum="<<alphasum<<endl;
@@ -141,33 +144,44 @@
141144 cout<<"Regularize Constant is too small="<<alphasum<<endl;
142145 double m = (double)x.size();
143146 Particles<double> f(number, dimension);
147+ vector<double> psuma(x.size() + 1);
148+ psuma[0] = 0.0;
149+ for(int i=1;i<x.size();++i)
150+ {
151+ int l;
152+ psuma[i]=0.0;
153+ for(l=0;l<i;++l)
154+ {
155+ psuma[i]+=alpha[l];
156+ }
157+ }
158+ psuma[x.size()-1] = psuma[x.size()-2]+alpha[x.size()-1];
144159 for(int j=0;j<x.size();++j)
145160 {
146161 double u = ((j+1)-0.5)/m;
147-cout<<"u="<<u<<endl;
148- int sample_i = 0;
162+//cout<<"u="<<u<<endl;
163+ int sample_i = -1;
149164 // for(int i=1;i<((int)m-2);++i)
150- for(int i=1;i<((int)m);++i)
165+ for(int i=1;i<=x.size();++i)
151166 {
152- int l;
153167 double a1=0.0, a2=0.0;
154- for(l=0;l<i;++l)
155- {
156- a1+=alpha[l];
157- }
158- a2 = a1 + alpha[i];
159-cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl;
160-cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl;
161-cout<<(u<=a2/alphasum)<<"?"<<endl;
162-cout<<(a1/alphasum<u)<<endl;
168+ a1 = psuma[i-1];
169+ a2 = psuma[i];
170+//cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl;
171+//cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl;
172+//cout<<(u<=a2/alphasum)<<"?"<<endl;
173+//cout<<(a1/alphasum<u)<<endl;
163174 if((a1/alphasum<u) && (a2/alphasum>=u))
164175 {
165- sample_i = i ;
176+ sample_i = i - 1;
166177 break;
167178 }
168179 }
169-cout<<"sample="<<sample_i<<endl;
170- f[j] = x[sample_i];
180+//cout<<"sample="<<(sample_i+0)<<endl;
181+ if(sample_i>=0)
182+ {
183+ f[j] = x[sample_i+0];
184+ }
171185 }
172186 x = f;
173187 return(true);
Show on old repository browser