• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版30 (tree)
时间2011-01-04 17:22:18
作者linuxchecker

Log Message

started additional test using sin curve

更改概述

差异

--- test/test_filter.cc (revision 29)
+++ test/test_filter.cc (revision 30)
@@ -49,6 +49,47 @@
4949 return(1.0/sqrt(2.0*M_PI*OBS_DIST*OBS_DIST)*exp(-w*w/(2.0*OBS_DIST*OBS_DIST)));
5050 }
5151
52+Particles<double> sin_system_func(Particles<double> &p, Particles<double> &v)
53+{
54+ for(int i=0;i<p.size();++i)
55+ {
56+ for(int j=0;j<p[i].size();++j)
57+ {
58+ p[i][j]= p[i][j]+v[i][j];
59+ }
60+ }
61+ cout<<"state function"<<endl;
62+ return(p);
63+}
64+double sin_reversed_observed_func(Particles<double> &p, Particle<double> &q)
65+{
66+ double diff = 0.0;
67+ for(int i=0;i<p.size();++i)
68+ {
69+ Particle<double> x = p[i];
70+ double d = 0.0;
71+ for(int j=0;j<x.size();++j)
72+ {
73+ d += (x[j]-q[j])*(x[j]-q[j]);
74+ }
75+ diff += sqrt(d);
76+ }
77+
78+// cout<<"robserve function"<<endl;
79+// return(p[0][0]-q[0]);
80+ return(diff/p.size());
81+}
82+double sin_reversed_jacobian_func(Particles<double> &p, Particle<double> &q)
83+{
84+// cout<<"robserve jacobian function"<<endl;
85+ return(1.0);
86+}
87+double sin_reversed_observed_density_func(double w)
88+{
89+// cout<<"robserve density function"<<endl;
90+ return(1.0/sqrt(2.0*M_PI*OBS_DIST*OBS_DIST)*exp(-w*w/(2.0*OBS_DIST*OBS_DIST)));
91+}
92+
5293 namespace test_filter
5394 {
5495 void cut_setup()
@@ -113,7 +154,7 @@
113154 Particles<double> predict = prediction + noise;
114155 // initial value+system noise
115156 cut_assert_equal_string(predict.toString().c_str(),a.get_predict_particles().toString().c_str());
116-cout<<"predict1:"<<predict.toString()<<endl;
157+//cout<<"predict1:"<<predict.toString()<<endl;
117158 Particles<double> y(NUM_PARTICLE,3);
118159 Particle<double> x(3);
119160 x[0]=0.1; x[1]=-1.1; x[2]=1.5;
@@ -123,7 +164,7 @@
123164 {
124165 y[i] = x;
125166 }
126-cout<<"observed1:"<<y.toString()<<endl;
167+//cout<<"observed1:"<<y.toString()<<endl;
127168 a.set_observed_data(y);
128169 double diff;
129170 diff = x[0]-predict[0][0];
@@ -131,7 +172,7 @@
131172 cut_assert_equal_double(1.0,1.0 ,a.get_robserved_density_value(diff));
132173 a.compute_likelihood();
133174 a.resampling();
134-cout<<"resampled1:"<<a.get_predict_particles().toString()<<endl;
175+//cout<<"resampled1:"<<a.get_predict_particles().toString()<<endl;
135176 for(int j=0;j<a.get_predict_particles().size();++j)
136177 {
137178 for(int i=0;i<x.size();++i)
@@ -144,7 +185,7 @@
144185
145186 a.create_system_noise(0.0, 1.0);
146187 a.get_next_state();
147-cout<<"predict2:"<<a.get_predict_particles().toString()<<endl;
188+//cout<<"predict2:"<<a.get_predict_particles().toString()<<endl;
148189 for(int j=0;j<a.get_predict_particles().size();++j)
149190 {
150191 for(int i=0;i<x.size();++i)
@@ -157,7 +198,7 @@
157198 {
158199 y[i] = x;
159200 }
160-cout<<"observed2:"<<y.toString()<<endl;
201+//cout<<"observed2:"<<y.toString()<<endl;
161202 a.set_observed_data(y);
162203 cut_assert_equal_double(x[0],10.0,a.get_observed_noise(0));
163204 a.set_robserve_density_func(func4);
@@ -165,7 +206,7 @@
165206 cut_assert_equal_double(1.0,1.0,a.get_robserved_density_value(diff));
166207 a.compute_likelihood();
167208 a.resampling();
168-cout<<"resampled2:"<<a.get_predict_particles().toString()<<endl;
209+//cout<<"resampled2:"<<a.get_predict_particles().toString()<<endl;
169210 for(int j=0;j<a.get_predict_particles().size();++j)
170211 {
171212 for(int i=0;i<x.size();++i)
@@ -175,6 +216,51 @@
175216 }
176217
177218 }
219+ void test_sin_curve(void)
220+ {
221+ double t = 0.0;
222+ Filter a(0.0);
223+ const unsigned int num_particle = 100;
224+ a.setNumber(num_particle);
225+ a.setDimension(1);
226+ a.set_state_func(&sin_system_func);
227+ a.set_robserve_func(&sin_reversed_observed_func);
228+ a.set_robserve_jacobian_func(&sin_reversed_jacobian_func);
229+ a.set_robserve_density_func(&sin_reversed_observed_density_func);
230+
231+ a.createInitialParticles();
232+ for(unsigned l=0;l<100;++l)
233+ {
234+ a.create_system_noise(0.0, 1.0);
235+
236+ // initial value
237+ for(unsigned int i=0;i<num_particle;++i)
238+ {
239+ // initial values should be zero[currently]
240+ cut_assert_equal_double(0.0, 0.1, a.get_particles()[i][0]);
241+ }
242+ for(unsigned int i=0;i<num_particle;++i)
243+ {
244+ // initial values should be near zero
245+ cut_assert_equal_double(0.0, 10.0, a.get_system_noise()[i][0]);
246+ }
247+
248+ Particles<double> noise(a.get_system_noise());
249+ Particles<double> prediction(a.get_predict_particles());
250+
251+ a.get_next_state();
252+ Particles<double> predict = prediction + noise;
253+ // initial value+system noise(random walk)
254+ cut_assert_equal_string(predict.toString().c_str(),a.get_predict_particles().toString().c_str());
255+//cout<<"predict1:"<<predict.toString()<<endl;
256+ Particles<double> y(num_particle,1);
257+ Particle<double> x(1);
258+ for(unsigned int i=0;i<num_particle;++i)
259+ {
260+ y[i][0]=sin(t);
261+ }
262+ }
263+ }
178264 }
179265 int main()
180266 {
--- src/filter.cc (revision 29)
+++ src/filter.cc (revision 30)
@@ -5,8 +5,6 @@
55 number = -1;
66 dimension = -1;
77 state_func = NULL;
8-// x = NULL;
9-// v = NULL;
108 mtrand.seed(time(NULL));
119 }
1210 Filter::Filter(double seed)
@@ -14,14 +12,10 @@
1412 number = -1;
1513 dimension = -1;
1614 state_func = NULL;
17-// x = NULL;
18-// v = NULL;
1915 mtrand.seed(seed);
2016 }
2117 Filter::~Filter()
2218 {
23-// if(x) delete x;
24-// if(v) delete v;
2519 }
2620
2721 bool Filter::setNumber(int n)
@@ -55,7 +49,7 @@
5549 {
5650 for(int j=0;j<v[i].size();++j)
5751 {
58- v[i][j] = mtrand.randNorm(mean, dist); // ToDO: functionize
52+ v[i][j] = mtrand.randNorm(mean, dist);
5953 }
6054 }
6155 return(true);
Show on old repository browser