#include"time.h"
#include"stdio.h"
#include"stdlib.h"
#include"conio.h"
#include"math.h"
//Initialisation.
//===============
#define PI 3.1415
#define N 50
// Number of time steps
#define Q 10
// Process noise variance
#define R 1
// Measurement noise variance
#define x0 0.1
//Initial state
#define initVar 5
#define numSamples 500
// Number of Monte Carlo samples per time step
#define LEN
sizeof(double)
void main()
{
double Normal(double x);
double NormalRandom();
double AverageRandom(double min, double max);
void bootstrap(double **x1, double **q1, double yy[]);
void predictstates(double *p, int tt);
void importanceweights(double *pointer_1, double *pointer_2, double yy);
void updatestates(double *pointer_3, double *pointer_1, double *pointer_2);
int i, j, t;
double xx[N];
// Hidden states
double v[N],w[N];
double y[N];
// Observations
FILE *fp1, *fp2;
double **x, **q;
x=(double **)malloc(N*sizeof(double*));
// The estimated state samples
q=(double **)malloc(N*sizeof(double*));
for(i=0;i<N;i++)
{
x[i]=(double*)malloc(numSamples*LEN);
// The estimated state samples
q[i]=(double*)malloc(numSamples*LEN);
}
xx[0] = x0;
// Initial state
fp1=fopen("d:\\output_x.txt","wt+");
// Open a file
fp2=fopen("d:\\output_q.txt","wt+");
// NormalRandom() is a function which generate a random number that obeys Gauss distribute.
// Generate process and measurement noise.
//==========================================================================================
srand(time(NULL));
for(i=0;i<N;i++)
{
v[i]=NormalRandom();
// v[50];Generate measurement noise
w[i]=sqrt(10)*NormalRandom();
// w[50];Generate process noise
}
//Generate state and measurements.
// ===============================
y[0]=(xx[0]*xx[0])/20 + v[0];
for(t=1;t<N;t++)
{
xx[t]=0.5*xx[t-1]+25*xx[t-1]/(1+xx[t-1]*xx[t-1])+8*cos(1.2*(t-1))+w[t];
y[t]=xx[t]*xx[t]/20+v[t];
}
//Perform sequential Monte Carlo filtering.
//=========================================
bootstrap(x, q, y);
//Result output a file.
if(fp1==NULL)
{
printf("cann't open this file\n");
return;
}
for(i=0;i<N;i++)
{
for(j=0;j<numSamples;j++)
fprintf(fp1,"%10.4f ", x[i][j]);
fputs("\n",fp1);
fputs("\n",fp1);
fputs("\n",fp1);
}
if(fp2==NULL)
{
printf("cann't open this file\n");
return;
}
for(i=0;i<N;i++)
{
for(j=0;j<numSamples;j++)
fprintf(fp2,"%10.4f ", q[i][j]);
fputs("\n",fp2);
fputs("\n",fp2);
fputs("\n",fp2);
}
free(x);
free(q);
}
//Generate a uniform random number range from min to max.
//=======================================================
double AverageRandom(double min, double max)
{
int minInteger=(int)(min*10000);
int maxInteger=(int)(max*10000);
int randInteger=rand()*rand();
int diffInteger=maxInteger-minInteger;
int resultInteger=randInteger%diffInteger+minInteger;
double result=resultInteger/10000.0;
return result;
}
//Generate a normal Gauss random number.
//======================================
double Normal(double x)
//Probility Density Function
{
return 1.0/sqrt(2*PI) * exp(-1*(x*x)/2);
}
//Generate a random number that is normal distribution
double NormalRandom()
{
double x;
double dScope;
double y;
do
{
x = AverageRandom(-3,3);
y = Normal(x);
dScope = AverageRandom(0, Normal(0));
}while( dScope > y);
return x;
}
// Bootstrap algorithm.
//=====================
void bootstrap(double **x1, double **q1, double yy[])
{
int i, t;
int rows = N;
double *xu;
xu=(double *)malloc(numSamples*LEN);
// The estimated state samples
// Sample from the prior.
//=======================
srand(time(NULL));
for(i=0;i<numSamples;i++)
{
x1[0][i]=sqrt(initVar)*NormalRandom();
// x1[0]随机产生500个粒子
xu[i]=x1[0][i];
}
// Update and prediction stages.
// =============================
for(t=0;t<rows-1;t++)
{
predictstates(xu, t);
importanceweights(xu, q1[t+1], yy[t+1]);
updatestates(x1[t+1], xu, q1[t+1]);
for(i=0;i<numSamples;i++)
xu[i]=x1[t+1][i];
}
}
//Performs the prediction step of the sequential SIR algorithm for
//the model described in the file sirdemo1.m
//===============================================================
void predictstates(double *p, int tt)
{
int j;
double temp;
double *w;
w=(double *)malloc(numSamples*LEN);
srand(time(NULL));
for(j=0;j<numSamples;j++)
{
w[j]=sqrt(Q)*NormalRandom();
temp=*(p+j);
temp=0.5*temp+25*temp/(1+temp*temp)+8*cos(1.2*tt)+w[j];
*(p+j)=temp;
}
free(w);
}
//Computes the normalised importance ratios
//===========================================
void importanceweights(double *pointer_1, double *pointer_2, double yy)
{
int i, s;
double temp;
double sum=0;
double *m;
m=(double *)malloc(numSamples*LEN);
for(i=0;i<numSamples;i++)
{
temp=*(pointer_1+i);
m[i]=temp*temp/20;
sum+=exp(-(yy-m[i])*(yy-m[i])/(2*R));
}
for(s=0;s<numSamples;s++)
{
temp=*(pointer_2+s);
temp=(exp(-(yy-m[s])*(yy-m[s])/(2*R)))/sum;
*(pointer_2+s)=temp;
}
free(m);
}
//Performs the resampling step of the SIR algorithm
//==================================================
void updatestates(double *pointer_3, double *pointer_1, double *pointer_2)
{
int k;
double temp, temp1;
int i=0;
int j=0;
double *u, *t1, *T1, *Q1, *QT;
u=(double *)malloc(numSamples*LEN);
t1=(double *)malloc(numSamples*LEN);
T1=(double *)malloc(numSamples*LEN);
Q1=(double *)malloc(numSamples*LEN);
QT=(double *)malloc(numSamples*LEN);
u[0]=AverageRandom(0, 1);
t1[0]=-log(u[0]);
T1[0]=t1[0];
Q1[0]=*pointer_2;
srand(time(NULL));
for(k=1;k<=numSamples;k++)
{
u[k]=AverageRandom(0, 1);
t1[k]=-log(u[k]);
T1[k]=T1[k-1]+t1[k];
temp=*(pointer_2+k);
Q1[k]=Q1[k-1]+temp;
}
while(j<=numSamples)
{
for(k=0;k<numSamples;k++)
QT[k]=Q1[j]*T1[k];
if(QT[j]>T1[i])
{
temp1=*(pointer_1+j);
*(pointer_3+i)=temp1;
i=i+1;
}
else
j=j+1;
}
free(u);
free(t1);
free(T1);
free(Q1);
free(QT);
}
