• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版16 (tree)
时间2010-05-07 11:35:51
作者tullio

Log Message

improved test code of filter

更改概述

差异

--- test/test_filter.cc (revision 15)
+++ test/test_filter.cc (revision 16)
@@ -16,12 +16,17 @@
1616 cout<<"state function"<<endl;
1717 return(p);
1818 }
19-double func2(Particles<double> &p, Particles<double> &q)
19+double func2(Particles<double> &p, Particle<double> &q)
2020 {
2121
2222 cout<<"robserve function"<<endl;
23- return(p[0][0]-q[0][0]);
23+ return(p[0][0]-q[0]);
2424 }
25+double func3(Particles<double> &p, Particles<double> &q)
26+{
27+ cout<<"robserve jacobian function"<<endl;
28+ return(1.0);
29+}
2530
2631 namespace test_filter
2732 {
@@ -35,25 +40,27 @@
3540 void test_construction(void)
3641 {
3742 // Constructor
38- Filter a;
43+ Filter a(0.0);
3944 a.setNumber(5);
4045 a.setDimension(3);
4146 a.createInitialParticles();
42- a.dump_predict_particles();
47+ 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());
48+// a.dump_predict_particles();
4349 }
4450 void test_manipulation(void)
4551 {
46- Filter a;
52+ Filter a(0.0);
4753 a.setNumber(5);
4854 a.setDimension(3);
4955 a.createInitialParticles();
5056 a.createSystemNoise();
51- a.dump_predict_particles();
52- a.dump_System_Noise();
57+ 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());
58+ 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());
5359 a.set_state_func(&func1);
5460 a.get_next_state();
55- a.dump_predict_particles();
61+ 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());
5662 a.set_robserve_func(&func2);
63+ a.set_robserve_jacobian_func(&func3);
5764 Particles<double> y(5,3);
5865 Particle<double> x(3);
5966 x[0]=1.0; x[1]=2.0; x[2]=3.0;
@@ -62,7 +69,7 @@
6269 y[i] = x;
6370 }
6471 a.set_observed_data(y);
65- cout<<a.get_observed_noise()<<endl;
72+ cut_assert_equal_double(-0.163078,1e-5,a.get_observed_noise(0));
6673 }
6774 }
6875 int main()
--- src/filter.cc (revision 15)
+++ src/filter.cc (revision 16)
@@ -9,6 +9,15 @@
99 // v = NULL;
1010 mtrand.seed(time(NULL));
1111 }
12+Filter::Filter(double seed)
13+{
14+ number = -1;
15+ dimension = -1;
16+ state_func = NULL;
17+// x = NULL;
18+// v = NULL;
19+ mtrand.seed(seed);
20+}
1221 Filter::~Filter()
1322 {
1423 // if(x) delete x;
@@ -18,6 +27,7 @@
1827 bool Filter::setNumber(int n)
1928 {
2029 number = n;
30+ alpha.resize(n);
2131 return(true);
2232 }
2333 bool Filter::setDimension(int d)
@@ -55,27 +65,58 @@
5565 v.dump_particles();
5666 return(0);
5767 }
68+string Filter::predict_particles_toString()
69+{
70+ return(x.toString());
71+}
72+string Filter::system_noise_toString()
73+{
74+ return(v.toString());
75+}
5876 bool Filter::set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v))
5977 {
6078 state_func = func;
6179 return(true);
6280 }
63-bool Filter::set_robserve_func(double (*func)(Particles<double> &p, Particles<double> &q))
81+bool Filter::set_robserve_func(double (*func)(Particles<double> &p, Particle<double> &q))
6482 {
6583 robserve_func = func;
6684 return(true);
6785 }
86+bool Filter::set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particles<double> &q))
87+{
88+ robserve_jacobian_func = func;
89+ return(true);
90+}
91+bool Filter::set_robserve_density_func(double (*func)(double w))
92+{
93+ robserve_density_func = func;
94+ return(true);
95+}
6896 Filter & Filter::get_next_state()
6997 {
7098 (*state_func)(x, v);
7199 return(*this);
72100 }
73-double Filter::get_observed_noise()
101+double Filter::get_observed_noise(int i)
74102 {
75- return((*robserve_func)(y, x));
103+ return((*robserve_func)(y, x[i]));
76104 }
105+double Filter::get_robserved_density_value(double w)
106+{
107+ return((*robserve_density_func)(w));
108+}
109+
77110 bool Filter::set_observed_data(Particles<double> &p)
78111 {
79112 y = p;
80113 return(true);
81114 }
115+bool Filter::compute_likelihood()
116+{
117+ for(unsigned int i=0;i<alpha.size();++i)
118+ {
119+ alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]));
120+ }
121+ return(true);
122+}
--- src/particles.cc (revision 15)
+++ src/particles.cc (revision 16)
@@ -56,6 +56,15 @@
5656 }
5757 return(0);
5858 }
59+template<class C> string Particles<C>::toString()
60+{
61+ ostringstream s;
62+ for(unsigned int i=0;i<p.size();++i)
63+ {
64+ s<<p[i].toString();
65+ }
66+ return(s.str());
67+}
5968
6069 template class Particles<int>;
6170 template class Particles<double>;
--- src/filter.h (revision 15)
+++ src/filter.h (revision 16)
@@ -11,13 +11,17 @@
1111 {
1212 Particles<double> x, y;
1313 Particles<double> v;
14+ vector<double> alpha;
1415 double w;
1516 MTRand mtrand;
1617 int number, dimension;
1718 Particles<double> (*state_func)(Particles<double> &p, Particles<double> &v);
18- double (*robserve_func)(Particles<double> &p, Particles<double> &q);
19+ double (*robserve_func)(Particles<double> &p, Particle<double> &q);
20+ double (*robserve_jacobian_func)(Particles<double> &p, Particles<double> &q);
21+ double (*robserve_density_func)(double w);
1922 public:
2023 Filter();
24+ Filter(double seed);
2125 ~Filter();
2226 bool setNumber(int n);
2327 bool setDimension(int d);
@@ -24,12 +28,18 @@
2428 bool createInitialParticles();
2529 bool createSystemNoise();
2630 int dump_predict_particles();
31+ string predict_particles_toString();
32+ string system_noise_toString();
2733 bool set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v));
28- bool set_robserve_func(double (*func)(Particles<double> &p, Particles<double> &q));
34+ bool set_robserve_func(double (*func)(Particles<double> &p, Particle<double> &q));
35+ bool set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particles<double> &q));
36+ bool set_robserve_density_func(double (*func)(double w));
2937 Filter & get_next_state();
30- double get_observed_noise();
38+ double get_observed_noise(int i);
39+ double get_robserved_density_value(double w);
3140 int dump_System_Noise();
3241 bool set_observed_data(Particles<double> &p);
42+ bool compute_likelihood();
3343 };
3444
3545 #endif
--- src/particles.h (revision 15)
+++ src/particles.h (revision 16)
@@ -20,6 +20,7 @@
2020 Particle<C> operator[](unsigned int i) const;
2121 vector<Particle<C> > get_particles();
2222 int dump_particles();
23+ string toString();
2324 };
2425
2526
Show on old repository browser