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