卡尔曼滤波在单片机上的使用
原文出处
#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;
}