| 修订版 | 26 (tree) |
|---|---|
| 时间 | 2010-06-08 19:48:37 |
| 作者 | (del#42041) |
now sample/step.cc works fine
sample/step.cc (diff) src/filter.cc (diff)| @@ -6,6 +6,7 @@ | ||
| 6 | 6 | { |
| 7 | 7 | for(int j=0;j<p[i].size();++j) |
| 8 | 8 | { |
| 9 | +//cout<< p[i][j]<<"+"<<v[i][j]<<endl; | |
| 9 | 10 | p[i][j]= p[i][j]+v[i][j]; |
| 10 | 11 | } |
| 11 | 12 | } |
| @@ -17,7 +18,17 @@ | ||
| 17 | 18 | |
| 18 | 19 | // cout<<"robserve function"<<endl; |
| 19 | 20 | // cout<<"noise="<<(p[0][0]-q[0])<<endl; |
| 20 | - return(p[0][0]-q[0]); | |
| 21 | +// return(p[0][0]-q[0]); | |
| 22 | + double err = 0.0, n=0.0; | |
| 23 | + for(int i=0;i<p.size();++i) | |
| 24 | + { | |
| 25 | + for(int j=0;j<p[i].size();++j) | |
| 26 | + { | |
| 27 | + err += p[i][j] - q[j]; | |
| 28 | + n += 1.0; | |
| 29 | + } | |
| 30 | + } | |
| 31 | + return(err/n); | |
| 21 | 32 | } |
| 22 | 33 | double func3(Particles<double> &p, Particle<double> &q) |
| 23 | 34 | { |
| @@ -27,7 +38,7 @@ | ||
| 27 | 38 | double func4(double w) |
| 28 | 39 | { |
| 29 | 40 | // cout<<"robserve density function"<<endl; |
| 30 | - double rho = 0.08; | |
| 41 | + double rho = 1.51; | |
| 31 | 42 | double r = (1.0/sqrt(2.0*M_PI*rho*rho)*exp(-w*w/(2.0*rho*rho))); |
| 32 | 43 | return(r); |
| 33 | 44 | } |
| @@ -36,7 +47,7 @@ | ||
| 36 | 47 | { |
| 37 | 48 | Filter track; |
| 38 | 49 | MTRand mtrand; |
| 39 | -#define NUM 5 | |
| 50 | +#define NUM 1000 | |
| 40 | 51 | mtrand.seed(time(NULL)); |
| 41 | 52 | track.setNumber(NUM); |
| 42 | 53 | track.setDimension(1); |
| @@ -51,11 +62,26 @@ | ||
| 51 | 62 | { |
| 52 | 63 | track.create_system_noise(); |
| 53 | 64 | track.get_next_state(); // predict |
| 65 | + x = track.get_particles(); | |
| 66 | + double xp_=0.0; | |
| 67 | + for(int j=0;j<x.size();++j) | |
| 68 | + { | |
| 69 | + xp_ += x[j][0]; | |
| 70 | + } | |
| 71 | +//cout<<xp_/x.size()<<endl; | |
| 72 | + | |
| 54 | 73 | t = i<25?1:i<50?0:i<75?-1:1; |
| 55 | 74 | for(int j=0;j<x.size();++j) |
| 56 | 75 | { |
| 57 | - y[j][0] = t + mtrand.rand()*0.00; | |
| 76 | + y[j][0] = t + (mtrand.rand()-0.5)*8.0; | |
| 58 | 77 | } |
| 78 | + double y_=0.0; | |
| 79 | + for(int j=0;j<x.size();++j) | |
| 80 | + { | |
| 81 | + y_ += y[j][0]; | |
| 82 | + } | |
| 83 | +//cout<<"observed="<<y_/x.size()<<endl; | |
| 84 | +//cout<<y_/x.size()<<endl; | |
| 59 | 85 | |
| 60 | 86 | //cout<<y[0][0]<<endl; |
| 61 | 87 | track.set_observed_data(y); |
| @@ -63,17 +89,17 @@ | ||
| 63 | 89 | x = track.get_particles(); |
| 64 | 90 | for(int j=0;j<x.size();++j) |
| 65 | 91 | { |
| 66 | - cout<<x[j][0]<<endl; | |
| 92 | +// cout<<"["<<j<<"]"<<x[j][0]<<endl; | |
| 67 | 93 | } |
| 68 | 94 | track.resampling(); |
| 69 | -cout<<endl; | |
| 95 | +//cout<<"--resampled--"<<endl; | |
| 70 | 96 | x = track.get_particles(); |
| 71 | 97 | for(int j=0;j<x.size();++j) |
| 72 | 98 | { |
| 73 | - cout<<x[j][0]<<endl; | |
| 99 | +// cout<<"["<<j<<"]"<<x[j][0]<<endl; | |
| 74 | 100 | } |
| 75 | -cout<<endl; | |
| 76 | -cout<<endl; | |
| 101 | +//cout<<endl; | |
| 102 | +//cout<<endl; | |
| 77 | 103 | |
| 78 | 104 | double x_=0.0; |
| 79 | 105 | for(int j=0;j<x.size();++j) |
| @@ -80,6 +106,6 @@ | ||
| 80 | 106 | { |
| 81 | 107 | x_ += x[j][0]; |
| 82 | 108 | } |
| 83 | -cout<<x_/x.size()<<endl<<endl; | |
| 109 | +cout<<x_/x.size()<<endl; | |
| 84 | 110 | } |
| 85 | 111 | } |
| \ No newline at end of file |
| @@ -50,7 +50,8 @@ | ||
| 50 | 50 | { |
| 51 | 51 | for(int j=0;j<v[i].size();++j) |
| 52 | 52 | { |
| 53 | - v[i][j] = (mtrand.randNorm()-0.5)*0.8; // ToDO: functionize | |
| 53 | + v[i][j] = mtrand.randNorm(0.0, 1.2); // ToDO: functionize | |
| 54 | +//cout<<"noise="<<v[i][j]<<endl; | |
| 54 | 55 | } |
| 55 | 56 | } |
| 56 | 57 | return(true); |
| @@ -131,9 +132,11 @@ | ||
| 131 | 132 | bool Filter::resampling() |
| 132 | 133 | { |
| 133 | 134 | double alphasum = 0.0; |
| 135 | + int k=0; | |
| 134 | 136 | for(vector<double>::iterator i=alpha.begin();i!=alpha.end();++i) |
| 135 | 137 | { |
| 136 | -cout<<"alpha="<<(*i)<<endl; | |
| 138 | +//cout<<"alpha["<<k<<"]="<<(*i)<<endl; | |
| 139 | + ++k; | |
| 137 | 140 | alphasum += (*i); |
| 138 | 141 | } |
| 139 | 142 | //cout<<"alpha sum="<<alphasum<<endl; |
| @@ -141,33 +144,44 @@ | ||
| 141 | 144 | cout<<"Regularize Constant is too small="<<alphasum<<endl; |
| 142 | 145 | double m = (double)x.size(); |
| 143 | 146 | Particles<double> f(number, dimension); |
| 147 | + vector<double> psuma(x.size() + 1); | |
| 148 | + psuma[0] = 0.0; | |
| 149 | + for(int i=1;i<x.size();++i) | |
| 150 | + { | |
| 151 | + int l; | |
| 152 | + psuma[i]=0.0; | |
| 153 | + for(l=0;l<i;++l) | |
| 154 | + { | |
| 155 | + psuma[i]+=alpha[l]; | |
| 156 | + } | |
| 157 | + } | |
| 158 | + psuma[x.size()-1] = psuma[x.size()-2]+alpha[x.size()-1]; | |
| 144 | 159 | for(int j=0;j<x.size();++j) |
| 145 | 160 | { |
| 146 | 161 | double u = ((j+1)-0.5)/m; |
| 147 | -cout<<"u="<<u<<endl; | |
| 148 | - int sample_i = 0; | |
| 162 | +//cout<<"u="<<u<<endl; | |
| 163 | + int sample_i = -1; | |
| 149 | 164 | // for(int i=1;i<((int)m-2);++i) |
| 150 | - for(int i=1;i<((int)m);++i) | |
| 165 | + for(int i=1;i<=x.size();++i) | |
| 151 | 166 | { |
| 152 | - int l; | |
| 153 | 167 | double a1=0.0, a2=0.0; |
| 154 | - for(l=0;l<i;++l) | |
| 155 | - { | |
| 156 | - a1+=alpha[l]; | |
| 157 | - } | |
| 158 | - a2 = a1 + alpha[i]; | |
| 159 | -cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl; | |
| 160 | -cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl; | |
| 161 | -cout<<(u<=a2/alphasum)<<"?"<<endl; | |
| 162 | -cout<<(a1/alphasum<u)<<endl; | |
| 168 | + a1 = psuma[i-1]; | |
| 169 | + a2 = psuma[i]; | |
| 170 | +//cout<<a1<<"<"<<u*alphasum<<"<="<<a2<<"?"<<endl; | |
| 171 | +//cout<<a1/alphasum<<"<"<<u<<"<="<<a2/alphasum<<"?"<<endl; | |
| 172 | +//cout<<(u<=a2/alphasum)<<"?"<<endl; | |
| 173 | +//cout<<(a1/alphasum<u)<<endl; | |
| 163 | 174 | if((a1/alphasum<u) && (a2/alphasum>=u)) |
| 164 | 175 | { |
| 165 | - sample_i = i ; | |
| 176 | + sample_i = i - 1; | |
| 166 | 177 | break; |
| 167 | 178 | } |
| 168 | 179 | } |
| 169 | -cout<<"sample="<<sample_i<<endl; | |
| 170 | - f[j] = x[sample_i]; | |
| 180 | +//cout<<"sample="<<(sample_i+0)<<endl; | |
| 181 | + if(sample_i>=0) | |
| 182 | + { | |
| 183 | + f[j] = x[sample_i+0]; | |
| 184 | + } | |
| 171 | 185 | } |
| 172 | 186 | x = f; |
| 173 | 187 | return(true); |
Fork