卡尔曼滤波器介绍.pdf

《卡尔曼滤波器介绍.pdf》由会员分享，可在线阅读，更多相关《卡尔曼滤波器介绍.pdf（12页珍藏版）》请在三一文库上搜索。

1、卡尔曼滤波器介绍 Greg Welch1and Gary Bishop2 TR 95-041 Department of Computer Science University of North Carolina at Chapel Hill3 Chapel Hill, NC 27599-3175 翻译：姚旭晨 更新日期: 2006年7月24日，星期一 中文版更新日期：2007年1月8日，星期一 摘要 1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波 问题的论文。从那以后，得益于数字计算技术的进步，卡尔曼滤波器 已成为推广研究和应用的主题，尤其是在自主或协助导航领域。 卡尔曼滤波

2、器由一系列递归数学公式描述。它们提供了一种高效可 计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波 器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能 估计将来的状态，即使并不知道模型的确切性质。 这篇文章介绍了离散卡尔曼理论和实用方法，包括卡尔曼滤波器及 其衍生：扩展卡尔曼滤波器的描述和讨论，并给出了一个相对简单的 带图实例。 1welchcs.unc.edu, http:/www.cs.unc.edu/welch 2gbcs.unc.edu, http:/www.cs.unc.edu/gb 3北卡罗来纳大学教堂山分校，译者注。 1 Welch & Bishop,卡尔曼

3、滤波器介绍2 1离散卡尔曼滤波器 1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问 题的论文 Kalman60 。从那以后，得益于数字计算技术的进步，卡尔曼 滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。 Maybeck79 的第一章给出了一个非常“友好”的介绍，更全面的讨论可以 参考 Sorenson70 ，后者还包含了一些非常有趣的历史故事。更广泛的参 考包括 Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93 。 被估计的过程信号 卡尔曼滤波器用于估计离散时间过程的状态变量 x n。这个离散时 间

4、过程由以下离散随机差分方程描述： xk= Axk1+ Buk1+ wk1,(1.1) 定义观测变量 z m，得到量测方程： zk= Hxk+ vk.(1.2) 随机信号 wk和 vk分别表示过程激励噪声1和观测噪声。假设它们为相 互独立，正态分布的白色噪声： p(w) N(0,Q),(1.3) p(v) N(0,R).(1.4) 实际系统中，过程激励噪声协方差矩阵Q 和观测噪声协方差矩阵R 可 能会随每次迭代计算而变化。但在这儿我们假设它们是常数。 当控制函数uk1或过程激励噪声 wk1为零时，差分方程1.1中的 n n 阶增益矩阵 A 将上一时刻 k 1 的状态线性映射到当前时刻 k 的状态

5、。实际 中 A 可能随时间变化，但在这儿假设为常数。n l 阶矩阵 B 代表可选的控 制输入 u l的增益。量测方程1.2中的 m n 阶矩阵 H 表示状态变量 xk 对测量变量 zk的增益。实际中 H 可能随时间变化，但在这儿假设为常数。 滤波器的计算原型 定义 x k n（ 代表先验，代表估计）为在已知第 k 步以前状态情 况下第 k 步的先验状态估计。定义 xk n为已知测量变量 zk时第 k 步的 后验状态估计。由此定义先验估计误差和后验估计误差： e k xk x k, ek xk xk 1原文为 process noise，本该翻译作过程噪声，由时间序列信号模型的观点，平稳随机序

6、列可以看成是由典型噪声源激励线性系统产生，故译作过程激励噪声。 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍3 先验估计误差的协方差为： P k = Ee ke k T ,(1.5) 后验估计误差的协方差为： Pk= EekekT,(1.6) 式1.7构造了卡尔曼滤波器的表达式：先验估计 x k 和加权的测量变量 zk及其预测 H x k 之差的线性组合构成了后验状态估计 xk。式1.7的理论解 释请参看“滤波器的概率原型”一节。 xk= x k + K(zk H x k) (1.7) 式1.7中测量变量及其

