#ifndef _KALMAN_H_
#define _KALMAN_H_
extern KalmanGain;// 卡爾曼增益
extern EstimateCovariance;//估計(jì)協(xié)方差
extern MeasureCovariance;//測量協(xié)方差
extern EstimateValue;//估計(jì)值
extern void KalmanFilterInit(void);
extern KalmanFilter( Measure);
#endif
#include "config.h"
#include "math.h"
KalmanGain;// 卡爾曼增益
EstimateCovariance;//估計(jì)協(xié)方差
MeasureCovariance;//測量協(xié)方差
EstimateValue;//估計(jì)值
void KalmanFilterInit(void);
extern float KalmanFilter(float Measure);
void KalmanFilterInit(void)
{
EstimateValue=0;
EstimateCovariance=0.1;
MeasureCovariance=0.02;
}
KalmanFilter( Measure)
{
//計(jì)算卡爾曼增益
KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance))；
//計(jì)算本次濾波估計(jì)值
EstimateValue=EstimateValue+KalmanGain*（Measure-EstimateValue）；
//更新估計(jì)協(xié)方差
EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance;
//更新測量方差
MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance;
//返回估計(jì)值
return EstimateValue;
}