Kalman滤波器笔记整理（一）：Kalman滤波器简单理解和引入
文章参考资料链接：
1.DR_CAN对Kalman滤波器的理解，非常通透！强推！
1.Introduction to Kalman Filter
对于自然界中某一个量的具体数值，往往可以通过两种手段来获取：通过系统分析和建模的方式计算得到，或者利用某种测量工具来进行测量得到。不管是哪一种方式，得到的结果都不可避免地会与实际值之间存在误差。系统建模时由于很难考虑环境中的过程噪声因素，因此建模计算的结果会因为忽略噪声的影响而产生偏差；同样地，测量器具也会存在测量噪声干扰，因此测量结果与实际值之间也会存在误差。
举个简单的例子，如果我们想要知道当前房间的室温是多少。有两种方法，一是对房间整体温度变化看成是一个系统来进行建模，在具体数学解析式中推算出当前的室温；二是利用温度计来对室温进行测量。如果采用建模的方法，由于存在太多不可控因素，我们往往不能获取室温变化的很准确的数学模型，因此此时得到的结果自然也是不准确的；而如果采用温度计测量的方法，则又会由于温度计本身精度问题或者环境温度的波动而使测量结果也是不准确的。
总的来说，不管采用哪种方法，总是不能获取真实值的准确大小，而只能得到一个与真实值相差不大的理论估计值或者实际测量值。在这种时候我们希望能够借由这两种方法得到的两个估计值，利用某种滤波算法对其进行融合和分析，采用某种最优化的方法，从而产生一个更接近于真实值的结果，这就是所谓Kalman滤波的方法。
首先我们先理解两个概念：递归（recursion）和数据融合（data fusion），考虑下面两个简单的例子。
2.Recursion
对某一个量进行了n次的测量，测量结果分别为
z
1
,
z
2
,
.
.
.
,
z
n
z_1,z_2,...,z_n
z1,z2,...,zn，要寻找一个合适的估计值，我们很自然地便知道应当去计算测量结果的平均值来作为估计值，于是
z
n
^
=
1
n
(
z
1
+
z
2
+
.
.
.
+
z
n
)
\widehat{z_n}=\frac{1}{n}(z_1+z_2+...+z_n)
zn
=n1(z1+z2+...+zn)
进一步对公式进行变化，我们可以得到
z
n
^
=
1
n
(
z
1
+
z
2
+
.
.
.
+
z
n
−
1
)
+
1
n
z
n
=
n
−
1
n
⋅
1
n
−
1
(
z
1
+
z
2
+
.
.
.
+
z
n
−
1
)
+
1
n
z
n
=
n
−
1
n
⋅
z
n
−
1
^
+
1
n
z
n
=
(
1
−
1
n
)
z
n
−
1
^
+
1
n
z
n
=
z
n
−
1
^
+
1
n
(
z
n
−
z
n
−
1
^
)
\begin{aligned} \widehat{z_n} &=\frac{1}{n}(z_1+z_2+...+z_{n-1})+\frac{1}{n}z_n\\ &=\frac{n-1}{n}·\frac{1}{n-1}(z_1+z_2+...+z_{n-1})+\frac{1}{n}z_n\\ &=\frac{n-1}{n}·\widehat{z_{n-1}}+\frac{1}{n}z_n\\ &=(1-\frac{1}{n})\widehat{z_{n-1}}+\frac{1}{n}z_n\\ &=\widehat{z_{n-1}}+\frac{1}{n}(z_n-\widehat{z_{n-1}})\\ \end{aligned}
zn
=n1(z1+z2+...+zn−1)+n1zn=nn−1⋅n−11(z1+z2+...+zn−1)+n1zn=nn−1⋅zn−1
+n1zn=(1−n1)zn−1
+n1zn=zn−1
+n1(zn−zn−1
)
其中
z
n
^
,
z
n
−
1
^
\widehat{z_n},\widehat{z_{n-1}}
zn
,zn−1
分别是前n次和前n-1次测量时的均值。在上面的公式我们可以看到，当n的取值很大时，
z
n
^
→
z
n
−
1
^
\widehat{z_{n}}\rightarrow\widehat{z_{n-1}}
zn
→zn−1
，说明当测量次数足够多时，多测量一次和少测量一次对估计结果的贡献不大，基本上已经趋于一个稳定的值，后一次测量的结果参考意义不大；而当测量次数n比较小时，多测量一次和少测量一次则会对估计结果有较大的影响，这时后一次测量的结果意义就较大了。
基于这一种递归的想法，我们令
1
n
=
K
k
\frac{1}{n}=K_k
n1=Kk，表示某种增益系数，则上式可以表示成
z
n
^
=
z
n
−
1
^
+
K
k
(
z
n
−
z
n
−
1
^
)
\widehat{z_n}=\widehat{z_{n-1}}+K_k(z_n-\widehat{z_{n-1}})
zn
=zn−1
+Kk(zn−zn−1
)
当
K
k
K_k
Kk取较小的值时，对于第n次的估计结果
z
n
^
\widehat{z_n}
zn
就主要参考第n-1次的估计
z
n
−
1
^
\widehat{z_{n-1}}
zn−1
，公式后一项所占的权重就比较小；而当
K
k
K_k
Kk取较大的数时，后一项占的权重就会较多，此时第n次的估计结果
z
n
^
\widehat{z_n}
zn
就更倾向于参考第n次的测量结果
z
n
z_n
zn.
极端地，
当 K k = 0 K_k=0 Kk=0时， z n ^ = z n − 1 ^ \widehat{z_n}=\widehat{z_{n-1}} zn =zn−1 ，完全参考前一次的估计值 z n − 1 ^ \widehat{z_{n-1}} zn−1 ，而不关心本次的测量值 z n z_n zn；
当 K k = 1 K_k=1 Kk=1时， z n ^ = z n \widehat{z_n}=z_n zn =zn，完全参考本次的测量值 z n z_n zn，而不关心前一次的估计值 z n − 1 ^ \widehat{z_{n-1}} zn−1 。
在实际的操作过程中，我们一般都想要最终的估计结果既要参考一部分前一次的估计值，又要参考一部分本次的测量值，两者结合起来以得到一个更为准确的估计结果。而至于两部分的权重谁大谁小，则要通过 K k K_k Kk值的选取来决定。如果我们更信任前一次的估计值，便可以取较小的 K k K_k Kk值；而如果我们更信任该次的测量值，便可以取较大的 K k K_k Kk值。
这便是Kalman滤波算法的主要思想，其中 K k K_k Kk在算法中被称为Kalman增益，是算法中极为重要的一个参数。Kalman滤波算法通过某种最优化指标来获取最优的 K k K_k Kk值，使得最终能够得到一个相对准确的估计结果。具体 K k K_k Kk值如何选取的详细数学推导和何为最优化指标的问题，都会在后面加以阐述。
3.Data Fusion
用两把尺子同时测量某一个物体的长度，测量结果和测量的标准差如下：
z
1
=
6.5
m
m
σ
1
=
0.2
m
m
z
2
=
7.3
m
m
σ
2
=
0.4
m
m
z_1=6.5mm\qquad\sigma_1=0.2mm\\ z_2=7.3mm\qquad\sigma_2=0.4mm\\
z1=6.5mmσ1=0.2mmz2=7.3mmσ2=0.4mm
我们怎么样结合两次测量的结果和标准差来得到一个较为准确的估计值呢？先直观分析一下，由于第一次测量的标准差较小，所以最终我们估计的结果应该更倾向于第一个测量结果，也就是6.5mm。所以我们利用前面例子所讲的增益公式来进行估计
z
^
=
z
1
+
K
k
(
z
2
−
z
1
)
\hat{z}=z_1+K_k(z_2-z_1)
z^=z1+Kk(z2−z1)
这里我们要选取合适的
K
k
K_k
Kk值，通过怎么样的标准来选取呢？我们如何评判一个估计结果的准确性？题目也已经给出了提示，我们可以通过估计结果的标准差，抑或是方差来衡量估计结果的准确性。而估计结果越准确，方差应该就越小。我们通过给出估计结果的方差的具体表达式，然后对
K
k
K_k
Kk求导以寻找极小值，不就可以得到一个合适的
K
k
K_k
Kk了吗？于是，我们便有
σ
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
]
=
(
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} \sigma_{\hat{z}}^2 &=Var[z_1+K_k(z_2-z_1)]\\ &=Var[(1-K_k)z_1+K_kz_2]\\ &=(1-K_k)^2Var[z_1]+K_k^2Var[z_2]\\ &=(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2 \end{aligned}
σz^2=Var[z1+Kk(z2−z1)]=Var[(1−Kk)z1+Kkz2]=(1−Kk)2Var[z1]+Kk2Var[z2]=(1−Kk)2σ12+Kk2σ22
σ
z
^
2
\sigma_{\hat{z}}^2
σz^2对
K
k
K_k
Kk进行求导，可以得到
d
σ
z
^
2
d
K
k
=
−
2
(
1
−
K
k
)
σ
1
2
+
2
K
k
σ
2
2
=
0
K
k
=
σ
1
2
σ
1
2
+
σ
2
2
=
0.
2
2
0.
2
2
+
0.
4
2
=
0.2
\begin{aligned} \frac{d\sigma_{\hat{z}}^2}{dK_k} &=-2(1-K_k)\sigma_1^2+2K_k\sigma_2^2=0\\ K_k&=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}\\ &=\frac{0.2^2}{0.2^2+0.4^2}=0.2 \end{aligned}
dKkdσz^2Kk=−2(1−Kk)σ12+2Kkσ22=0=σ12+σ22σ12=0.22+0.420.22=0.2
即当
K
k
K_k
Kk取0.2时，估计结果的方差最小，可以认为是最优估计。此时计算出估计结果为
z
^
=
z
1
+
K
k
(
z
2
−
z
1
)
=
6.5
+
0.2
×
(
7.3
−
6.5
)
m
m
=
6.66
m
m
\begin{aligned} \hat{z} &=z_1+K_k(z_2-z_1)\\ &=6.5+0.2×(7.3-6.5)\;mm\\ &=6.66\;mm \end{aligned}
z^=z1+Kk(z2−z1)=6.5+0.2×(7.3−6.5)mm=6.66mm
此时估计长度为6.66mm，也符合上面对于估计结果应该更倾向于6.5mm的推测。
这里的估计结果参考了两个测量数据，并且在它们之间进行了合适的权重匹配，来得到更为准确的估计结果。这就是所谓数据融合（Data fusion）的思想，在后面的Kalman滤波器中也有所体现。
End of this Chapter
扫一扫
449
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
