您的位置 首页 电路

卡尔曼滤波在单片机上的运用

ifndef_KALMAN_H_define_KALMAN_H_externKalmanGain;卡尔曼增益externEstimateCovariance;估计协方差externMeasureCov






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

声明:本文内容来自网络转载或用户投稿,文章版权归原作者和原出处所有。文中观点,不代表本站立场。若有侵权请联系本站删除(kf@86ic.com)https://www.86ic.net/fangan/dianlu/255665.html

为您推荐

联系我们

联系我们

在线咨询: QQ交谈

邮箱: kf@86ic.com

关注微信
微信扫一扫关注我们

微信扫一扫关注我们

返回顶部