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

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

原文出处

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