/*
Particle Filter
一般系：
x_t = F_t x_{t-1} + G_t v_t
y_t = H_t x_t + w_t
ここでは：
x_t = x_{t-1} + v_t
y_t = x_t + w_t
where v_t, w_t は N(0,1).
*/
#include ＜stdio.h>
#include ＜stdlib.h>
#include ＜math.h>
#define PI 3.1415926
#define NN 1100 /*粒子の最大数*/
#define MM 100 /*時刻ステップの最大数*/
#define LL 30 /*平滑化の区間*/
/*for random number*/
#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
double gauss(double mu, double sigma2);
double pdf(double x, double mu, double sigma);
void showData(double d[], int num );
void showParticle(double f[], double v[], double w[], double p[], double y[], double alpha[], int num) ;
double ran2();
unsigned long int idum =1024;
main()
{
int l,n,n1,nmax,nend;/*step*/
int j,j1,jmax;/*粒子数*/
double v[NN],mu,sigma2,w[NN],mu2,sigma3;/*v,wの平均、分散*/
double v1,w1,x[MM],y1[MM];/*観測値のノイズ、観測値の状態と観測値の配列*/
double p[MM][NN],y[MM][NN];/*状態pj、観測値yj*/
double alfa[NN],a1,a2; /*尤度*/
double A,A1,a[NN],ran;/*alfaの合計,尤度のメモリの配列*/
double f[MM][NN],fh[MM][NN];/*リサンプリングした配列*/
int k,K[MM][NN];/*平滑化の時に使う配列と引数*/
double fwa,fa,bun,hensa,sa,Y,pbun,yhensa;
nmax=100;/*ステップ数*/
/*ノイズの特定化*/
mu=0.0;
mu2=0.0;
sigma2=1.0;
sigma3=1.0;
jmax=1000; /*粒子数*/
/*尤度の計算で使用*/
a1=1.0/(sqrt(2.0*M_PI))/sigma3;
a2=1.0;/*Gの偏微分の絶対値*/
n=0;
x[0]=0.0;
/*データ作成*/
for(n=1;n＜nmax;++n) {
v1=gauss(mu,sigma2);/*vの生成*/
w1=gauss(mu2,sigma3);/*wの生成*/
/*システムモデル*/
x[n]=x[n-1]+v1;
/*観測値モデル*/
y1[n]=x[n]+w1;
/*printf("%d %lf %lf\n",n,x[n],y1[n]);*/
}
/* +-30で一様分布にとる */
for (j=0;j＜jmax;++j) {
f[0][j]=(60.0/jmax)*j-30.0;
}
for (n=1;n＜nmax;++n) {
A=0.0;
for (j=0;j＜jmax;++j) {
v[j]=gauss(mu,sigma2);
w[j]=gauss(mu2,sigma3);
p[n][j]=f[n-1][j]+v[j];
y[n][j]=p[n][j]+w[j];
alfa[j]=pdf((y1[n]-p[n][j]),mu2,sigma3)*a2;
A+=alfa[j];
}
A1=1.0/A;
a[0]=0;
for(j=0;j＜jmax;++j){
a[j+1]=a[j]+alfa[j]*A1;
}
for(n1=0;n1＜n;++n1){
for(j=0;j＜jmax;++j){
fh[n1][j]=f[n1][j];
}
}
/*リサンプリング*/
for(j=0;j＜jmax;++j){
ran=ran2(&idum);
for(l=0;l＜jmax;++l){
if(ran＜a[l+1] && a[l]＜ran){
k=l;
}
}
f[n][j]=p[n][k];
/*固定区間平滑化*/
nend=n-1-LL;
if(nend＜0)nend=0;
for(n1=n-1;n1>nend;--n1){
f[n1][j]=fh[n1][k];
}
}
}
/*平均と標準偏差*/
for(n=1;n＜nmax;++n){
fwa=0.0;bun=0.0;
for(j=0;j＜jmax;++j){
fwa+=f[n][j];
}
fa=fwa/jmax;
for(j=0;j＜jmax;++j){
bun+=pow(f[n][j]-fa,2.0);
}
hensa=sqrt(bun/jmax);
/*printf("%d %lf %lf %lf\n",n,fa,hensa,fa-x[n]);*/
}
for(n=1;n＜nmax;++n){
Y=0.0;pbun=0.0;sa=0.0;
for(j=0;j＜jmax;++j){
Y+=p[n][j];
}
for(j=0;j＜jmax;++j){
pbun+=pow(p[n][j]-(Y/jmax),2.0);
}
sa=(Y/jmax)-y1[n]; /*予測観測値の平均と理論値の差*/
printf("%d %lf %lf %lf\n",n,Y/jmax,sqrt(pbun/jmax),sa);
}
}
/*表示関数*/
void showData(double d[], int num)
{
int i;
for (i=0; i＜num;++i ) {
printf("%lf ", d[i]);
}
printf("\n");
}
/*結果確認*/
void showParticle(double f[], double v[], double w[], double p[], double y[], double alpha[], int num)
{
printf("f\n");
showData(f,num);
printf("v\n");
showData(v,num);
printf("w\n");
showData(w,num);
printf("p\n");
showData(p,num);
printf("y\n");
showData(y,num);
printf("alpha\n");
showData(alpha,num);
}
/*確率密度関数 (ここでは正規分布)*/
double pdf(double x, double mu, double sigma) {
return exp(-pow((x-mu),2.0)/(2.0*sigma*sigma))/(sqrt(2.0*M_PI)*sigma);
}
/*ノイズが従う分布 (ここでは正規分布)*/
double gauss(double mu, double sigma2)
{
double t,u,r1,r2;
double sigma=sqrt(sigma2);
t=sqrt(-2.0*log(1.0-ran2(&idum)));
u=2.0*PI*ran2(&idum);
r1=t*cos(u);
return r1*sigma+mu;
}
/*Random Number*/
double ran2(long *idum)
{
int j;
long k;
static long idum2=123456789;
static long iy=0;
static long iv[NTAB];
float temp;
if(*idum <= 0){
if(-(*idum) < 1) *idum=1;
else *idum = -(*idum);
idum2=(*idum);
for(j=NTAB+7; j>=0; j--){
k=(*idum)/IQ1;
*idum=IA1*(*idum-k*IQ1)-k*IR1;
if(*idum < 0) *idum += IM1;
if(j < NTAB) iv[j] = *idum;
}
iy=iv[0];
}
k=(*idum)/IQ1;
*idum=IA1*(*idum-k*IQ1)-k*IR1;
if(*idum < 0) *idum += IM1;
k=idum2/IQ2;
idum2=IA2*(idum2-k*IQ2)-k*IR2;
if(idum2 < 0) idum2 += IM2;
j=iy/NDIV;
iy=iv[j]-idum2;
iv[j]=*idum;
if(iy<1) iy += IMM1;
if((temp = AM*iy) > RNMX) return RNMX;
else return temp;
}
[出力結果]
step 予測観測の平均 偏差 理論値との差 1 0.017988 17.373540 -0.003108 2 0.025417 1.344455 0.393198 3 -0.248597 1.277703 1.247872 4 -1.053549 1.277196 -0.769730 5 -0.569006 1.282229 4.513963 6 -3.242339 1.148304 -0.432243 7 -3.042069 1.250402 -1.913078 8 -1.807175 1.242435 -0.877669 9 -1.222287 1.256939 -2.122946 10 0.037819 1.335024 -2.122451 11 1.319921 1.294579 -3.332589 12 3.444641 1.298284 0.778013 13 2.956242 1.274834 0.990396 14 2.351089 1.269184 -0.372974 15 2.585562 1.240364 -0.437311 16 2.857707 1.244902 -1.110906 17 3.567647 1.281882 0.771209 18 3.043078 1.257063 -0.242051 19 3.243303 1.276396 1.616374 20 2.276833 1.252842 -0.956716 21 2.893434 1.283294 2.082407 22 1.600277 1.272351 -1.630626 23 2.534451 1.237347 -1.503776 24 3.430353 1.233525 1.257182 25 2.676560 1.236485 0.110273 26 2.620786 1.294432 -0.924263 27 3.189752 1.316879 -1.207734 28 3.977979 1.263428 -1.980239 29 5.139980 1.326943 0.754355 30 4.619487 1.337006 0.105379 31 4.564191 1.343342 0.644016 32 4.108552 1.262138 2.052912 33 2.848548 1.251307 1.922607 34 1.762655 1.290456 -3.690113 35 4.087774 1.286305 0.247373 36 3.987683 1.279503 0.588147 37 3.671689 1.289687 2.361987 38 2.154840 1.323967 2.082500 39 0.908723 1.272497 -0.206598 40 1.095729 1.257794 -0.306813 41 1.253017 1.253046 0.582154 42 0.808314 1.274043 0.322751 43 0.508966 1.298451 -2.901013 44 2.409413 1.284859 -0.735228 45 2.893339 1.287879 0.295820 46 2.758199 1.330833 -1.357038 47 3.592417 1.278142 0.404373 48 3.304833 1.281612 2.531589 49 1.758497 1.280394 0.392202 50 1.545999 1.310920 -1.056786 51 2.190174 1.267770 2.639912 52 0.595522 1.208428 5.103554 53 -2.521735 1.267895 -2.039339 54 -1.292546 1.302815 -0.032658 55 -1.255533 1.336068 1.814952 56 -2.424788 1.301066 1.313037 57 -3.217728 1.262489 0.166638 58 -3.330066 1.305039 0.247324 59 -3.503007 1.285833 -1.418116 60 -2.645417 1.267560 1.547358 61 -3.613956 1.302293 1.123600 62 -4.385690 1.308220 -0.800382 63 -3.898306 1.265432 -3.359117 64 -1.749634 1.248332 0.552694 65 -2.072527 1.306821 1.165875 66 -2.863412 1.296486 1.304900 67 -3.669239 1.243378 2.034793 68 -4.971230 1.249551 -2.323981 69 -3.538345 1.287443 0.811753 70 -4.081381 1.310238 -2.725708 71 -2.268759 1.274856 2.796109 72 -4.088349 1.280111 4.171715 73 -6.730337 1.338879 1.973857 74 -7.985089 1.234455 1.822011 75 -9.086156 1.242905 -0.820753 76 -8.618827 1.267814 0.335695 77 -8.830110 1.245011 -1.241534 78 -8.089841 1.268387 1.303797 79 -8.894709 1.270887 -0.716428 80 -8.467357 1.322398 1.281794 81 -9.332648 1.244045 1.207775 82 -10.065665 1.271964 -3.455282 83 -7.883522 1.268311 -1.029089 84 -7.283919 1.333828 1.598512 85 -8.281383 1.240839 -3.585497 86 -6.093962 1.314501 1.553135 87 -7.096817 1.262598 1.197301 88 -7.803457 1.284039 0.281371 89 -8.013835 1.254791 -1.133229 90 -7.334920 1.312391 0.464980 91 -7.626792 1.269901 0.302128 92 -7.890816 1.264972 -0.205159 93 -7.744679 1.316248 -0.247753 94 -7.545355 1.248156 2.743462 95 -9.283384 1.264485 -0.345352 96 -9.095572 1.299782 -3.329767 97 -7.045077 1.297091 2.719507 98 -8.649321 1.221566 -3.299159 99 -6.662245 1.226565 -0.206662
Back to C Language
