久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標題: 卡爾曼濾波 [打印本頁]

作者: 15702422216 時間: 2016-8-10 12:14
標題: 卡爾曼濾波

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

}

歡迎光臨 (http://m.zg4o1577.cn/bbs/)