Kalman滤波在单片机上的使用

来源：互联网发布：算法的乐趣.pdf 编辑：程序博客网时间：2024/05/18 21:47

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

0 0