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

参考：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("初探迭代算法");

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

效果是不是蛮不错的，随着迭代次数的增加，它真的是收敛到了我们预设的真实值~

 又介绍了数据融合的例子，这也是卡尔曼滤波器最核心的本质

 接着进行了长篇幅复杂而严密的推导，得到了卡尔曼滤波的五个核心公式

 

最后，举了一个人在匀速运动时的一个例子，我们对其进行了二维卡尔曼滤波

 接下来，我们用matlab仿真实现这一例子

excel中的datareal数据如下：

0.000	1.000
0.823	0.650
1.508	0.888
2.738	1.158
3.993	0.966
5.547	0.217
6.090	0.281
6.373	-0.101
6.593	0.466
7.380	0.587
8.529	0.454
9.073	0.509
10.274	0.917
11.277	0.954
12.319	1.418
14.029	1.189
15.266	0.975
16.319	1.138
17.643	1.110
18.705	0.505
19.409	0.734
20.806	0.449
21.303	0.224
21.373	0.351
21.787	0.644
22.840	0.500
23.572	0.395
24.250	-0.057
24.059	-0.445
23.601	-0.260
22.732	0.353

代码如下：

clear all;clc;close all;
%randn产生均值为0，方差为1的矩阵
data_real=xlsread('datareal.xlsx');%读取数据
%参数设置
iters=31;
A=[1 1;
    0 1];
H=[1 0;
    0 1];
%产生误差，并生成Q R矩阵，可以调节W V前面系数，看不同情况下的拟合情况
W=0.1*randn(2,iters);
V=0.5*randn(2,iters);
Q=[std(W(1,:)).^2 0;
    0 std(W(2,:)).^2];
R=[std(V(1,:)).^2 0;
    0 std(V(2,:)).^2];
disp("Q矩阵为：")
disp(Q);
disp("R矩阵为：")
disp(R);
X=zeros(2,iters);
X(:,1)=[0;1];
X_pre=zeros(2,iters);
X_pre(:,1)=[0;1];
Z=data_real'+V;
P=[1 0;0 1];%初始化
for k = 2:iters
    X_pre(:,k)=A*X(:,k-1);
    Pk_pre=A*P*A'+Q;
    K=Pk_pre*H'/(H*Pk_pre*H'+R);
    X(:,k)=X_pre(:,k)+K*(Z(:,k)-H*X_pre(:,k));
    P=(1-K*H)*Pk_pre;
end
t=1:iters;

%绘图
figure(1);
plot(t,data_real(:,1),"LineWidth",2);hold on;
plot(t,X(1,:),"LineWidth",2);plot(t,X_pre(1,:),"LineWidth",2);
plot(t,Z(1,:),"LineWidth",2);
legend('真实值','估计值','先验估计值','测量值')
title("位置");

figure(2);
plot(t,data_real(:,2),"LineWidth",2);
hold on;
plot(t,X(2,:),"LineWidth",2);plot(t,X_pre(2,:),"LineWidth",2);
plot(t,Z(2,:),"LineWidth",2);
legend('真实值','估计值','先验估计值','测量值')
title("速度");

%计算速度的均方误差
err1=sum((data_real(:,2)'-X(2,:)).^2/iters);
err2=sum((data_real(:,2)'-X_pre(2,:)).^2/iters);
err3=sum((data_real(:,2)'-Z(2,:)).^2/iters);

效果如下：

 

 理论上，橙色的线应该与蓝色的线更加接近的，但是从图上看并不明显，我在代码中加了一部分求均方差(速度)的代码，err1对应估计值与真实值之差，err2代表预测值与真实值之差，err3代表观测值与真实值之差

 可见，大部分情况下，err1最小，说明滤波起到了一定的作用。

还有一点要注意，

由于这是我们的仿真，因此协方差矩阵是已知的，但是实际应用中，有可能不知道协方差矩阵，这就需要我们进一步在相关领域学习，来不断调节Q R这两个超参数，从而找到最优估计。