| 修订版 | 19 (tree) |
|---|---|
| 时间 | 2010-05-11 20:56:59 |
| 作者 | (del#42041) |
implemented resampling
test/test_filter.cc (diff) src/filter.cc (diff)
src/filter.h (diff)| @@ -82,6 +82,8 @@ | ||
| 82 | 82 | a.set_robserve_density_func(func4); |
| 83 | 83 | cut_assert_equal_double(1.14882e-24,1e-5,a.get_robserved_density_value(-1.06308)); |
| 84 | 84 | a.compute_likelihood(); |
| 85 | + a.resampling(); | |
| 86 | + cut_assert_equal_string("(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)(0.153025,-1.168815,1.558071)",a.predict_particles_toString().c_str()); | |
| 85 | 87 | } |
| 86 | 88 | } |
| 87 | 89 | int main() |
| @@ -121,16 +121,40 @@ | ||
| 121 | 121 | } |
| 122 | 122 | return(true); |
| 123 | 123 | } |
| 124 | +Particles<double> Filter::get_particles() | |
| 125 | +{ | |
| 126 | + return(x); | |
| 127 | +} | |
| 124 | 128 | bool Filter::resampling() |
| 125 | 129 | { |
| 126 | 130 | double alphasum = 0.0; |
| 127 | - for(vector<double>::iterator i=0;i!=alpha.end();++i) | |
| 131 | + for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i) | |
| 128 | 132 | { |
| 129 | - alphasum += alpha[i]; | |
| 133 | + alphasum += (*i); | |
| 130 | 134 | } |
| 131 | 135 | double m = (double)x.size(); |
| 136 | + Particles<double> f(number, dimension); | |
| 132 | 137 | for(int j=0;j<x.size();++j) |
| 133 | 138 | { |
| 134 | - double u = (j-0.5)/m; | |
| 139 | + double u = ((j+1)-0.5)/m, a1=0.0, a2=0.0; | |
| 140 | + int sample_i = 0; | |
| 141 | + for(int i=1;i<((int)m-2);++i) | |
| 142 | + { | |
| 143 | + int l; | |
| 144 | + for(l=0;l<i;++l) | |
| 145 | + { | |
| 146 | + a1+=alpha[l]; | |
| 147 | + } | |
| 148 | + a2 = a1 + alpha[l]; | |
| 149 | +//cout<<a1/alphasum<<"<"<<u<<"<"<<a2/alphasum<<"?"<<endl; | |
| 150 | + if(a1/alphasum<u && a2/alphasum>=u) | |
| 151 | + { | |
| 152 | + sample_i = i; | |
| 153 | + break; | |
| 154 | + } | |
| 155 | + } | |
| 156 | + f[j] = x[sample_i]; | |
| 135 | 157 | } |
| 158 | + x = f; | |
| 159 | + return(true); | |
| 136 | 160 | } |
| \ No newline at end of file |
| @@ -29,6 +29,7 @@ | ||
| 29 | 29 | bool createInitialParticles(); |
| 30 | 30 | bool create_system_noise(); |
| 31 | 31 | int dump_predict_particles(); |
| 32 | + Particles<double> get_particles(); | |
| 32 | 33 | string predict_particles_toString(); |
| 33 | 34 | string system_noise_toString(); |
| 34 | 35 | bool set_state_func(Particles<double> (*func)(Particles<double> &p, Particles<double> &v)); |
Fork