• R/O
  • SSH
  • HTTPS

particle-filter: 提交


Commit MetaInfo

修订版17 (tree)
时间2010-05-07 20:38:18
作者(del#42041)

Log Message

prepared resampling

更改概述

差异

--- test/test_filter.cc (revision 16)
+++ test/test_filter.cc (revision 17)
@@ -19,14 +19,19 @@
1919 double func2(Particles<double> &p, Particle<double> &q)
2020 {
2121
22- cout<<"robserve function"<<endl;
22+// cout<<"robserve function"<<endl;
2323 return(p[0][0]-q[0]);
2424 }
25-double func3(Particles<double> &p, Particles<double> &q)
25+double func3(Particles<double> &p, Particle<double> &q)
2626 {
27- cout<<"robserve jacobian function"<<endl;
27+// cout<<"robserve jacobian function"<<endl;
2828 return(1.0);
2929 }
30+double func4(double w)
31+{
32+// cout<<"robserve density function"<<endl;
33+ return(1.0/sqrt(2.0*M_PI*0.1*0.1)*exp(-w*w/(2.0*0.1*0.1)));
34+}
3035
3136 namespace test_filter
3237 {
@@ -53,23 +58,30 @@
5358 a.setNumber(5);
5459 a.setDimension(3);
5560 a.createInitialParticles();
56- a.createSystemNoise();
61+ a.create_system_noise();
62+
63+ // initial value
5764 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());
5865 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());
5966 a.set_state_func(&func1);
6067 a.get_next_state();
68+
69+ // initial value+system noise
6170 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());
6271 a.set_robserve_func(&func2);
6372 a.set_robserve_jacobian_func(&func3);
6473 Particles<double> y(5,3);
6574 Particle<double> x(3);
66- x[0]=1.0; x[1]=2.0; x[2]=3.0;
75+ x[0]=0.1; x[1]=-1.1; x[2]=1.5;
6776 for(int i=0;i<y.size();++i)
6877 {
6978 y[i] = x;
7079 }
7180 a.set_observed_data(y);
72- cut_assert_equal_double(-0.163078,1e-5,a.get_observed_noise(0));
81+ cut_assert_equal_double(-1.06308,1e-4,a.get_observed_noise(0));
82+ a.set_robserve_density_func(func4);
83+ cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308));
84+ a.compute_likelihood();
7385 }
7486 }
7587 int main()
--- src/filter.cc (revision 16)
+++ src/filter.cc (revision 17)
@@ -44,7 +44,7 @@
4444 v.resize(number, dimension);
4545 return(true);
4646 }
47-bool Filter::createSystemNoise()
47+bool Filter::create_system_noise()
4848 {
4949 for(int i=0;i<v.size();++i)
5050 {
@@ -83,7 +83,7 @@
8383 robserve_func = func;
8484 return(true);
8585 }
86-bool Filter::set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particles<double> &q))
86+bool Filter::set_robserve_jacobian_func(double (*func)(Particles<double> &p, Particle<double> &q))
8787 {
8888 robserve_jacobian_func = func;
8989 return(true);
@@ -116,7 +116,11 @@
116116 {
117117 for(unsigned int i=0;i<alpha.size();++i)
118118 {
119- alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]));
119+ alpha[i] = (*robserve_density_func)((*robserve_func)(y, x[i]))
120+ *(*robserve_jacobian_func)(y, x[i]);
120121 }
121122 return(true);
122123 }
124+bool Filter::resampling()
125+{
126+}
\ No newline at end of file
--- src/filter.h (revision 16)
+++ src/filter.h (revision 17)
@@ -5,6 +5,7 @@
55 #include <particles.h>
66 #include <MersenneTwister.h>
77 #include <ctime>
8+#include <cmath>
89 using namespace std;
910
1011 class Filter
@@ -17,7 +18,7 @@
1718 int number, dimension;
1819 Particles<double> (*state_func)(Particles<double> &p, Particles<double> &v);
1920 double (*robserve_func)(Particles<double> &p, Particle<double> &q);
20- double (*robserve_jacobian_func)(Particles<double> &p, Particles<double> &q);
21+ double (*robserve_jacobian_func)(Particles<double> &p, Particle<double> &q);
2122 double (*robserve_density_func)(double w);
2223 public:
2324 Filter();
@@ -26,13 +27,13 @@
2627 bool setNumber(int n);
2728 bool setDimension(int d);
2829 bool createInitialParticles();
29- bool createSystemNoise();
30+ bool create_system_noise();
3031 int dump_predict_particles();
3132 string predict_particles_toString();
3233 string system_noise_toString();
3334 bool set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v));
3435 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_jacobian_func(double (*func)(Particles<double> &p, Particle<double> &q));
3637 bool set_robserve_density_func(double (*func)(double w));
3738 Filter & get_next_state();
3839 double get_observed_noise(int i);
@@ -40,6 +41,7 @@
4041 int dump_System_Noise();
4142 bool set_observed_data(Particles<double> &p);
4243 bool compute_likelihood();
44+ bool resampling();
4345 };
4446
4547 #endif
Show on old repository browser