7、预测之差 (zk H x k) 被称为测量过程的革新或残 余。残余反映了预测值和实际值之间的不一致程度。残余为零表明二者完 全吻合。 式1.7中 n m 阶矩阵 K 叫做残余的增益或混合因数，作用是使1.6式 中的后验估计误差协方差最小。可以通过以下步骤计算 K ：首先将1.7式 代入 ek的定义式，再将 ek代入1.6式中，求得期望后，将1.6式中的 Pk对 K 求导。并使一阶导数为零从而解得 K 值。详细推导清参照 Maybeck79, Brown92, Jacobs93 。 K 的一种表示形式为： Kk= P k HT(HP k HT+ R)1 = P k HT HP k HT+ R .

8、(1.8) 由1.8式可知，观测噪声协方差 R 越小，残余的增益越大 K 越大。特别 地， R 趋向于零时，有： lim Rk0 Kk= H1. 另一方面，先验估计误差协方差 P k 越小，残余的增益 K 越小。特别 地， P k 趋向于零时，有： lim P k 0 Kk= 0. 增益 K 的另一种解释是随着测量噪声协方差 R 趋于零，测量变量 zk 的权重越来越大，而 zk的预测 H x k 的权重越来越小。另一方面，随着先 验估计误差协方差 P k 趋于零，测量变量 zk的权重越来越小，而 zk的预测 H x k 的权重越来越大。 UNC-Chapel Hill, TR 95-041,

9、July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍4 滤波器的概率原型解释 1.7式的解释来源于贝叶斯规则： xk的更新取决于在已知先前的测量变 量 zk的情况下 xk的先验估计 x k 的概率分布。卡尔曼滤波器表达式中包含 了状态分布的前二阶矩。 Exk = xk E(xk xk)(xk xk)T = Pk. 后验状态估计1.7式反应了状态分布的均值（一阶矩）如果条件 式1.3和1.4成立，均值的估计便是正态分布的。后验估计误差协方差1.6式反 映了状态分布的方差（二阶非中心矩）。在已知 zk的情况下， xk的分布可 写为： p(xk|zk) N(Exk,E(xk x

10、k)(xk xk)T) = N( xk,Pk). 有关卡尔曼滤波器的概率原型的更多讨论，请参考Maybeck79, Brown92, Jacobs93。 离散卡尔曼滤波器算法 我们先给出卡尔曼滤波器的总体性概述，然后讨论方程式的具体细节 及其作用。 卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一 时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼 滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负 责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间 状态构造先验估计。测量更新方程负责反馈也就是说，它将先验估计 和新的测量变量结合以构造改进

11、的后验估计。 时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最 后的估计算法成为一种具有数值解的预估校正算法，如图1-1所示。 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍5 图 1-1: 离散卡尔曼滤波器循环更新图。时间更新方程将当前状态变量作为 先验估计及时地向前投射到测量更新方程，测量更新方程校正先验估计以 获得状态的后验估计。 表1-1和表1-2分别给出了时间更新方程和测量更新方程的具体形式。 表 1-1: 离散卡尔曼滤波器时间更新方程 x k = A xk1+ Buk1(1.9) P k

12、 = APk1AT+ Q(1.10) 请再次注意表1-1中的时间更新方程怎样将状态估计 x k 和协方差估计 P k 从 k 1 时刻向前推算到 k 时刻。 A 和 B 来自式1.1， Q 来自式1.3，滤 波器的初始条件在早先的引用中讨论过。 表 1-2: 离散卡尔曼滤波器状态更新方程 Kk= P k HT(HP k HT+ R)1(1.11) xk= x k + Kk(zk H x k) (1.12) Pk= (I KkH)P k (1.13) 测量更新方程首先做的是计算卡尔曼增益 Kk。注意1.11式和1.8式是相 同的。其次便测量输出以获得 zk，然后按1.12式（与1.7式相同）产生

