教你在单片机上套公式实现卡尔曼滤波器

2019-04-15 17:35发布

站内文章 / PIC单片机
11970 0 1749








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

热门文章