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

关注

转载最新推荐文章于 2026-08-18 11:00:59 发布 1w 阅读 AI 写同款· GEO 优化 ›

（一）C语言嵌入式开发单片机通用KALMAN滤波一直以来用过很多次KALMAN滤波，没有进行系统的总结，本次笔记将对此进行从原理到推导到代码实现及应用进行总结和分析，阐述一些个人使用的观点。首先，了解该滤波的原理及使用场景和应用范畴，然后，说明使用此滤波要达到的目的，之后，代码中如何加入该滤波模块及其调节增益，最后分析其带来的优缺点。原理：KALMAN滤波器是一种线性的离散时间的有限维系统。卡尔曼滤波器由一系列递归数学公式描述。它们提供了一... 阅读详情

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

原文出处

这里写图片描述

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