13、状态 的后验估计。最后按1.13式估计状态的后验协方差。 计算完时间更新方程和测量更新方程，整个过程再次重复。上一次计 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍6 算得到的后验估计被作为下一次计算的先验估计2。这种递归推算是卡尔曼 滤波器最吸引人的特性之一它比其它滤波器更容易实现：例如维纳滤 波器 Brown92 ，每次估计必须直接计算全部数据，而卡尔曼滤波器每次只 根据以前的测量变量递归计算当前的状态估计。图1-2将表1-1和表1-2结合 显示了滤波器的整个操作流程。 图 1-2: 卡尔曼滤波器工作原理

14、图，由图1-1和表1-1及表1-2结合得到。 滤波器系数及调整 滤波器实际实现时，测量噪声协方差 R 一般可以观测得到，是滤波器 的已知条件。观测测量噪声协方差 R 一般是可实现的（可能的），毕竟我 们要观测整个系统过程。因此通常我们离线获取一些系统观测值以计算测 量噪声协方差。 通常更难确定过程激励噪声协方差的 Q 值，因为我们无法直接观测到 过程信号 xk。有时可以通过 Q 的选择给过程信号“注入”足够的不确定性 来建立一个简单的（差的）过程模型而产生可以接受的结果。当然在这种 情况下人们希望信号观测值是可信的。 在这两种情况下，不管我们是否有一个合理的标准来选择系数，我们 通常（统计学上

15、的）都可以通过调整滤波器系数来获得更好的性能。调整 2即将1.12和1.13式的结果代入1.9和1.10式，译者注。 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍7 通常离线进行，并经常与另一个（确定无误的）在线滤波器对比，这个过 程称为系统识别。 在讨论的结尾，我们指出在 Q 和 R 都是常数的条件下，过程估计误差 协方差 R 和卡尔曼增益 Kk都会快速收敛并保持为常量（参照图1-2中的更 新方程）。若实际情况也如此，那么滤波器系数便可以通过预先离线运行 滤波器计算，或者，比如说，用 Grewal93 中的

16、方法计算 Pk的稳定值。 实际中，观测误差 R 尤其不易保持不变。例如，用我们的光电跟踪仪 观察挂在房间顶棚面板上的信号灯时，较近的信号灯会比较远的信号灯具 有较小的观测噪声。不仅是观测噪声会变化，有时过程激励噪声协方差 Q 也会随着滤波器运行而动态变化这样 Q 变成了 Qk来适应不同的 动态状态。例如，在跟踪三维虚拟环境中用户头部位置时，如果用户头部 缓慢移动，我们会减小 Qk的幅度，如果移动开始快速变化，则增加幅度。 在这些情况下， Qk的幅度要根据用户的移动方向和模型的不确定性来选 择。 2扩展卡尔曼滤波器 被估计的过程信号 如第一节所述，卡尔曼滤波器估计一个用线性随机差分方程描述的离

17、散时间过程的状态变量 x n。但如果被估计的过程和（或）观测变量与 过程的关系是非线性的，那应怎么办？一些最著名和有趣的卡尔曼滤波应 用就是处理这些情况的。将期望和方差线性化的卡尔曼滤波器称作扩展卡 尔曼滤波器（Extended Kalman Filter），简称EKF。 同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方 程的偏导来线性化并计算当前估计。我们将第一节中的公式换一种方式表 示。假设过程仍具有状态向量 x n，但其状态方程已变为非线性随机差 分方程的形式。 xk= f(xk1,uk1,wk1),(2.1) 观测变量 z m为： zk= h(xk,vk),(2.2) 随机

18、变量 wk和 vk仍代表过程激励噪声和观测噪声。差分方程式2.1中 的非线性函数 f 将上一时刻 k 1 的状态映射到当前时刻 k 的状态。量测方 程2.2中的驱动函数 uk和零均值过程噪声 wk是它的参数。非线性函数 h 反 映了状态变量 xk和观测变量 zk的关系。 实际中我们显然不知道每一时刻噪声 wk和 vk各自的值。但是，我们 可以将它们假设为零，从而估计状态向量和观测向量为： xk= f( xk1,uk1,0)(2.3) 和 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍8 zk= h( xk,0)

