improved test code of filter
@@ -16,12 +16,17 @@ | ||
16 | 16 | cout<<"state function"<<endl; |
17 | 17 | return(p); |
18 | 18 | } |
19 | -double func2(Particles<double> &p, Particles<double> &q) | |
19 | +double func2(Particles<double> &p, Particle<double> &q) | |
20 | 20 | { |
21 | 21 | |
22 | 22 | cout<<"robserve function"<<endl; |
23 | - return(p[0][0]-q[0][0]); | |
23 | + return(p[0][0]-q[0]); | |
24 | 24 | } |
25 | +double func3(Particles<double> &p, Particles<double> &q) | |
26 | +{ | |
27 | + cout<<"robserve jacobian function"<<endl; | |
28 | + return(1.0); | |
29 | +} | |
25 | 30 | |
26 | 31 | namespace test_filter |
27 | 32 | { |
@@ -35,25 +40,27 @@ | ||
35 | 40 | void test_construction(void) |
36 | 41 | { |
37 | 42 | // Constructor |
38 | - Filter a; | |
43 | + Filter a(0.0); | |
39 | 44 | a.setNumber(5); |
40 | 45 | a.setDimension(3); |
41 | 46 | 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(); | |
43 | 49 | } |
44 | 50 | void test_manipulation(void) |
45 | 51 | { |
46 | - Filter a; | |
52 | + Filter a(0.0); | |
47 | 53 | a.setNumber(5); |
48 | 54 | a.setDimension(3); |
49 | 55 | a.createInitialParticles(); |
50 | 56 | 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()); | |
53 | 59 | a.set_state_func(&func1); |
54 | 60 | 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()); | |
56 | 62 | a.set_robserve_func(&func2); |
63 | + a.set_robserve_jacobian_func(&func3); | |
57 | 64 | Particles<double> y(5,3); |
58 | 65 | Particle<double> x(3); |
59 | 66 | x[0]=1.0; x[1]=2.0; x[2]=3.0; |
@@ -62,7 +69,7 @@ | ||
62 | 69 | y[i] = x; |
63 | 70 | } |
64 | 71 | 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)); | |
66 | 73 | } |
67 | 74 | } |
68 | 75 | int main() |
@@ -9,6 +9,15 @@ | ||
9 | 9 | // v = NULL; |
10 | 10 | mtrand.seed(time(NULL)); |
11 | 11 | } |
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 | +} | |
12 | 21 | Filter::~Filter() |
13 | 22 | { |
14 | 23 | // if(x) delete x; |
@@ -18,6 +27,7 @@ | ||
18 | 27 | bool Filter::setNumber(int n) |
19 | 28 | { |
20 | 29 | number = n; |
30 | + alpha.resize(n); | |
21 | 31 | return(true); |
22 | 32 | } |
23 | 33 | bool Filter::setDimension(int d) |
@@ -55,27 +65,58 @@ | ||
55 | 65 | v.dump_particles(); |
56 | 66 | return(0); |
57 | 67 | } |
68 | +string Filter::predict_particles_toString() | |
69 | +{ | |
70 | + return(x.toString()); | |
71 | +} | |
72 | +string Filter::system_noise_toString() | |
73 | +{ | |
74 | + return(v.toString()); | |
75 | +} | |
58 | 76 | bool Filter::set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v)) |
59 | 77 | { |
60 | 78 | state_func = func; |
61 | 79 | return(true); |
62 | 80 | } |
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)) | |
64 | 82 | { |
65 | 83 | robserve_func = func; |
66 | 84 | return(true); |
67 | 85 | } |
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 | +} | |
68 | 96 | Filter & Filter::get_next_state() |
69 | 97 | { |
70 | 98 | (*state_func)(x, v); |
71 | 99 | return(*this); |
72 | 100 | } |
73 | -double Filter::get_observed_noise() | |
101 | +double Filter::get_observed_noise(int i) | |
74 | 102 | { |
75 | - return((*robserve_func)(y, x)); | |
103 | + return((*robserve_func)(y, x[i])); | |
76 | 104 | } |
105 | +double Filter::get_robserved_density_value(double w) | |
106 | +{ | |
107 | + return((*robserve_density_func)(w)); | |
108 | +} | |
109 | + | |
77 | 110 | bool Filter::set_observed_data(Particles<double> &p) |
78 | 111 | { |
79 | 112 | y = p; |
80 | 113 | return(true); |
81 | 114 | } |
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 | +} |
@@ -56,6 +56,15 @@ | ||
56 | 56 | } |
57 | 57 | return(0); |
58 | 58 | } |
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 | +} | |
59 | 68 | |
60 | 69 | template class Particles<int>; |
61 | 70 | template class Particles<double>; |
@@ -11,13 +11,17 @@ | ||
11 | 11 | { |
12 | 12 | Particles<double> x, y; |
13 | 13 | Particles<double> v; |
14 | + vector<double> alpha; | |
14 | 15 | double w; |
15 | 16 | MTRand mtrand; |
16 | 17 | int number, dimension; |
17 | 18 | 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); | |
19 | 22 | public: |
20 | 23 | Filter(); |
24 | + Filter(double seed); | |
21 | 25 | ~Filter(); |
22 | 26 | bool setNumber(int n); |
23 | 27 | bool setDimension(int d); |
@@ -24,12 +28,18 @@ | ||
24 | 28 | bool createInitialParticles(); |
25 | 29 | bool createSystemNoise(); |
26 | 30 | int dump_predict_particles(); |
31 | + string predict_particles_toString(); | |
32 | + string system_noise_toString(); | |
27 | 33 | 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)); | |
29 | 37 | Filter & get_next_state(); |
30 | - double get_observed_noise(); | |
38 | + double get_observed_noise(int i); | |
39 | + double get_robserved_density_value(double w); | |
31 | 40 | int dump_System_Noise(); |
32 | 41 | bool set_observed_data(Particles<double> &p); |
42 | + bool compute_likelihood(); | |
33 | 43 | }; |
34 | 44 | |
35 | 45 | #endif |
@@ -20,6 +20,7 @@ | ||
20 | 20 | Particle<C> operator[](unsigned int i) const; |
21 | 21 | vector<Particle<C> > get_particles(); |
22 | 22 | int dump_particles(); |
23 | + string toString(); | |
23 | 24 | }; |
24 | 25 | |
25 | 26 |