程序十分简洁易懂，而且使用效果不错，分享
#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; }
复制代码
/3 
