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

卡尔曼滤波在单片机上的使用2011-04-1313:43:43|分类：默认分类|标签：|字号大中小订阅#ifndef_KALMAN_H_#define_KALMAN_H_externKalmanGain;//卡尔曼增益externEstimateCovariance;//估计协方差externMeasureCovariance;//测量协方差externEstimateValue;//估计值externvoidKalmanFilterInit(void);externKalmanFilter(Measure);#endif#includeconfig.h#includemath.hKalmanGain;//卡尔曼增益EstimateCovariance;//估计协方差MeasureCovariance;//测量协方差EstimateValue;//估计值voidKalmanFilterInit(void);externfloatKalmanFilter(floatMeasure);voidKalmanFilterInit(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;//返回估计值returnEstimateValue;}