简单卡尔曼滤波

double filter(double in)
{
static int co;
co++;
static double dat0; //现在的数据
static double dat1; //前一次的数据
static double dat2; //前两次的数据
static double dat3; //前三次的数据
if(co>4)
{
co=999;
//滤波实现部分===========================
double prd0; //本次预测值
double prd1;  //上次预测值
double result; //本次测量结果
double de;
static double p_dx; //上一次最优偏差
double gz; //高斯噪声
double kg; //协方差
prd1=dat1-dat2+dat1;  //计算上次预测值
de=dat0-prd1;  //算出上次预测偏差
prd0=dat0-dat1+dat0;  //计算本次预测值

gz=sqrt(p_dx*p_dx+de*de+0.00000001);
kg=gz*gz/(gz*gz+de*de+0.00000001);
result=prd0+kg*(in-prd0);
p_dx=sqrt((1.0-kg)*gz*gz);

//=======================================
dat3=dat2;
dat2=dat1;
dat1=dat0;
dat0=result;
//printf("%f %f %f %f\n",dat0,dat1,dat2,dat3);
return result;
}
else
{
if(co==1) dat3=in;
else if(co==2) dat2=in;
else if(co==3) dat1=in;
else if(co==4) dat0=in;
//printf("%f %f %f %f\n",dat0,dat1,dat2,dat3);
return in;
}
}