简单卡尔曼滤波【转】

来自： 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;
	}
}