原帖地址： http://blog.163.com/tianjunqiang666@126/blog/static/87259119201131314343274/
         #ifndef _KALMAN_H_
#define _KALMAN_H_ extern  KalmanGain;//  卡尔曼增益
extern  EstimateCovariance;//估计协方差
extern  MeasureCovariance;//测量协方差
extern  EstimateValue;//估计值
extern void KalmanFilterInit(void);
extern      KalmanFilter(   Measure);
#endif   #include "config.h"
#include "math.h"   KalmanGain;//  卡尔曼增益
  EstimateCovariance;//估计协方差
  MeasureCovariance;//测量协方差
  EstimateValue;//估计值 void KalmanFilterInit(void); extern    float  KalmanFilter(float   Measure);
void KalmanFilterInit(void)
{
EstimateValue=0; EstimateCovariance=0.1;
 MeasureCovariance=0.02;
}
 
 KalmanFilter(   Measure)
{ //计算卡尔曼增益
KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance))； //计算本次滤波估计值
EstimateValue=EstimateValue+KalmanGain*（Measure-EstimateValue）；
//更新估计协方差 EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance;
//更新测量方差
MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance;
//返回估计值
return EstimateValue;
}