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

#ifndef _KALMAN_H_ #define _KALMAN_H_ extern KalmanGain;// 卡尔曼增益 extern EstimateCovariance;//估计协方差 extern MeasureCovariance;//测量协方差 extern EstimateValue;//估计值 extern void KalmanFilterInit(void); extern #endif KalmanFilter( Measure); #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+MeasureCov ariance*MeasureCovariance))； //计算本次滤波估计值 EstimateValue=EstimateValue+KalmanGain*（Measure-EstimateValue）； //更新估计协方差 EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance; //更新测量方差 MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance; //返回估计值 return EstimateValue; }