19、,(2.4) 其中， xk是过程相对前一时刻 k 的后验估计。 有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随 机变量的分布（或连续随机变量的密度）在经过非线性系统转化后不再是 正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝 叶斯决策的特殊状态估计器。Julier96中描述了一项有趣的研究，Julier et al. 设计了扩展卡尔曼滤波器的一种变体，使得通过非线性转换后的随机变 量仍具有正态分布特性。 滤波器的计算原型 为了估计一个具有非线性差分和量测关系的过程，我们先给出式2.3和 式2.4的一个新的线性化表示： xk xk+ A(xk1 xk1) + Ww

20、k1,(2.5) zk zk+ H(xk xk)V vk.(2.6) 其中， xk和 zk是状态向量和观测向量的真值， xk和 zk来自2.3式和2.4式，是状态向量和观测向量的观测值， xk是 k 时刻状态向量的后验估计， 类似于1.3式和1.4式，随机变量 wk和 vk表示过程激励噪声和观测噪 声。 A 是 f 对 x 的偏导的雅可比矩阵： Ai,j= fi xj ( xk1,uk1,0), W 是 f 对 w 的偏导的雅可比矩阵： Wi,j= fi wj ( xk1,uk1,0), H 是 h 对 x 的偏导的雅可比矩阵： Hi,j= hi xj ( xk,0), UNC-Chapel

21、Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍9 V 是 h 对 v 的偏导的雅可比矩阵： Vi,j= hi vj ( xk,0). 简单起见，我们并没有在 A,W,H,V 中加入下标 k ，但它们实际上是 随时间变化的。 现在我们定义一个新的预测误差的表达式： exk xk xk,(2.7) 和观测变量的残余， ezk zk zk,(2.8) 请记住我们在实际中无法获得2.7式中的 xk,它是状态向量的真值，也 就是我们要估计的对象。同样，我们也无法获得2.8式中的 zk，它是我们用 来估计 xk的观测向量的真值。由2.7式和2.

22、8式我们可以写出误差过程的表 达式： exk A(xk1 xk1) + k,(2.9) ezk H exk+ k,(2.10) k和 k代表具有零均值和协方差矩阵 WQWT和 V RV T 的独立随机 变量， Q 和 R 分别来自1.3式和1.4式。 注意2.9式和2.10式是线性的，它们很像离散卡尔曼滤波器中的状态差 分方程1.1式和量测方程1.2式。这提示我们用2.8式中的观测残余真值 ezk和 第二个（假设的）卡尔曼滤波器去估计2.9式中的预测误差 exk。估计的结 果记为 ek，结合2.7式可以获得初始非线性过程的后验状态估计： xk= xk+ ek.(2.11) 式2.9和式2.10

23、中的随机变量具有如下概率分布（参看前面的脚注）： p( exk) N(0,E exk eT xk) p(k) N(0,WQkWT) p(k) N(0,V RkV T) 令 ek的估计值为零，由以上近似，可以写出估计 ek的卡尔曼滤波器表 达式： ek= Kk ezk.(2.12) 将2.8式和2.12式代回2.11式，我们看到实际上并不需要第二个（假设 的）卡尔曼滤波器： UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍10 xk= xk+ Kk ezk = xk+ Kk(zk zk)(2.13) 式2.13现在

24、可以用作扩展卡尔曼滤波器的观测变量的更新。其中 xk和 zk来自2.3式和2.4式，将1.11式中的观测误差协方差进行适当的替换可以得 到卡尔曼增益 Kk。 表2-1和表2-2给出了扩展卡尔曼滤波器的全部表达式。注意我们用 x k 替换了 xk来表达先验概率的意思，并且雅可比矩阵 A,W,H,V 也被加上了 下标，显式地表明了它们在不同的时刻具有变化的值，每次需要被重复计 算。 表 2-1: 扩展卡尔曼滤波器时间更新方程 x k = f( xk1,uk1,0)(2.14) P k = AkPk1AT k + WkQk1WT k (2.15) 就像基本的离散卡尔曼滤波器，表2-1中的时间更新方程

