• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版25 (tree)
时间2010-06-04 21:46:35
作者(del#42041)

Log Message

sample not work fine

更改概述

差异

--- sample/step.cc (revision 24)
+++ sample/step.cc (revision 25)
@@ -16,6 +16,7 @@
1616 {
1717
1818 // cout<<"robserve function"<<endl;
19+// cout<<"noise="<<(p[0][0]-q[0])<<endl;
1920 return(p[0][0]-q[0]);
2021 }
2122 double func3(Particles<double> &p, Particle<double> &q)
@@ -26,7 +27,9 @@
2627 double func4(double w)
2728 {
2829 // cout<<"robserve density function"<<endl;
29- return(1.0/sqrt(2.0*M_PI*0.1*0.1)*exp(-w*w/(2.0*0.1*0.1)));
30+ double rho = 0.08;
31+ double r = (1.0/sqrt(2.0*M_PI*rho*rho)*exp(-w*w/(2.0*rho*rho)));
32+ return(r);
3033 }
3134
3235 int main()
@@ -33,8 +36,9 @@
3336 {
3437 Filter track;
3538 MTRand mtrand;
39+#define NUM 5
3640 mtrand.seed(time(NULL));
37- track.setNumber(50);
41+ track.setNumber(NUM);
3842 track.setDimension(1);
3943 track.set_state_func(&func1);
4044 track.set_robserve_func(&func2);
@@ -42,23 +46,40 @@
4246 track.set_robserve_density_func(&func4);
4347 track.createInitialParticles();
4448 double t;
45- Particles<double> x(50,1), y(50,1);
49+ Particles<double> x(NUM,1), y(NUM,1);
4650 for(int i=0;i<100;++i)
4751 {
4852 track.create_system_noise();
4953 track.get_next_state(); // predict
5054 t = i<25?1:i<50?0:i<75?-1:1;
51- y[0][0] = t + mtrand.rand();
55+ for(int j=0;j<x.size();++j)
56+ {
57+ y[j][0] = t + mtrand.rand()*0.00;
58+ }
59+
5260 //cout<<y[0][0]<<endl;
5361 track.set_observed_data(y);
5462 track.compute_likelihood();
63+ x = track.get_particles();
64+ for(int j=0;j<x.size();++j)
65+ {
66+ cout<<x[j][0]<<endl;
67+ }
5568 track.resampling();
69+cout<<endl;
5670 x = track.get_particles();
71+ for(int j=0;j<x.size();++j)
72+ {
73+ cout<<x[j][0]<<endl;
74+ }
75+cout<<endl;
76+cout<<endl;
77+
5778 double x_=0.0;
5879 for(int j=0;j<x.size();++j)
5980 {
6081 x_ += x[j][0];
6182 }
62-cout<<x_/x.size()<<endl;
83+cout<<x_/x.size()<<endl<<endl;
6384 }
6485 }
\ No newline at end of file
--- src/filter.cc (revision 24)
+++ src/filter.cc (revision 25)
@@ -50,7 +50,7 @@
5050 {
5151 for(int j=0;j<v[i].size();++j)
5252 {
53- v[i][j] = mtrand.randNorm();
53+ v[i][j] = (mtrand.randNorm()-0.5)*0.8; // ToDO: functionize
5454 }
5555 }
5656 return(true);
@@ -116,6 +116,9 @@
116116 {
117117 for(unsigned int i=0;i<alpha.size();++i)
118118 {
119+//cout<<"y="<<y.toString()<<" x="<<x[i].toString()<<endl;
120+//cout<<"alpha="<<(*robserve_density_func)((*robserve_func)(y, x[i]))
121+// <<"*"<<(*robserve_jacobian_func)(y, x[i])<<endl;
119122 alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]))
120123 *(*robserve_jacobian_func)(y, x[i]);
121124 }
@@ -130,29 +133,40 @@
130133 double alphasum = 0.0;
131134 for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i)
132135 {
136+cout<<"alpha="<<(*i)<<endl;
133137 alphasum += (*i);
134138 }
139+//cout<<"alpha sum="<<alphasum<<endl;
140+ if(alphasum*alphasum<DBL_EPSILON)
141+ cout<<"Regularize Constant is too small="<<alphasum<<endl;
135142 double m = (double)x.size();
136143 Particles<double> f(number, dimension);
137144 for(int j=0;j<x.size();++j)
138145 {
139- double u = ((j+1)-0.5)/m, a1=0.0, a2=0.0;
146+ double u = ((j+1)-0.5)/m;
147+cout<<"u="<<u<<endl;
140148 int sample_i = 0;
141- for(int i=1;i<((int)m-2);++i)
149+// for(int i=1;i<((int)m-2);++i)
150+ for(int i=1;i<((int)m);++i)
142151 {
143152 int l;
153+ double a1=0.0, a2=0.0;
144154 for(l=0;l<i;++l)
145155 {
146156 a1+=alpha[l];
147157 }
148- a2 = a1 + alpha[l];
149-//cout<<a1/alphasum<<"<"<<u<<"<"<<a2/alphasum<<"?"<<endl;
150- if(a1/alphasum<u && a2/alphasum>=u)
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;
163+ if((a1/alphasum<u) && (a2/alphasum>=u))
151164 {
152- sample_i = i;
165+ sample_i = i ;
153166 break;
154167 }
155168 }
169+cout<<"sample="<<sample_i<<endl;
156170 f[j] = x[sample_i];
157171 }
158172 x = f;
--- src/filter.h (revision 24)
+++ src/filter.h (revision 25)
@@ -6,6 +6,7 @@
66 #include <MersenneTwister.h>
77 #include <ctime>
88 #include <cmath>
9+#include <cfloat>
910 using namespace std;
1011
1112 class Filter
Show on old repository browser