卡尔曼滤波器简介 (阎泓著)
卡尔曼滤波器简介
阎泓 博士1 著
2008 年美国工程学院把 Charles Stark Draper 奖授予 Rudolf Emil Kalman 博士,以表彰他在
最优数字化技术方面所作的,被人们称之为卡尔曼滤波器(Kalman Filter)的杰出贡献2。
卡尔曼博士 1930 年出生于匈牙利首都布达佩斯特,1954 年于麻省理工学院获得电机工程
学硕士,1957 年获得哥伦比亚大学博士。1960 年卡尔曼博士发表了他的著名论文
[Kalman60],论述了被后人称为卡尔曼滤波器的算法。
下面的照片为卡尔曼博士在瑞士联邦技术学院(Swiss Federal Institute of Technology)自
己的办公室内。
卡尔曼滤波器可以从数据序列中排除干扰噪声。使用卡尔曼滤波器可以从不完整的信息给
出一个不断变化中的复杂系统的最优估计。从阿波罗登月,到最近风靡全球的 Wii 游戏机
的遥控定向系统,可以毫不夸张地讲,任何移动的系统都要用到卡尔曼滤波器。
卡尔曼滤波器已经被应用到几乎所有的工程系统中,包括雷达的目标追踪,全球定位系
统,水利模型,大气观测,测量经济学中的时间序列,以及药剂受控施放等方面。
本文作者对卡尔曼滤波器感兴趣是因为它在成对交易、统计套利模型中的广泛应用。
本文对离散时间系统的卡尔曼滤波作简单介绍。
1阎泓:理论物理学博士,美籍,曾在多家华尔街金融公司任对冲基金经理、资深量化分析师、算法交易系
统架构师、高频交易系统设计师等职。本文作者联络方式:wallst82@yeah.net。作者保留全部版权。All
rights reserved by the author.
2 请见 http://www.nae.edu/nae/awardscom.nsf/weblinks/JMAN-7A4HZV?OpenDocument
第 1 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
第一章、 标量线性系统......................................................................................................... 6
1.1 卡尔曼问题..................................................................................................................... 6
1.1.1 信号流程图............................................................................................................. 6
1.1.2 加入白噪声............................................................................................................. 7
1.1.3 加入可测量............................................................................................................. 8
1.1.4 卡尔曼问题陈述..................................................................................................... 8
1.2 标量卡尔曼滤波器......................................................................................................... 8
1.2.1 最优的 k 值 ........................................................................................................... 10
1.2.2 计算先验均方差................................................................................................... 10
1.2.3 计算后验均方差................................................................................................... 11
1.2.4 计算 k 值 ............................................................................................................... 12
1.2.5 最优 k 值时的后验均方差 ................................................................................... 12
1.3 算法............................................................................................................................... 13
第一步、时间更新............................................................................................................... 13
第二步、测量更新............................................................................................................... 13
算法的神奇之处................................................................................................................... 14
1.4 特殊情况....................................................................................................................... 14
第一种情况、先验误差极小............................................................................................... 14
第二种情况、先验误差极大............................................................................................... 14
第三种情况、测量噪声极大............................................................................................... 15
1.5 标量 KF 的 MatLab 实现 ............................................................................................. 15
1.5.1 程序的核心........................................................................................................... 15
1.5.2 模拟噪声............................................................................................................... 16
1.5.3 初始化................................................................................................................... 16
1.6 MatLab 程序试运行 ..................................................................................................... 17
第二章、 矢量线性系统....................................................................................................... 22
2.1 所观察的系统............................................................................................................... 22
2.1.1 高维差分方程....................................................................................................... 22
2.1.2 流程图................................................................................................................... 22
2.1.3 噪声....................................................................................................................... 23
2.2 矢量卡尔曼滤波器....................................................................................................... 23
2.2.1 先验估计............................................................................................................... 24
2.2.2 后验估计............................................................................................................... 24
2.2.3 计算先验均方差................................................................................................... 25
2.2.4 计算后验均方差................................................................................................... 26
2.2.5 计算 k 值 ............................................................................................................... 27
k 值最优时的后验均方差 ................................................................................... 28
2.2.6
2.2.7 一个常见的错误................................................................................................... 29
2.3 算法............................................................................................................................... 29
第 2 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
3.3
第一步、时间更新............................................................................................................... 29
第二步、测量更新............................................................................................................... 29
2.4 特殊情况....................................................................................................................... 30
第一种情况、先验误差极小............................................................................................... 30
第二种情况、先验误差极大............................................................................................... 30
第三种情况、测量噪声极大............................................................................................... 31
第三章、 标量 EKF.............................................................................................................. 32
3.1 非线性状态模型........................................................................................................... 32
3.2 模型线性化................................................................................................................... 33
3.2.1 过程噪声项的线性化........................................................................................... 33
3.2.2 测量噪声项的线性化........................................................................................... 34
3.2.3 过程和测量噪声项同时线性化........................................................................... 35
3.2.4 过程的线性化....................................................................................................... 35
3.2.5 测量的线性化....................................................................................................... 36
EKF 滤波器 .................................................................................................................. 37
3.3.1 应用卡尔曼滤波器............................................................................................... 37
3.3.2 计算先验均方差................................................................................................... 37
3.3.3 计算后验均方差................................................................................................... 37
3.3.4 计算 k 值 ............................................................................................................... 38
k 值为最优时的后验均方差 ............................................................................... 38
3.3.5
3.4 算法............................................................................................................................... 39
第一步、时间更新............................................................................................................... 39
第二步、测量更新............................................................................................................... 39
EKF 的缺陷 .................................................................................................................. 40
第四章、 矢量 EKF.............................................................................................................. 41
4.1 非线性矢量状态模型................................................................................................... 41
4.2 矢量模型线性化........................................................................................................... 42
4.2.1 矢量泛函的泰勒展开。....................................................................................... 42
4.2.2 过程噪声项的线性化........................................................................................... 42
4.2.3 测量噪声项的线性化........................................................................................... 43
4.2.4 过程和测量噪声项同时线性化........................................................................... 44
4.2.5 过程的线性化....................................................................................................... 44
4.2.6 测量的线性化....................................................................................................... 45
4.3 矢量 EKF 滤波器 ......................................................................................................... 46
4.3.1 应用矢量卡尔曼滤波器....................................................................................... 46
4.3.2 计算先验均方差................................................................................................... 46
4.3.3 计算后验均方差................................................................................................... 47
4.3.4 计算 k 值 ............................................................................................................... 47
k 值为最优时的后验均方差 ............................................................................... 48
4.3.5
4.5 算法............................................................................................................................... 48
第一步、时间更新............................................................................................................... 49
3.5
第 3 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
第二步、测量更新............................................................................................................... 49
4.4 特殊情况....................................................................................................................... 49
第一种情况、先验误差极小............................................................................................... 50
第二种情况、先验误差极大............................................................................................... 50
第三种情况、测量噪声极大............................................................................................... 50
第五章、 标量无迹变换 UT ................................................................................................ 52
5.1 无迹变换的任务........................................................................................................... 52
5.2 真值............................................................................................................................... 52
5.3 无迹测试点................................................................................................................... 54
5.3.1 标量的无迹测试点............................................................................................... 54
5.3.2 无迹权重系数....................................................................................................... 55
5.3.3 统计性质公式....................................................................................................... 55
5.4 测试点的无迹变换....................................................................................................... 56
5.4.1 从测试点得到后验期待值................................................................................... 56
5.4.2 从测试点得到后验方差....................................................................................... 57
5.5 讨论............................................................................................................................... 58
第六章、 矢量无迹变换 UT ................................................................................................ 59
6.1 矢量微分回顾............................................................................................................... 59
6.1.1 计算真值会用到的恒等式................................................................................... 59
6.2 矢量无迹变换的任务................................................................................................... 60
6.3 真值............................................................................................................................... 61
6.3 无迹测试点................................................................................................................... 62
6.3.1 矢量的无迹测试点............................................................................................... 63
6.3.2 无迹权重系数....................................................................................................... 64
6.3.3
UT 变换下的对称性 ............................................................................................ 64
6.4 测试点的无迹变换....................................................................................................... 65
6.4.1 几个恒等式........................................................................................................... 65
6.4.2 从测试点得到后验期待值................................................................................... 66
6.4.2 从测试点得到后验协方差................................................................................... 67
6.5 讨论..................................................................................................................................... 68
第七章、无迹滤波器 UKF.......................................................................................................... 69
7.1 高维非线性问题........................................................................................................... 69
7.1.1 标量特例............................................................................................................... 70
7.2 无迹滤波器................................................................................................................... 70
7.2.1 无迹测试点........................................................................................................... 71
7.2.2 无迹权重系数....................................................................................................... 72
7.2.3 先验估计............................................................................................................... 72
7.2.4 应用卡尔曼滤波器............................................................................................... 73
7.2.5 计算后验均方差................................................................................................... 73
7.2.6 计算 k 值 ............................................................................................................... 74
7.3 算法............................................................................................................................... 75
第 4 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
第零步、初始化................................................................................................................... 75
第一步、时间更新............................................................................................................... 75
第二步、测量更新............................................................................................................... 76
第 5 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
第一章、 标量线性系统
实际工作中的线性系统很少有标量的,但是标量的卡尔曼滤波器的理论推导比较直观、易
于理解,因此作为学习的切入点比较合适。
首先必须清楚地陈述卡尔曼滤波器要解决的问题。
1.1 卡尔曼问题
在离散时间中,一个标量线性系统的状态演化常常可以表述为下面的随机差分方程式:
(1)
x
t
ax
1
t
bu
t
1
其中t 为时间。 tx 是一个标量随机变量,代表t 时刻系统的内禀状态。 a 和b 为常标量。
1tu 为 1t 时刻的输入,也是一个标量。
1.1.1 信号流程图
上面的(1)式也可以用下面的信号流程图表示。
直线表示信号的传送,箭头代表传送的方向。流程图中的图标有三种,第一种方框图标代
表时间延迟,见下图
第二种方框图标代表乘法(增益),见下图
第 6 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
第三种圆形图标代表加法(混合),见下图
这些图标可以按照有意义的方式组合起来,描述一个差分方程。
必须指出,这些图标并不局限于标量情形,而且适用于矢量情形,譬如 tx 为一个矢量,而
a 和b 可以为矩阵。
1.1.2 加入白噪声
假设在这个线性过程中有一个噪声项 tw
x
t
ax
t
1
(2)
bu
t
1
w
t
1
则此方程式可以用下面的信号流程图表示:
假定这个噪声 tw 是一个高斯白噪声,它满足3
0
QN
,0
Q
ww
t
s
w
t
~
,
0
(
s
t
)
(3)
3 在本文采用物理学中常用的记号,
x
xE
表示 x 的期待值。
第 7 页(共 77 页)
卡尔曼滤波器简介 (阎泓著)
此外假定 tw 与 su 没有关联,也即
uw t
s
0
(
ts
),
(4)
1.1.3 加入可测量
假设系统的状态量 tx 是不可以直接测量的。可以测量的是另外一个量 tz ,称为可测量。可
测量 tz 依赖于系统的状态量 tx 和一个激励倍数 h ,见下式。
z
t
hx
t
v
t
(5)
在实际工作中 h 可能会随着时间而变化,但在这里假定为常数,为常标量。
此时流程图如下。
测量过程本身带有一个噪声 tv ,影响了测量的准确度。同样我们假定 tv 是一个白噪声
vt
~
RN
,0
R
0
vv t
s
0
(
s
t
)
此外假定 tv 与 tw 和 tu 都没有关联,也即
ts
),
(
0
wv
s
t
uv
s
t
(6)
(7)
1.1.4 卡尔曼问题陈述
现在要考虑的是如何从可观测量 tz 的观测数据中得出 tx 的最优估计值,把噪声 tw 和 tv 尽
最大可能过滤出去,把它们的影响减到最小。
这就是卡尔曼滤波器要解决的问题。
1.2 标量卡尔曼滤波器
卡尔曼对这个问题的解答就是卡尔曼滤波器。下面的流程图可以分成上下两个部分;上半
部分就是问题本身,下半部分就是卡尔曼滤波器。
第 8 页(共 77 页)