卡尔曼滤波在单片机上的使用(共3页).docx

《卡尔曼滤波在单片机上的使用(共3页).docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波在单片机上的使用(共3页).docx（3页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、精选优质文档-倾情为你奉上卡尔曼滤波在单片机上的使用 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#inclu

2、de config.h#include math.hKalmanGain; /卡尔曼增益EstimateCovariance; /估计协方差MeasureCovariance; /测量协方差EstimateValue; /估计值void KalmanFilterInit( void );extern float KalmanFilter( float Measure );void KalmanFilterInit( void ) EstimateValue = 0;EstimateCovariance = 0.1;MeasureCovariance = 0.02;KalmanFilter( M

3、easure )/计算卡尔曼增益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;专心-专注-专业