OSDN > 浏览软件 > Scientific/Engineering > Mathematics > 统计 > ParticleFilter > SCM > SVN > 提交

ParticleFilter
Fork
ParticleFilter
ParticleFilter

particle-filter: 提交

improved reversed observed function

--- test/test_filter.cc (revision 28)

+++ test/test_filter.cc (revision 29)

		@@ -4,6 +4,10 @@
4	4	#include <filter.h>
5	5	using namespace std;
6	6
	7	+#define NUM_PARTICLE 100
	8	+#define SYS_DIST 5.0
	9	+#define OBS_DIST 0.01
	10	+
7	11	Particles<double> func1(Particles<double> &p, Particles<double> &v)
8	12	{
9	13	for(int i=0;i<p.size();++i)

		@@ -18,9 +22,21 @@
18	22	}
19	23	double func2(Particles<double> &p, Particle<double> &q)
20	24	{
	25	+ double diff = 0.0;
	26	+ for(int i=0;i<p.size();++i)
	27	+ {
	28	+ Particle<double> x = p[i];
	29	+ double d = 0.0;
	30	+ for(int j=0;j<x.size();++j)
	31	+ {
	32	+ d += (x[j]-q[j])*(x[j]-q[j]);
	33	+ }
	34	+ diff += sqrt(d);
	35	+ }
21	36
22	37	// cout<<"robserve function"<<endl;
23		- return(p[0][0]-q[0]);
	38	+// return(p[0][0]-q[0]);
	39	+ return(diff/p.size());
24	40	}
25	41	double func3(Particles<double> &p, Particle<double> &q)
26	42	{

		@@ -30,7 +46,7 @@
30	46	double func4(double w)
31	47	{
32	48	// cout<<"robserve density function"<<endl;
33		- return(1.0/sqrt(2.0M_PI0.10.1)exp(-ww/(2.00.1*0.1)));
	49	+ return(1.0/sqrt(2.0M_PIOBS_DISTOBS_DIST)exp(-ww/(2.0OBS_DIST*OBS_DIST)));
34	50	}
35	51
36	52	namespace test_filter

		@@ -46,16 +62,23 @@
46	62	{
47	63	// Constructor
48	64	Filter a(0.0);
49		- a.setNumber(5);
	65	+ a.setNumber(NUM_PARTICLE);
50	66	a.setDimension(3);
51	67	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.get_predict_particles().toString().c_str());
	68	+ for(int i=0;i<NUM_PARTICLE;++i)
	69	+ {
	70	+ for(int j=0;j<3;++j)
	71	+ {
	72	+ // initial values should be zero
	73	+ cut_assert_equal_double(0.0, 0.1, a.get_particles().toVector().at(i).toVector().at(j));
	74	+ }
	75	+ }
53	76	// a.dump_predict_particles();
54	77	}
55	78	void test_manipulation(void)
56	79	{
57	80	Filter a(0.0);
58		- a.setNumber(5);
	81	+ a.setNumber(NUM_PARTICLE);
59	82	a.setDimension(3);
60	83	a.set_state_func(&func1);
61	84	a.set_robserve_func(&func2);

		@@ -63,11 +86,25 @@
63	86	a.set_robserve_density_func(func4);
64	87
65	88	a.createInitialParticles();
66		- a.create_system_noise();
	89	+ a.create_system_noise(0.0, 1.0);
67	90
68	91	// 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.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());
	92	+ for(int i=0;i<NUM_PARTICLE;++i)
	93	+ {
	94	+ for(int j=0;j<3;++j)
	95	+ {
	96	+ // initial values should be zero
	97	+ cut_assert_equal_double(0.0, 0.1, a.get_particles()[i][j]);
	98	+ }
	99	+ }
	100	+ for(int i=0;i<NUM_PARTICLE;++i)
	101	+ {
	102	+ for(int j=0;j<3;++j)
	103	+ {
	104	+ // initial values should be near zero
	105	+ cut_assert_equal_double(0.0, 10.0, a.get_system_noise()[i][j]);
	106	+ }
	107	+ }
71	108
72	109	Particles<double> noise(a.get_system_noise());
73	110	Particles<double> prediction(a.get_predict_particles());

		@@ -77,9 +114,11 @@
77	114	// initial value+system noise
78	115	cut_assert_equal_string(predict.toString().c_str(),a.get_predict_particles().toString().c_str());
79	116	cout<<"predict1:"<<predict.toString()<<endl;
80		- Particles<double> y(5,3);
	117	+ Particles<double> y(NUM_PARTICLE,3);
81	118	Particle<double> x(3);
82	119	x[0]=0.1; x[1]=-1.1; x[2]=1.5;
	120	+
	121	+ // no noise. tracking point should be (0.1, -1.1, 1.5)
83	122	for(int i=0;i<y.size();++i)
84	123	{
85	124	y[i] = x;

		@@ -86,9 +125,10 @@
86	125	}
87	126	cout<<"observed1:"<<y.toString()<<endl;
88	127	a.set_observed_data(y);
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));
	128	+ double diff;
	129	+ diff = x[0]-predict[0][0];
	130	+ cut_assert_equal_double(0.0, 2.0, diff);
	131	+ cut_assert_equal_double(1.0,1.0 ,a.get_robserved_density_value(diff));
92	132	a.compute_likelihood();
93	133	a.resampling();
94	134	cout<<"resampled1:"<<a.get_predict_particles().toString()<<endl;

		@@ -102,7 +142,7 @@
102	142
103	143	// step 2
104	144
105		- a.create_system_noise();
	145	+ a.create_system_noise(0.0, 1.0);
106	146	a.get_next_state();
107	147	cout<<"predict2:"<<a.get_predict_particles().toString()<<endl;
108	148	for(int j=0;j<a.get_predict_particles().size();++j)

		@@ -119,9 +159,10 @@
119	159	}
120	160	cout<<"observed2:"<<y.toString()<<endl;
121	161	a.set_observed_data(y);
122		- cut_assert_equal_double(-0.36319,1.0,a.get_observed_noise(0));
	162	+ cut_assert_equal_double(x[0],10.0,a.get_observed_noise(0));
123	163	a.set_robserve_density_func(func4);
124		- cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308));
	164	+ diff = x[0]-predict[0][0];
	165	+ cut_assert_equal_double(1.0,1.0,a.get_robserved_density_value(diff));
125	166	a.compute_likelihood();
126	167	a.resampling();
127	168	cout<<"resampled2:"<<a.get_predict_particles().toString()<<endl;

--- src/filter.cc (revision 28)

+++ src/filter.cc (revision 29)

		@@ -49,13 +49,13 @@
49	49	v.resize(number, dimension);
50	50	return(true);
51	51	}
52		-bool Filter::create_system_noise()
	52	+bool Filter::create_system_noise(double mean, double dist)
53	53	{
54	54	for(int i=0;i<v.size();++i)
55	55	{
56	56	for(int j=0;j<v[i].size();++j)
57	57	{
58		- v[i][j] = mtrand.randNorm(0.0, 1.2); // ToDO: functionize
	58	+ v[i][j] = mtrand.randNorm(mean, dist); // ToDO: functionize
59	59	}
60	60	}
61	61	return(true);

--- src/filter.h (revision 28)

+++ src/filter.h (revision 29)

		@@ -28,7 +28,7 @@
28	28	bool setNumber(int n);
29	29	bool setDimension(int d);
30	30	bool createInitialParticles();
31		- bool create_system_noise();
	31	+ bool create_system_noise(double mean, double dist);
32	32	bool createInitialParticles(double d);
33	33	int dump_predict_particles();
34	34	Particles<double> get_particles();