简单卡尔曼滤波【转】

来源：互联网发布：百度大数据分析编辑：程序博客网时间：2024/05/21 17:54

来自：http://blog.csdn.net/calcular/article/details/47909275

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;}}

0 0