25、将状态和协方 差估计从 k 1 时刻向前推算到 k 时刻。2.14式中的 f 来自式2.3， Ak和 Wk是 k 时刻的过程雅可比矩阵， Qk是式1.3中 k 时刻的过程激励噪声协方 差矩阵。 表 2-2: 扩展卡尔曼滤波器状态更新方程 Kk= P k HT k(HkP k HT k + VkRkV T k )1(2.16) xk= x k + Kk(zk h( x k,0) (2.17) Pk= (I KkHk)P k (2.18) 就像基本的离散卡尔曼滤波器，表2-2中的测量更新方程利用观测值变 量 zk的值校正状态估计和协方差估计。2.17式中的 h 来自式2.4， Hk和 V 是 k

26、时刻的测量雅可比矩阵， Rk是式1.4中 k 时刻的观测噪声协方差矩阵 （注意下标 k 表示 Rk随时间变化）。 扩展卡尔曼滤波器的基本运行流程与图1-1中的线性离散卡尔曼滤波器 相同。图2-1将图1-1与表2-1和表2-2中的表达式结合。 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍11 图 2-1:扩展卡尔曼滤波器工作原理图，由图1-1和表2-1及表2-2结合得到。 扩展卡尔曼滤波器的一个重要特性是卡尔曼增益 Kk的表达式中的雅可 比矩阵 Hk能够正确地传递或“加权”观测信息中的有用部分。例如，如果 通过

27、 h 观测变量 zk和状态变量没有一一对应的关系，雅可比矩阵 Hk便通 过改变卡尔曼增益从而使得残余 zk h( x k,0) 中真正作用于状态变量的部 分被加权。当然，如果整个观测中观测变量 zk和状态变量通过 h 都没有一 个一一对应的关系，那么滤波器很快就会发散。这种情况下过程是不可观 测的。 3卡尔曼滤波器实践：估计随机常数 前两节我们讨论了离散卡尔曼滤波器和扩展卡尔曼滤波器的基本形 式。在这儿，我们给出一个简单的例子以帮助读者更好地理解卡尔曼 滤波器的实现和性能。 Andrew Straw在 http:/www.scipy.org/Cookbook/ KalmanFiltering

28、上给出了使用Python/SciPy的具体实现方法。 过程模型 在这个简单的例子里我们估计一个常数随机变量，比如电压。假设 我们可以测量这个常数的幅值，但观测幅值中掺入了幅值均方根（Root- UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop,卡尔曼滤波器介绍12 Mean-Square，RMS）为 0.1 伏的白噪声（比如在模数转换器不是很准确的 情况下）。下面的线性差分方程描述了整个过程： xk= Axk1+ Buk1+ wk = xk1+ wk, 观测变量 z 1为： zk= Hxk+ vk = xk+ vk. 过程的状态不

29、随时间变化，所以 A = 1；没有控制输入，所以 u = 0； 包含噪声的观测值是状态变量的直接体现，所以 H = 1 。（注意有些地方 我们忽略了下标k ，因为对应的系数在这个例子中为常数。） 滤波器方程和参数 时间更新方程为： x k = xk1, P k = Pk1+ Q. 测量更新方程为： Kk= P k (P k + R)1 = P k P k + R ,(3.1) xk= x k + Kk(zk x k), Pk= (I Kk)P k . 假设过程激励噪声方差 Q 非常小， Q = 105（也可以令 Q = 0 ，但 是一个小的非零常数可以方便地调整滤波器参数，下面将会证明）。再假 设由经验我们知道随机常数的真值具有标准正态分布，因此我们令滤波器 的初始条件为零，即 xk1= 0 。 类似地，我们要选择Pk1的初值P0。如果确定初始状态估计 x0= 0， 可以令 P0= 0 。但因为初始状态估计 x0并不确定，令 P0= 0 可能会使滤 波器一直产生 xk= 0 的结果。就像实验验证的那样， P0的选择并不关键， 几乎任何 P06= 0 都会使滤波器最终收敛。在这里我们令 P0= 1 。 UNC-Chapel Hill, TR 95-041, July 24, 2006