标题: 卡尔曼滤波 [打印本页]

---

作者: 15702422216    时间: 2016-8-10 12:14
标题: 卡尔曼滤波

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

}

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1