[单片机分享] 教你在单片机上套公式实现卡尔曼滤波器

http://bbs.elecfans.com/jishu_484132_1_1.html

逛博客看到的一篇讲解卡尔曼滤波的文章，发现程序十分简洁易懂，而且使用效果不错，是一个学习的典范，在此分享了！

#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;
}