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

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