程序十分简洁易懂,而且使用效果不错,分享
#ifndef _KALMAN_H_
#define _KALMAN_H_
externEstimateCovariance;//估计协方差
externMeasureCovariance;//测量协方差
externEstimateValue;//估计值
extern void KalmanFilterInit(void);
extern KalmanFilter( Measure);
#endif
#include "config.h"
#include "math.h"
KalmanGain;//卡尔曼增益
EstimateCovariance;//估计协方差
MeasureCovariance;//测量协方差
EstimateValue;//估计值
void KalmanFilterInit(void);
extern floatKalmanFilter(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;
}
-
单片机
+关注
关注
6036文章
44556浏览量
635007 -
程序
+关注
关注
117文章
3786浏览量
81023 -
卡尔曼滤波
+关注
关注
3文章
165浏览量
24650
原文标题:在单片机上实现卡尔曼滤波
文章出处:【微信号:mcu168,微信公众号:硬件攻城狮】欢迎添加关注!文章转载请注明出处。
发布评论请先 登录
相关推荐
评论