卡尔曼滤波在单片机上的使用.docx

卡尔曼滤波在单片机上的使用
2011-04-13 13:43:43| 分类： 默认分类 | 标签： |字号大中小 订阅

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