在单片机上实现卡尔曼滤波

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

    }