Kalman滤波在单片机上的使用

最新推荐文章于 2024-07-12 08:20:21 发布

cp32212116

最新推荐文章于 2024-07-12 08:20:21 发布

阅读量1.3k

点赞数

分类专栏：算法参悟文章标签： Kalman

本文链接：https://blog.csdn.net/cp32212116/article/details/40659371

版权

算法参悟专栏收录该内容

18 篇文章 1 订阅

订阅专栏

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