| 修订版 | 17 (tree) |
|---|---|
| 时间 | 2010-05-07 20:38:18 |
| 作者 | (del#42041) |
prepared resampling
test/test_filter.cc (diff) src/filter.cc (diff)
src/filter.h (diff)| @@ -19,14 +19,19 @@ | ||
| 19 | 19 | double func2(Particles<double> &p, Particle<double> &q) |
| 20 | 20 | { |
| 21 | 21 | |
| 22 | - cout<<"robserve function"<<endl; | |
| 22 | +// cout<<"robserve function"<<endl; | |
| 23 | 23 | return(p[0][0]-q[0]); |
| 24 | 24 | } |
| 25 | -double func3(Particles<double> &p, Particles<double> &q) | |
| 25 | +double func3(Particles<double> &p, Particle<double> &q) | |
| 26 | 26 | { |
| 27 | - cout<<"robserve jacobian function"<<endl; | |
| 27 | +// cout<<"robserve jacobian function"<<endl; | |
| 28 | 28 | return(1.0); |
| 29 | 29 | } |
| 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 | +} | |
| 30 | 35 | |
| 31 | 36 | namespace test_filter |
| 32 | 37 | { |
| @@ -53,23 +58,30 @@ | ||
| 53 | 58 | a.setNumber(5); |
| 54 | 59 | a.setDimension(3); |
| 55 | 60 | a.createInitialParticles(); |
| 56 | - a.createSystemNoise(); | |
| 61 | + a.create_system_noise(); | |
| 62 | + | |
| 63 | + // initial value | |
| 57 | 64 | 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 | 65 | 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()); |
| 59 | 66 | a.set_state_func(&func1); |
| 60 | 67 | a.get_next_state(); |
| 68 | + | |
| 69 | + // initial value+system noise | |
| 61 | 70 | 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()); |
| 62 | 71 | a.set_robserve_func(&func2); |
| 63 | 72 | a.set_robserve_jacobian_func(&func3); |
| 64 | 73 | Particles<double> y(5,3); |
| 65 | 74 | 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; | |
| 67 | 76 | for(int i=0;i<y.size();++i) |
| 68 | 77 | { |
| 69 | 78 | y[i] = x; |
| 70 | 79 | } |
| 71 | 80 | 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(); | |
| 73 | 85 | } |
| 74 | 86 | } |
| 75 | 87 | int main() |
| @@ -44,7 +44,7 @@ | ||
| 44 | 44 | v.resize(number, dimension); |
| 45 | 45 | return(true); |
| 46 | 46 | } |
| 47 | -bool Filter::createSystemNoise() | |
| 47 | +bool Filter::create_system_noise() | |
| 48 | 48 | { |
| 49 | 49 | for(int i=0;i<v.size();++i) |
| 50 | 50 | { |
| @@ -83,7 +83,7 @@ | ||
| 83 | 83 | robserve_func = func; |
| 84 | 84 | return(true); |
| 85 | 85 | } |
| 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)) | |
| 87 | 87 | { |
| 88 | 88 | robserve_jacobian_func = func; |
| 89 | 89 | return(true); |
| @@ -116,7 +116,11 @@ | ||
| 116 | 116 | { |
| 117 | 117 | for(unsigned int i=0;i<alpha.size();++i) |
| 118 | 118 | { |
| 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]); | |
| 120 | 121 | } |
| 121 | 122 | return(true); |
| 122 | 123 | } |
| 124 | +bool Filter::resampling() | |
| 125 | +{ | |
| 126 | +} | |
| \ No newline at end of file |
| @@ -5,6 +5,7 @@ | ||
| 5 | 5 | #include <particles.h> |
| 6 | 6 | #include <MersenneTwister.h> |
| 7 | 7 | #include <ctime> |
| 8 | +#include <cmath> | |
| 8 | 9 | using namespace std; |
| 9 | 10 | |
| 10 | 11 | class Filter |
| @@ -17,7 +18,7 @@ | ||
| 17 | 18 | int number, dimension; |
| 18 | 19 | Particles<double> (*state_func)(Particles<double> &p, Particles<double> &v); |
| 19 | 20 | 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); | |
| 21 | 22 | double (*robserve_density_func)(double w); |
| 22 | 23 | public: |
| 23 | 24 | Filter(); |
| @@ -26,13 +27,13 @@ | ||
| 26 | 27 | bool setNumber(int n); |
| 27 | 28 | bool setDimension(int d); |
| 28 | 29 | bool createInitialParticles(); |
| 29 | - bool createSystemNoise(); | |
| 30 | + bool create_system_noise(); | |
| 30 | 31 | int dump_predict_particles(); |
| 31 | 32 | string predict_particles_toString(); |
| 32 | 33 | string system_noise_toString(); |
| 33 | 34 | bool set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v)); |
| 34 | 35 | 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)); | |
| 36 | 37 | bool set_robserve_density_func(double (*func)(double w)); |
| 37 | 38 | Filter & get_next_state(); |
| 38 | 39 | double get_observed_noise(int i); |
| @@ -40,6 +41,7 @@ | ||
| 40 | 41 | int dump_System_Noise(); |
| 41 | 42 | bool set_observed_data(Particles<double> &p); |
| 42 | 43 | bool compute_likelihood(); |
| 44 | + bool resampling(); | |
| 43 | 45 | }; |
| 44 | 46 | |
| 45 | 47 | #endif |
Fork