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

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

-电子元器件采购网（www.ruidan.com）是本土元器件目录分销商，采用“小批量、现货、样品”销售模式，致力于满足客户多型号、高质量、快速交付的采购需求。 自建高效智能仓储，拥有自营库存超过50,000种，提供一站式正品现货采购、个性化解决方案、选型替代等多元化服务。