OSDN -- 开发和下载开源软件
简体中文翻译状态 translation status for zh
  • R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版28 (tree)
时间2010-10-28 20:12:44
作者(del#42041)

Log Message

added unit test cases, some methods

更改概述

差异

--- test/test_particle.cc (revision 27)
+++ test/test_particle.cc (revision 28)
@@ -15,21 +15,57 @@
1515
1616 void test_construction(void)
1717 {
18- // Constructor
1918 Particle<int> a(5);
20- vector<int> p = a.get_particle();
21- cppcut_assert_equal(5, (int)p.size());
19+ for(int i=1;i<=5;++i) a[i-1]=i;
20+ cut_assert_equal_int(5,a.size());
21+ cut_assert_equal_string("(1,2,3,4,5)",a.toString().c_str());
2222
23- // Copy constructor
24- vector<int> b(5);
25- for(int i=1;i<5;++i) b.at(i-1)=i;
26- a = b;
27- for(int i=1;i<5;++i) cut_assert_equal_int(i, a[i-1]);
2823 Particle<int> c(5);
29- cut_assert_equal_int(5,c.size());
3024 cut_assert_equal_string("(0,0,0,0,0)",c.toString().c_str());
3125 for(int i=1;i<5;++i) cut_assert_equal_int(i, a[i-1]);
3226 }
27+ void test_toVector(void)
28+ {
29+ Particle<int> a(5);
30+ for(int i=1;i<=5;++i) a[i-1]=i;
31+
32+ vector<int> p = a.toVector();
33+ cppcut_assert_equal(5, (int)p.size());
34+ for(int i=1;i<=5;++i) cut_assert_equal_int(i, p.at(i-1));
35+ }
36+ void test_plus_equal(void)
37+ {
38+ Particle<int> a(5), b(5);
39+ for(int i=1;i<=5;++i)
40+ {
41+ a[i-1]=i; // 1, 2, 3, 4, 5
42+ b[i-1]=2-i; // 1, 0, -1, -2, -3
43+ }
44+ a += b;
45+ for(int i=0;i<4;++i) cut_assert_equal_int(2, a[i]);
46+
47+ }
48+ void test_plus(void)
49+ {
50+ Particle<int> a(5), b(5);
51+ for(int i=1;i<=5;++i)
52+ {
53+ a[i-1]=i; // 1, 2, 3, 4, 5
54+ b[i-1]=2-i; // 1, 0, -1, -2, -3
55+ }
56+ Particle<int> c = a + b;
57+ for(int i=0;i<4;++i) cut_assert_equal_int(2, c[i]);
58+
59+ }
60+ void test_copyConstructor(void)
61+ {
62+ Particle<int> a(5);
63+ for(int i=1;i<=5;++i) a[i-1]=i;
64+
65+ Particle<int> b;
66+ b = a;
67+ for(int i=1;i<=5;++i) cut_assert_equal_int(i, b[i-1]);
68+ }
3369 void test_MersennneTwister(void)
3470 {
3571 MTRand r;
--- test/test_filter.cc (revision 27)
+++ test/test_filter.cc (revision 28)
@@ -49,7 +49,7 @@
4949 a.setNumber(5);
5050 a.setDimension(3);
5151 a.createInitialParticles();
52- cut_assert_equal_string("(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)",a.predict_particles_toString().c_str());
52+ cut_assert_equal_string("(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)",a.get_predict_particles().toString().c_str());
5353 // a.dump_predict_particles();
5454 }
5555 void test_manipulation(void)
@@ -66,12 +66,17 @@
6666 a.create_system_noise();
6767
6868 // initial value
69- cut_assert_equal_string("(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)",a.predict_particles_toString().c_str());
70- cut_assert_equal_string("(1.163078,0.483805,0.299564)(0.153025,-1.168815,1.558071)(-0.545944,-2.355630,0.541440)(2.678507,1.254634,-0.548774)(-0.681064,-0.135316,0.377231)",a.system_noise_toString().c_str());
69+ cut_assert_equal_string("(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)",a.get_predict_particles().toString().c_str());
70+ cut_assert_not_equal_string("(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)(0.000000,0.000000,0.000000)",a.get_system_noise().toString().c_str());
71+
72+ Particles<double> noise(a.get_system_noise());
73+ Particles<double> prediction(a.get_predict_particles());
74+
7175 a.get_next_state();
72-
76+ Particles<double> predict = prediction + noise;
7377 // initial value+system noise
74- cut_assert_equal_string("(1.163078,0.483805,0.299564)(0.153025,-1.168815,1.558071)(-0.545944,-2.355630,0.541440)(2.678507,1.254634,-0.548774)(-0.681064,-0.135316,0.377231)",a.predict_particles_toString().c_str());
78+ cut_assert_equal_string(predict.toString().c_str(),a.get_predict_particles().toString().c_str());
79+cout<<"predict1:"<<predict.toString()<<endl;
7580 Particles<double> y(5,3);
7681 Particle<double> x(3);
7782 x[0]=0.1; x[1]=-1.1; x[2]=1.5;
@@ -79,30 +84,54 @@
7984 {
8085 y[i] = x;
8186 }
87+cout<<"observed1:"<<y.toString()<<endl;
8288 a.set_observed_data(y);
83- cut_assert_equal_double(-1.06308,1e-4,a.get_observed_noise(0));
84- cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308));
89+ double diff = x[0]-predict[0][0];
90+ cut_assert_equal_double(diff ,1e-4,a.get_observed_noise(0));
91+ cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(diff));
8592 a.compute_likelihood();
8693 a.resampling();
87- 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());
94+cout<<"resampled1:"<<a.get_predict_particles().toString()<<endl;
95+ for(int j=0;j<a.get_predict_particles().size();++j)
96+ {
97+ for(int i=0;i<x.size();++i)
98+ {
99+ cut_assert_equal_double(x[i],10.0, a.get_predict_particles()[j][i]);
100+ }
101+ }
88102
89103 // step 2
90104
91105 a.create_system_noise();
92106 a.get_next_state();
93- cut_assert_equal_string("(0.563190,-0.597547,-1.199891)(1.229306,-1.782948,3.388836)(-0.993781,-1.114977,-0.949410)(-0.438624,-0.310210,1.330129)(0.354340,-0.818760,2.094123)",a.predict_particles_toString().c_str());
107+cout<<"predict2:"<<a.get_predict_particles().toString()<<endl;
108+ for(int j=0;j<a.get_predict_particles().size();++j)
109+ {
110+ for(int i=0;i<x.size();++i)
111+ {
112+ cut_assert_equal_double(x[i],10.0, a.get_predict_particles()[j][i]);
113+ }
114+ }
94115 x[0]=0.2; x[1]=-1.0; x[2]=1.5;
95116 for(int i=0;i<y.size();++i)
96117 {
97118 y[i] = x;
98119 }
120+cout<<"observed2:"<<y.toString()<<endl;
99121 a.set_observed_data(y);
100- cut_assert_equal_double(-0.36319,1e-4,a.get_observed_noise(0));
122+ cut_assert_equal_double(-0.36319,1.0,a.get_observed_noise(0));
101123 a.set_robserve_density_func(func4);
102124 cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308));
103125 a.compute_likelihood();
104126 a.resampling();
105- cut_assert_equal_string("(0.563190,-0.597547,-1.199891)(0.563190,-0.597547,-1.199891)(0.563190,-0.597547,-1.199891)(0.563190,-0.597547,-1.199891)(0.563190,-0.597547,-1.199891)",a.predict_particles_toString().c_str());
127+cout<<"resampled2:"<<a.get_predict_particles().toString()<<endl;
128+ for(int j=0;j<a.get_predict_particles().size();++j)
129+ {
130+ for(int i=0;i<x.size();++i)
131+ {
132+ cut_assert_equal_double(x[i],10.0, a.get_predict_particles()[j][i]);
133+ }
134+ }
106135
107136 }
108137 }
--- test/test_particles.cc (revision 27)
+++ test/test_particles.cc (revision 28)
@@ -15,13 +15,102 @@
1515 void test_construction(void)
1616 {
1717 Particles<int> a(3);
18- vector<Particle<int> > p = a.get_particles();
19- cppcut_assert_equal(3, (int)p.size());
20- cppcut_assert_equal(3, a.size());
21- Particles<int> b(3,2);
18+ Particle<int> dummy(5);
19+ for(int j=1;j<=3;++j)
20+ {
21+ for(int i=1;i<=5;++i)
22+ {
23+ dummy[i-1]=j*10+i;
24+ }
25+ a[j-1] = dummy;
26+ }
27+ for(int j=1;j<=3;++j)
28+ {
29+ for(int i=1;i<=5;++i)
30+ {
31+ cppcut_assert_equal(j*10+i, a[j-1][i-1]);
32+ }
33+ }
34+
35+ Particles<int> b(3,2); // (number, dimension)
2236 for(int i=0;i<3;++i)
2337 cut_assert_equal_string("(0,0)", b[i].toString().c_str());
2438 }
39+ void test_copyConstructor(void)
40+ {
41+ Particles<int> a(3);
42+ Particle<int> dummy(5);
43+ for(int j=1;j<=3;++j)
44+ {
45+ for(int i=1;i<=5;++i)
46+ {
47+ dummy[i-1]=j*10+i;
48+ }
49+ a[j-1] = dummy;
50+ }
51+ Particles<int> b;
52+ b = a;
53+ for(int j=1;j<=3;++j)
54+ {
55+ for(int i=1;i<=5;++i)
56+ {
57+ cppcut_assert_equal(j*10+i, b[j-1][i-1]);
58+ }
59+ }
60+ }
61+ void test_plus_equal(void)
62+ {
63+ Particles<int> a(3), b(3);
64+ Particle<int> dummy(5);
65+ for(int j=1;j<=3;++j)
66+ {
67+ for(int i=1;i<=5;++i)
68+ {
69+ dummy[i-1]=j*10+i; // 11, 12, 13, 14, 15, 21, 22, ... 55
70+ }
71+ a[j-1] = dummy;
72+ for(int i=1;i<=5;++i)
73+ {
74+ dummy[i-1]=j*20+i; // 21, 22, 23, 24, 25, 31, 32, ... 65
75+ }
76+ b[j-1] = dummy;
77+ }
78+ b += a;
79+ for(int j=1;j<=3;++j)
80+ {
81+ for(int i=1;i<=5;++i)
82+ {
83+ cppcut_assert_equal(j*30+2*i, b[j-1][i-1]);
84+ }
85+ }
86+ }
87+ void test_plus(void)
88+ {
89+ Particles<int> a(3), b(3);
90+ Particle<int> dummy(5);
91+ for(int j=1;j<=3;++j)
92+ {
93+ for(int i=1;i<=5;++i)
94+ {
95+ dummy[i-1]=j*10+i; // 11, 12, 13, 14, 15, 21, 22, ... 55
96+ }
97+ a[j-1] = dummy;
98+ for(int i=1;i<=5;++i)
99+ {
100+ dummy[i-1]=j*20+i; // 21, 22, 23, 24, 25, 31, 32, ... 65
101+ }
102+ b[j-1] = dummy;
103+ }
104+ Particles<int> c = a+b;
105+ for(int j=1;j<=3;++j)
106+ {
107+ for(int i=1;i<=5;++i)
108+ {
109+ cppcut_assert_equal(j*30+2*i, c[j-1][i-1]);
110+ }
111+ }
112+
113+ }
25114 void test_manipulate(void)
26115 {
27116 Particles<int> b(3,2);
--- sample/Makefile.in (revision 27)
+++ sample/Makefile.in (revision 28)
@@ -1,6 +1,6 @@
11 CC=@CC@
22 CXX=@CXX@
3-SRCS=step.cc
3+SRCS=step.cc stdin.cc
44 OBJS=$(SRCS:.cc=.o)
55 HEADS=$(SRCS:.cc=.h)
66 TARGETS=$(SRCS:.cc=)
@@ -11,6 +11,8 @@
1111
1212 step: $(OBJS)
1313 $(CXX) -o $@ $@.o $(LDFLAGS)
14+stdin: $(OBJS)
15+ $(CXX) -o $@ $@.o $(LDFLAGS)
1416
1517 .cc.o: $(HEADS) $(SRCS)
1618 $(CXX) $(CFLAGS) -c $<
--- sample/stdin.cc (nonexistent)
+++ sample/stdin.cc (revision 28)
@@ -0,0 +1,117 @@
1+#include <MersenneTwister.h>
2+#include <filter.h>
3+#include <iostream>
4+using namespace std;
5+Particles<double> func1(Particles<double> &p, Particles<double> &v)
6+{
7+ for(int i=0;i<p.size();++i)
8+ {
9+ for(int j=0;j<p[i].size();++j)
10+ {
11+//cout<< p[i][j]<<"+"<<v[i][j]<<endl;
12+ p[i][j]= p[i][j]+v[i][j];
13+ }
14+ }
15+// cout<<"state function"<<endl;
16+ return(p);
17+}
18+double func2(Particles<double> &p, Particle<double> &q)
19+{
20+
21+// cout<<"robserve function"<<endl;
22+// cout<<"noise="<<(p[0][0]-q[0])<<endl;
23+// return(p[0][0]-q[0]);
24+ double err = 0.0, n=0.0;
25+ for(int i=0;i<p.size();++i)
26+ {
27+ for(int j=0;j<p[i].size();++j)
28+ {
29+ err += p[i][j] - q[j];
30+ n += 1.0;
31+ }
32+ }
33+ return(err/n);
34+}
35+double func3(Particles<double> &p, Particle<double> &q)
36+{
37+// cout<<"robserve jacobian function"<<endl;
38+ return(1.0);
39+}
40+double func4(double w)
41+{
42+// cout<<"robserve density function"<<endl;
43+ double rho = 5.0;
44+ double r = (1.0/sqrt(2.0*M_PI*rho*rho)*exp(-w*w/(2.0*rho*rho)));
45+ return(r);
46+}
47+
48+int main()
49+{
50+ Filter track;
51+ MTRand mtrand;
52+#define NUM 1000
53+#define MAX 10000000
54+ mtrand.seed(time(NULL));
55+ track.setNumber(NUM);
56+ track.setDimension(1);
57+ track.set_state_func(&func1);
58+ track.set_robserve_func(&func2);
59+ track.set_robserve_jacobian_func(&func3);
60+ track.set_robserve_density_func(&func4);
61+ track.createInitialParticles(117398192/MAX);
62+ double t;
63+ Particles<double> x(NUM,1), y(NUM,1);
64+ for(int i=0;i<28;++i)
65+ {
66+ track.create_system_noise();
67+ track.get_next_state(); // predict
68+ x = track.get_particles();
69+ double xp_=0.0;
70+ for(int j=0;j<x.size();++j)
71+ {
72+ xp_ += x[j][0];
73+ }
74+cout<<xp_/x.size()<<endl;
75+
76+// t = i<25?1:i<50?0:i<75?-1:1;
77+ cin>>t;
78+// cout<<"["<<fixed<<t<<"]"<<endl;
79+ t /= MAX;
80+ for(int j=0;j<x.size();++j)
81+ {
82+ y[j][0] = t + (mtrand.rand()-0.5)*8.0;
83+ }
84+ double y_=0.0;
85+ for(int j=0;j<x.size();++j)
86+ {
87+ y_ += y[j][0];
88+ }
89+//cout<<"observed="<<y_/x.size()<<endl;
90+//cout<<y_/x.size()<<endl;
91+
92+//cout<<y[0][0]<<endl;
93+ track.set_observed_data(y);
94+ track.compute_likelihood();
95+ x = track.get_particles();
96+ for(int j=0;j<x.size();++j)
97+ {
98+// cout<<"["<<j<<"]"<<x[j][0]<<endl;
99+ }
100+ track.resampling();
101+//cout<<"--resampled--"<<endl;
102+ x = track.get_particles();
103+ for(int j=0;j<x.size();++j)
104+ {
105+// cout<<"["<<j<<"]"<<x[j][0]<<endl;
106+ }
107+//cout<<endl;
108+//cout<<endl;
109+
110+ double x_=0.0;
111+ for(int j=0;j<x.size();++j)
112+ {
113+ x_ += x[j][0];
114+ }
115+//cout<<x_/x.size()<<endl;
116+ }
117+}
\ No newline at end of file
Added: svn:mergeinfo
## -0,0 +0,0 ##
--- sample/stdin.h (nonexistent)
+++ sample/stdin.h (revision 28)
Added: svn:mergeinfo
## -0,0 +0,0 ##
--- src/particle.cc (revision 27)
+++ src/particle.cc (revision 28)
@@ -67,6 +67,12 @@
6767 this->dimension = n;
6868 return(0);
6969 }
70+template<class C> int Particle<C>::resize(int n, C a0)
71+{
72+ p.resize(n, a0);
73+ this->dimension = n;
74+ return(0);
75+}
7076 template<class C> int Particle<C>::size()
7177 {
7278 return(dimension);
--- src/filter.cc (revision 27)
+++ src/filter.cc (revision 28)
@@ -38,8 +38,6 @@
3838 bool Filter::createInitialParticles()
3939 {
4040 if(number<0||dimension<0) return(false);
41-// x = new Particles(number, dimension);
42-// v = new Particles(number, dimension);
4341 x.resize(number, dimension);
4442 v.resize(number, dimension);
4543 return(true);
@@ -47,9 +45,7 @@
4745 bool Filter::createInitialParticles(double d)
4846 {
4947 if(number<0||dimension<0) return(false);
50-// x = new Particles(number, dimension);
51-// v = new Particles(number, dimension);
52- x.resize(number, dimension);
48+ x.resize(number, dimension, d);
5349 v.resize(number, dimension);
5450 return(true);
5551 }
@@ -125,7 +121,7 @@
125121 {
126122 for(unsigned int i=0;i<alpha.size();++i)
127123 {
128-//cout<<"y="<<y.toString()<<" x="<<x[i].toString()<<endl;
124+//cout<<"y="<<y[0].toString()<<" x="<<x[i].toString()<<endl;
129125 //cout<<"alpha="<<(*robserve_density_func)((*robserve_func)(y, x[i]))
130126 // <<"*"<<(*robserve_jacobian_func)(y, x[i])<<endl;
131127 alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]))
--- src/particle.h (revision 27)
+++ src/particle.h (revision 28)
@@ -18,6 +18,7 @@
1818 Particle(const Particle &f);
1919 Particle& operator=(const Particle &f);
2020 int resize(int n);
21+ int resize(int n, C a0);
2122 int size();
2223 vector<C> toVector();
2324 string toString();
--- src/particles.cc (revision 27)
+++ src/particles.cc (revision 28)
@@ -67,6 +67,20 @@
6767 }
6868 return(true);
6969 }
70+template<class C> bool Particles<C>::resize(int number, int dimension, C a0)
71+{
72+ p.resize(number);
73+ this->number = number;
74+ typedef Particle<C> ParticleC;
75+// for(vector<ParticleC>::iterator i= p.begin();
76+// i!=p.end();++i)
77+ for(unsigned int i=0;i<p.size();++i)
78+ {
79+// i->resize(dimension);
80+ p[i].resize(dimension, a0);
81+ }
82+ return(true);
83+}
7084 template<class C> int Particles<C>::size()
7185 {
7286 return(number);
--- src/particles.h (revision 27)
+++ src/particles.h (revision 28)
@@ -19,6 +19,7 @@
1919 Particles<C> operator+(Particles<C> &a);
2020 ~Particles();
2121 bool resize(int number, int dimension);
22+ bool resize(int number, int dimension, C a0);
2223 int size();
2324 Particle<C> &operator[](unsigned int i);
2425 Particle<C> operator[](unsigned int i) const;
Show on old repository browser