卡尔曼滤波--递归算法
一、为什么会存在卡尔曼滤波
在生产生活中卡尔曼滤波器应用广泛，因为我们在描述系统时通常存在不确定性，不确定性主要存在三个方面：
1、不存在完美的数学模型（系统数学模型往往是理想模型，存在许多假设，与实际模型存在差距）；
２、系统的扰动不可控，也很难建模（自然界会对系统进行干扰且难以消除，如噪音干扰）；
３、测量传感器存在误差（测量过程中由于测量仪器本身会存在误差，或人为导致的误差） 。
所以无论如何系统总是会存在误差的，无论是计算上，还是传感器的检测上，只能认为当前状态是当前真实状态的一个最优估计。为了更加接近真实值找出这个最优估计，卡尔曼滤波应用而生。
那麽卡尔曼滤波就是在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态或真实信号。
二、卡尔曼滤波是什么
Kalman Filtering是一种最优化递归数字处理算法（Optimal Recursive Data Processing Algorithm）。通过利用线性系统状态方程和系统输入输出观测数据，对系统状态进行最优估计的算法。
数据滤波是去除噪声还原真实数据的一种数据处理技术，所以即使测量的一系列数据中存在测量噪声Kalman Filtering仍然适用并且估计动态系统的状态。甚至可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
递归估计即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。
最优估计
当前状态观测值*权重+上一时刻估计值 *权重 =修正值 *权重=当前状态的估计值。
Kalman Filtering通过调整观测值和估计值的权重，使修正后的值更趋向于相信观测值还是相信估计值的一个过程。
三、公式推导----递归
卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。
1、举例说明
例： 测量一枚硬币的直径，假设第一次测量结果为 z 1 = 100.1 m m z_{1}=100.1 \mathrm{~mm} z1=100.1 mm, 第二次测量结果为 z 2 = 100.3 z_{2}=100.3 z2=100.3 m m \mathrm{mm} mm, 第三次测量结果为 z 3 = 99.5 m m ∘ z_{3}=99.5 \mathrm{~mm}_{\circ} z3=99.5 mm∘ 通常来说，我们会取 x ^ = 1 3 ( z 1 + z 2 + z 3 ) = 99.97 m m \hat{x}=\frac{1}{3}\left(z_{1}+z_{2}+z_{3}\right)=99.97 \mathrm{~mm} x^=31(z1+z2+z3)=99.97 mm 作为计算结果。
在上述示例中，我们把所有测量值取平均值作为估计真实数据，当测量次数为
k
k
k 时，可得：
x
^
k
=
1
k
(
z
1
+
z
2
+
z
3
+
⋯
+
z
k
)
=
1
k
(
z
1
+
z
2
+
z
3
+
⋯
+
z
k
−
1
)
+
1
k
z
k
=
1
k
k
−
1
k
−
1
(
z
1
+
z
2
+
z
3
+
⋯
+
z
k
−
1
)
+
1
k
z
k
(1)
\begin{aligned} \hat{x}_{k} &=\frac{1}{k}\left(z_{1}+z_{2}+z_{3}+\cdots+z_{k}\right) \\ &=\frac{1}{k}\left(z_{1}+z_{2}+z_{3}+\cdots+z_{k-1}\right)+\frac{1}{k} z_{k} \\ &=\frac{1}{k} \frac{k-1}{k-1}\left(z_{1}+z_{2}+z_{3}+\cdots+z_{k-1}\right)+\frac{1}{k} z_{k}\tag 1\\ \end{aligned}
x^k=k1(z1+z2+z3+⋯+zk)=k1(z1+z2+z3+⋯+zk−1)+k1zk=k1k−1k−1(z1+z2+z3+⋯+zk−1)+k1zk(1)
测量
k
−
1
k-1
k−1 次时的平均值：
x
^
k
−
1
=
1
k
−
1
(
z
1
+
z
2
+
z
3
+
⋯
+
z
k
−
1
)
(2)
\hat{x}_{k-1}=\frac{1}{k-1}\left(z_{1}+z_{2}+z_{3}+\cdots+z_{k-1}\right)\tag 2
x^k−1=k−11(z1+z2+z3+⋯+zk−1)(2)
将
(
2
)
(2)
(2) 式代入
(
1
)
(1)
(1) 式可得：
x
^
k
=
k
−
1
k
x
^
k
−
1
+
1
k
z
k
=
x
^
k
−
1
−
1
k
x
^
k
−
1
+
1
k
z
k
⇒
x
^
k
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
(3)
\begin{aligned} \hat{x}_{k} &=\frac{k-1}{k} \hat{x}_{k-1}+\frac{1}{k} z_{k} \\ &=\hat{x}_{k-1}-\frac{1}{k} \hat{x}_{k-1}+\frac{1}{k} z_{k} \\ \Rightarrow \hat{x}_{k} &=\hat{x}_{k-1}+\frac{1}{k}\left(z_{k}-\hat{x}_{k-1}\right)\tag 3 \end{aligned}
x^k⇒x^k=kk−1x^k−1+k1zk=x^k−1−k1x^k−1+k1zk=x^k−1+k1(zk−x^k−1)(3)
上式 :
| 符号 | 含义 | 备注 |
|---|---|---|
| x ^ k \hat{x}_{k} x^k | k k k 次测量的平均值 | 当前的估计值 |
| x ^ k − 1 \hat{x}_{k-1} x^k−1 | k − 1 k-1 k−1 次测量的平均值 | 上一次的估计值 |
| k k k | 测量次数 | – |
| z k z_{k} zk | k k k次测量值 | 当前的测量值 |
当
k
→
∞
k \rightarrow \infty
k→∞ 时,
1
k
→
0
\frac{1}{k} \rightarrow 0
k1→0, 此时
x
^
k
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
=
x
^
k
−
1
+
0
⇒
x
^
k
−
1
→
x
^
k
(4)
\begin{gathered} \hat{x}_{k}=\hat{x}_{k-1}+\frac{1}{k}\left(z_{k}-\hat{x}_{k-1}\right)=\hat{x}_{k-1}+0 \\ \Rightarrow \hat{x}_{k-1} \rightarrow \hat{x}_{k}\tag 4 \end{gathered}
x^k=x^k−1+k1(zk−x^k−1)=x^k−1+0⇒x^k−1→x^k(4)
由上述推导可知，随着
k
k
k 增加，测量结果不再重要 ；而当
k
k
k 越小,
1
k
\frac{1}{k}
k1 越大，测量值
z
k
z_{k}
zk 作用较大。
令
K
k
=
1
k
K_{k}=\frac{1}{k}
Kk=k1 代入式
(
3
)
(3)
(3) 可得
：
：
：
x
^
k
=
x
^
k
−
1
+
K
k
(
z
k
−
x
^
k
−
1
)
(5)
\hat{x}_{k}=\hat{x}_{k-1}+K_{k}\left(z_{k}-\hat{x}_{k-1}\right)\tag 5
x^k=x^k−1+Kk(zk−x^k−1)(5)
式中 :
K
k
K_{k}
Kk 一卡尔曼增益因数 (Kalman Gain )。
卡尔曼滤波 即 :
当前的估计值 = 上一次的估计值 + 系数 (当前测量值 - 上一次的估计值)
从上式可以明显看到卡尔曼滤波是一种递归算法,当前的估计值只与上一次的估计值有关,而上一次只与它的前一次有关。而且当前估计值更接近上一次的估计值还是当前测量值与卡尔曼增益因数—— K k K_{k} Kk 有关。
在这里先给出
K
k
K_{k}
Kk的计算方式其推导过程详见后续博客
K
k
K_{k}
Kk 一卡尔曼增益因数 (Kalman Gain )：
K
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
K_{k}=\frac{e_{E S T _{k-1}}}{e_{E S T_ {k-1}}+e_{M E A _{k}}}
Kk=eESTk−1+eMEAkeESTk−1
式中：
| 符号 | 含义 | 备注 |
|---|---|---|
| e E S T e_{E S T} eEST | 估计误差 | – |
| e M E A e_{M E A} eMEA | 测量误差 | – |
| k k k | 本次 | – |
则在 k k k 时刻时：
- e E S T k − 1 ≫ e M E A k e_{E S T k-1} \gg e_{M E A k} eESTk−1≫eMEAk 时 , K k → 1 , x ^ k = x ^ k − 1 + z k − x ^ k − 1 = z k , K_{k} \rightarrow 1, \hat{x}_{k}=\hat{x}_{k-1}+z_{k}-\hat{x}_{k-1}=z_{k} ,Kk→1,x^k=x^k−1+zk−x^k−1=zk
- e E S T k − 1 ≪ e M E A k e_{E S T k-1} \ll e_{M E A k} eESTk−1≪eMEAk 时, K k → 0 , x ^ k = x ^ k − 1 K_{k} \rightarrow 0, \hat{x}_{k}=\hat{x}_{k-1} Kk→0,x^k=x^k−1
所以当上一次的估计误差远远大于本次测量误差时，我们更加相信本次的测量结果；反之我们更加相信上一次估计结果。 K k K_{k} Kk的取值就决定了本次的估计值更趋向于误差小的值 ，这样就减小了误差使之更加接近真实值。
2、寻找最优化步骤
- 计算 Kalman Gain : K k = e E S T k − 1 e E S T k − 1 + e M E A k ; K_{k}=\frac{e_{E S T _{k-1}}}{e_{E S T_ {k-1}}+e_{M E A _{k}}}; Kk=eESTk−1+eMEAkeESTk−1;
- 计算 x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_{k}=\hat{x}_{k-1}+K_{k}\left(z_{k}-\hat{x}_{k-1}\right) x^k=x^k−1+Kk(zk−x^k−1);
- 更新 e E S T k = ( 1 − K k ) e E S T k − 1 e_{E S T _k}=\left(1-K_{k}\right) e_{E S T _{k-1}} eESTk=(1−Kk)eESTk−1(仔细推导过程见后续博客)
这就体现了卡尔曼滤波器包括两个主要过程：预估与校正。
举例说明
测量一枚硬币的直径
3、拓展知识
噪音干扰
干扰噪音分类
1. 白噪声
所谓的高斯白噪声是指
1、信号的幅度分布服从高斯分布；
2、它的功率谱密度又是均匀分布的（是一个常数）。
系统表示过程中所用到的数据通常都是含有噪声的，从工程实际出发，这种噪声往往可以视为具有有理谱密度的平稳随机过程。白噪声是一种最简单的随机过程，是由一系列不相关的随机变量组成的理想化随机过程。其自相关函数为狄拉克
δ
\delta
δ函数。
白噪声是一种功率谱密度为常数的随机信号或随机过程。换而言之，此信号在各个频段上的功率是一样的，由于白光是各种频率的单色光混合而成，因此次信号的这种具有平坦功率谱的性质被称作是“白色的”，次信号也因此被称作白噪声。相对的，其他不具备这一性质的噪声信号被称为有色噪声（功率谱密度随频率变化）。
2. 有色噪声
理想的白噪声只是一种理论上的抽样，在物理上是很难实现的，现实中并不存在这样的噪声。因而，工程实际中测量的数据所包含的噪声往往是有色噪声。所谓的有色噪声（或相关噪声）是指序列中每一时刻都是相关的。有色噪声可以看成是由白噪声序列驱动的线性环节的输出。
3. 两者的区别
（1）由定义可以看出，白噪声在不同时刻是不相关的，自相关函数为脉冲函数：有色噪声则是相关的。
（2）实际测试中可以通过测试功率谱来区分，白噪声的功率谱在各频率的值都比较平均，有色噪声则会表现出较明显的峰值。
参考文献及致谢
本博客是对B站UP主DR_CAN 所讲的卡尔曼滤波视频的整理笔记，基本算是课堂笔记，在此十分感谢DR_CAN 博士他是控制专业学生的福音，本人能力有限若对本博客有不解的地方推荐直接观看DR_CAN 博士的视频通俗易懂，yyds。
【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)
3001
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
