#ifndef _KALMAN_H_ #define _KALMAN_H_ extern KalmanGain;// 卡爾曼增益 extern EstimateCovariance;//估計協(xié)方差 extern MeasureCovariance;//測量協(xié)方差 extern EstimateValue;//估計值 extern void KalmanFilterInit(void); extern KalmanFilter( Measure); #endif #include "config.h" #include "math.h" KalmanGain;// 卡爾曼增益 EstimateCovariance;//估計協(xié)方差 MeasureCovariance;//測量協(xié)方差 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）； //更新估計協(xié)方差 EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance; //更新測量方差 MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance; //返回估計值 return EstimateValue; }
| 
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
