Kalman滤波在单片机上的使用

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