一、什么是卡尔曼滤波
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优递归数据处理算法）”。
在自然界中往往存在各种不确定性，不管是传感器测量的数据还是系统模型计算得到的数据，往往不是物体真实的值，存在各种各样的干扰，卡尔曼滤波就是从有干扰的数据中获取最优（最接近真实）的数据。
二、卡尔曼滤波基础
先来看一个简单的例子，我们用一把尺子来测一个硬币的直径，如图1：
我们测量多组硬币的直径，用
z
1
,
z
2...
z
k
z1,z2...zk
z1,z2...zk表示，
z
k
zk
zk表示第n次测量的结果，此时我们怎么获得硬币比较接近真实值的直径，在现实中我们测量是有误差的，这个误差可能来自测量器具本身或者其他干扰，并且这个误差一般是服从正态分布，很显然我们可以通过求平均来减小误差，从而得到更加逼近真实值的结果,即
x
k
^
=
1
k
(
z
1
+
z
2
+
z
3
+
.
.
.
+
z
k
)
\hat{x_{k}}=\frac{1}{k}\left ( z_{1}+z_{2}+z_{3}+...+z_{k}\right )
xk^=k1(z1+z2+z3+...+zk)
将
z
k
zk
zk提出来，即
x
k
^
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
1
k
⋅
k
−
1
k
−
1
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
⋅
1
k
−
1
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
⋅
x
k
−
1
^
+
1
k
z
k
=
x
k
−
1
^
+
1
k
(
z
k
−
x
k
−
1
^
)
\begin{aligned} \hat{x_{k}}&=\frac{1}{k}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{1}{k}\cdot \frac{k-1}{k-1}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{k-1}{k}\cdot \frac{1}{k-1}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{k-1}{k}\cdot \hat{x_{k-1}}+\frac{1}{k}z_{k}\\ &=\hat{x_{k-1}}+\frac{1}{k}(z_{k}-\hat{x_{k-1}}) \end{aligned}
xk^=k1(z1+z2+...+zk−1)+k1zk=k1⋅k−1k−1(z1+z2+...+zk−1)+k1zk=kk−1⋅k−11(z1+z2+...+zk−1)+k1zk=kk−1⋅xk−1^+k1zk=xk−1^+k1(zk−xk−1^)
其中，
x
k
^
\hat{x_{k}}
xk^是我们计算的平均值，我们用此值估计硬币的直径，也可以将此值称为估计值，所以公式也可以表达如下：
当前的估计值=上一次的估计值+系数*（当前测量值-上次估计值）
我们分析一下公式，当k变大时，即测量次数变多时，
x
k
^
\hat{x_{k}}
xk^会随着k的增大而越发逼近
x
k
−
1
^
\hat{x_{k-1}}
xk−1^，也就是说，随着测量次数增加，测量结果将不再重要；当k很小时（k是测量次数，所以k>1），测量值对
x
k
^
\hat{x_{k}}
xk^的影响很大。我们令
K
k
=
1
k
K_{k}=\frac{1}{k}
Kk=k1
则
x
k
^
=
x
k
−
1
^
+
K
k
(
z
k
−
x
k
−
1
^
)
\hat{x_{k}}=\hat{x_{k-1}}+K_{k}(z_{k}-\hat{x_{k-1}})
xk^=xk−1^+Kk(zk−xk−1^)
我们往往把这个
K
k
K_{k}
Kk称为卡尔曼增益，这时大家可能有疑惑，这不就是一个求平均的递归形式嘛，算什么卡尔曼滤波，你这就是标题党。大家稍安勿躁，我们接着往下看
还是刚刚求硬币直径的问题，在上一个例子中，我们使用一把尺子来测硬币直径，我觉得一把尺子测得不准，这回我拿两把尺子，第一把尺子测得硬币直径z1=30mm,第二把尺子测得硬币直径z2=32mm，假定这两把尺子测得都有误差，且误差服从正态分布，第一把尺子的标准差为
σ
1
=
2
m
m
\sigma _{1}=2mm
σ1=2mm,第二把尺子标准差为
σ
2
=
4
m
m
\sigma _{2}=4mm
σ2=4mm,此时我们怎么得到比较准确的硬币直径？两尺子测量值的正态分布图如下
我们此时为了得到更准确的硬币直径，参考上一个例子求平均的公式,可以得到
z
^
=
z
1
+
K
k
(
z
2
−
z
1
)
,
(
K
k
∈
[
0
,
1
]
)
\hat{z}=z_{1}+K_{k}(z_{2}-z_{1}),(K_{k}\in \left [ 0,1 \right ])
z^=z1+Kk(z2−z1),(Kk∈[0,1])当
K
k
=
0
K_{k}=0
Kk=0时，
z
^
=
z
1
\hat{z}=z_{1}
z^=z1,当
K
k
=
1
K_{k}=1
Kk=1时，
z
^
=
z
2
\hat{z}=z_{2}
z^=z2，选择合适的
K
k
K_{k}
Kk可以得到更逼近真实值的值，这个问题由此转变为求优的问题。为了使估计值
z
^
\hat{z}
z^更接近真实值，那么
z
^
\hat{z}
z^的标准差
σ
z
^
\sigma _{\hat{z}}
σz^必须最小，才能得到更准确的值。
z
^
\hat{z}
z^的方差可以表示为
σ
z
^
2
=
v
a
r
(
z
1
+
K
k
(
z
2
−
z
1
)
)
=
v
a
r
(
(
1
−
K
k
)
z
1
+
K
k
z
2
)
\begin{aligned} \sigma _{\hat{z}}^{2}&=var(z_{1}+K_{k}(z_{2}-z_{1}))\\ &=var((1-K_{k})z_{1}+K_{k}z_{2}) \end{aligned}
σz^2=var(z1+Kk(z2−z1))=var((1−Kk)z1+Kkz2)又因为
z
1
z_{1}
z1与
z
2
z_{2}
z2相互独立，所以上式
=
v
a
r
(
(
1
−
K
k
)
z
1
)
+
v
a
r
(
K
k
z
2
)
=
(
1
−
K
k
)
2
⋅
v
a
r
(
z
1
)
+
K
k
2
⋅
v
a
r
(
z
2
)
=
(
1
−
K
k
)
2
σ
1
2
+
K
k
2
σ
2
2
\begin{aligned} &=var((1-K_{k})z_{1})+var(K_{k}z_{2})\\ &=(1-K_{k})^{2}\cdot var(z_{1})+K_{k}^{2}\cdot var(z_{2})\\ &=(1-K_{k})^{2}\sigma _{1}^{2}+K_{k}^{2}\sigma _{2}^{2} \end{aligned}
=var((1−Kk)z1)+var(Kkz2)=(1−Kk)2⋅var(z1)+Kk2⋅var(z2)=(1−Kk)2σ12+Kk2σ22相当于求二次函数最小值，我们可以解出此时
K
k
=
σ
1
2
σ
1
2
+
σ
2
2
=
0.2
K_{k}=\frac{\sigma _{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2} }=0.2
Kk=σ12+σ22σ12=0.2
z
^
=
30
+
0.2
×
(
32
−
30
)
=
30.4
m
m
\hat{z}=30+0.2\times (32-30)=30.4mm
z^=30+0.2×(32−30)=30.4mm
σ
z
^
2
=
3.2
\sigma _{\hat{z}}^{2}=3.2
σz^2=3.2,此时我们可以看到标准差只有
3.2
\sqrt{3.2}
3.2,小于
σ
1
\sigma _{1}
σ1和
σ
2
\sigma _{2}
σ2,说明此时估计值的可信度是高于两个传感器的测量值，即估计值
z
^
\hat{z}
z^更接近真实值，通过这种算法，我们从两个具有不同误差的传感器中获得了更加准确的值，这就是所谓的数据融合（Data Fusion）,将估计值曲线用matlab画出(中间黄色的曲线)
记住这个思想，接下来要开始我们的重头戏了，卡尔曼滤波五大公式的推导。（明天更新，学识有限，欢迎各位大佬批评指正）
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
