卡尔曼滤波器 | 理论简析加案例代码（总结）

最新推荐文章于 2025-04-17 11:00:34 发布

原创

最新推荐文章于 2025-04-17 11:00:34 发布 · 1.5k 阅读

标签

#算法 #matlab

本文深入探讨卡尔曼滤波器的基本概念与应用，通过实例演示如何使用MATLAB实现卡尔曼滤波，包括一维与二维场景下的数据融合与状态估计。

参考：b站Dr_can的视频讲解

卡尔曼滤波器可以描述为：一种最优化、递归、数字处理的算法

主要解决各种不确定性问题：

1.不存在完美的数学模型

2.系统的扰动不可控

3.传感器本身也存在一定误差

因而我们需要利用一定的理论，来对数据进行融合处理，得到最优估计值

可概括为：

当前的估计值=上一次的估计值+系数*（当前测量值-上一次估计值）

这个系数我们常称为：卡尔曼增益，它是我们进行卡尔曼滤波的核心

两个误差：估计误差、测量误差；可以利用这两个误差求算卡尔曼增益

在视频中，举了一个多次测量硬币的案例，来初探递归算法的原理，假设硬币的真实直径是50mm,经历了数次观测后，估计真实厚度

已知条件：真实直径50mm,随便设一个初始估计值为40mm,设观测值z是一个围绕50上下波动不超过3mm的值，估计误差为5，观测误差为3，根据理论估计的步骤可概括如下：

博主是用excel做的，我这里根据原理利用matlab重新做了一次

代码如下：

clear all;clc;close all;
iters=300;
z=(6*rand(iters,1)-3)+50;
eMEA=3;
eEST=5;
X=zeros(iters,1);
X_real=50*ones(1,iters);
X(1)=40;
for i=2:iters
    K=eEST/(eMEA+eEST);
    X(i)=X(i-1)+K*(z(i)-X(i-1));
    eEST=(1-K)*eEST;
end
t=1:iters;
figure(1);
plot(t,X,"LineWidth",2);xlabel("迭代次数");ylabel("测量值");axis([0 iters 40 55]);
hold on;plot(t,X_real,"LineWidth",1);legend("测量值","真实值");title("初探迭代算法");

其实很简单，就完全跟着上一张图片列出的步骤编写就可以，最终绘制出一张效果图，如下：