卡爾曼濾波在單片機(jī)上的使用

#ifndef _KALMAN_H_
#define _KALMAN_H_

extern  KalmanGain;//  卡爾曼增益
extern  EstimateCovariance;//估計(jì)協(xié)方差
extern  MeasureCovariance;//測(cè)量協(xié)方差
extern  EstimateValue;//估計(jì)值
extern void KalmanFilterInit(void);
extern      KalmanFilter(   Measure);
#endif

#include "config.h"
#include "math.h"

  KalmanGain;//  卡爾曼增益
  EstimateCovariance;//估計(jì)協(xié)方差
  MeasureCovariance;//測(cè)量協(xié)方差
  EstimateValue;//估計(jì)值

void KalmanFilterInit(void);

extern    float  KalmanFilter(float   Measure);

void KalmanFilterInit(void)
{
EstimateValue=0;

EstimateCovariance=0.1;
 MeasureCovariance=0.02;

}
 
 KalmanFilter(   Measure)
{

//計(jì)算卡爾曼增益
KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance))；

//計(jì)算本次濾波估計(jì)值
EstimateValue=EstimateValue+KalmanGain*（Measure-EstimateValue）；
//更新估計(jì)協(xié)方差

EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance;
//更新測(cè)量方差
MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance;
//返回估計(jì)值
return EstimateValue;
}