卡尔曼滤波器介绍.docx

卡尔曼滤波器介绍摘要在1960年，卡尔曼出版了他最著名的论文，描述了一个对离散数据线性滤 波问题的递归解决方法。从那以后，由于数字计算的进步，卡尔曼滤波器已经成 为广泛研究和应用的主题，特别在自动化或协助导航领域。卡尔曼滤波器是一系列方程式，提供了有效的计算（递归）方法去估计过程 的状态，是一种以平方误差的均值达到最小的方式。滤波器在很多方面都很强大: 它支持过去，现在，甚至将来状态的估计，而且当系统的确切性质未知时也可以 做。这篇论文的目的是对离散卡尔曼滤波器提供一个实际介绍。这次介绍包括对 基本离散卡尔曼滤波器推导的描述和一些讨论，扩展卡尔曼滤波器的描述和一些 讨论和一个相对简单的（切实的）实际例子。1离散卡尔曼滤波器在1960年，卡尔曼出版了他最著名的论文，描述了一个对离散数据线性滤 波问题的递归解决方法Kalman60。从那以后，由于数字计算的进步，卡尔曼滤 波器已经成为广泛研究和应用的主题，特别在自动化或协助导航领域。第一章讲 述了对卡尔曼滤波器非常“友好的”介绍Maybeck79，而一个完整的介绍可以 在Sorenson70找到，也包含了一些有趣的历史叙事。更加广泛的参考包括 Gelb74； Grewal93； Maybeck79； Lewis86； Brown92； Jacobs93.被估计的过程卡尔曼滤波器卡用于估计离散时间控制过程的状态变量尤6诉n。这个离散时 间过程由以下离散随机差分方程描述：工=Ax + bu + w（ 1 . 1）测量值 Z 6沉m， Zk = Hxk + 七（1.2）随机变量门和匕分别表示过程和测量噪声。他们之间假设是独立的，正态分布的高斯白噪：p（ w） - N （0 Q ）（1.3）p（v）N（0，R）（1.4）在实际系统中，过程噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每 次迭代计算而变化。但在这儿我们假设它们是常数。当控制函数uki或过程噪声wki为零时，差分方程1.1中的nxn阶增益矩 阵A将过去k-1时刻状态和现在的k时刻状态联系起来。实际中A可能随时间变 化，但在这儿假设为常数。nxl阶矩阵B代表可选的控制输入u 6沁的增益。测量方程1.2中的mxn阶矩阵H表示状态变量七对测量变量七的增益。实际中H 可能随时间变化，但在这儿假设为常数。滤波器的计算原型我们定义对6诉n（-代表先验，人代表估计）为在已知第k步以前的状态情况 下，第k步的先验状态估计。定义X 6诉n为已知测量变量匕时第k步的后验状态估计。由此定义先验估计误差和后验估计误差：气三xk - x _A e = x - xk先验估计误差的协方差为：P- = Ee-e-T（1.5）kk k后验估计误差的协方差为：P = Ee e T（1.6）kk k式1.7构造了卡尔曼滤波器的表达式:先验估计x_和加权的测量变量七及其预测hx-之差的线性组合构成了后验状态估计x,。式1.7的理论解释请参看“滤KK(1.7)波器的概率原型”一节。A A-工 d h A_ xk - xk + K (zk - Hxk)式1.7中测量变量及其预测之差（匕-Hx ；）被称为测量过程的革新或残余。残 余反映了预测值和实际值之间的不一致程度。残余为零则表明二者完全吻合。式1.7中nxm阶矩阵K叫做残余的增益或混合因数，作用是使1.6式中的后验估计误差协方差最小。可以通过以下步骤计算K :首先将1.7式代入e的定义 k式，再将。弋入1.6式中，求得期望后，将1.6式中的Pk对K求导。并使一阶导数 为零从而解得K值。详细推导清参照Maybeck79, Brown92, Jacobs93。K的一 种表示形式为：（1.8）K = P-Ht (HP-Ht + R)-1 = P-P-Ht / (HP-Ht + R) k kkk kk由1.8式可知，观测噪声协方差火越小，残余的增益越大K越大。特别地，R 趋向于零时，有：lim K = H-1。R"。k另一方面，先验估计误差协方差P越小，残余的增益K越小。特别地，P趋向于零时，有：lim % = 0。Pk T°增益K的另一种解释是随着测量噪声协方差R趋于零，测量变量七的权重 越来越大，而七的预测Hx_的权重越来越小。另一方面，随着先验估计误差协 方差P-趋于零，测量变量z的权重越来越小，而z的预测HX-的权重越来越大。kkkk滤波器的概率原型解释1.7式的解释来源于贝叶斯规则：Xk的更新取决于在已知先前的测量变量zk的情况下Xk的先验估计X:的概率分布。卡尔曼滤波器表达式中包含了状态分布 的前二阶矩。E 乂火=X kE(x - Xk)(x -令 )t = P k k k kk后验状态估计1.7式反应了状态分布的均值(一阶矩)一一如果条件式1.3 和1.4成立，均值的估计便是正态分布的。后验估计误差协方差1.6式反映了状态分布的方差(二阶非中心矩)。在已知zk的情况下，Xk的分布可写为：讯X | z ) N(EX E(X -X )(X -X )t) = N(X P ) p(X z )(妇X ,(X XJ 八X XJ )T ) =(XJ ,)k kkk k k kk k有关卡尔曼滤波器的概率原型的更多讨论，请参考Maybeck79, Brown92, Jacobs93。离散卡尔曼滤波器算法我们先给出卡尔曼滤波器的总体性概述，然后讨论方程式的具体细节及其作 用。卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的 状态，然后以(含噪声的)测量变量的方式获得反馈。因此卡尔曼滤波器可分为 两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前 状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更 新方程负责反馈一一也就是说，它将先验估计和新的测量变量结合以构造改进的 后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估 计算法成为一种具有数值解的预估一校正算法，如图1-1所示。离散卡尔曼滤波器循环更新图。时间更新方程将当前状态变量作为先验估计 及时地向前投射到测量更新方程，测量更新方程校正先验估计以获得状态的后验 估计。如下分别给出了时间更新方程和测量更新方程的具体形式。x_ = Ax 一 + Bu(1.9)kk-1k-1P- = AP At + Q(1.10)请再次注意上式中的时间更新方程怎样将状态估计x k和协方差估计P-从 k-1时刻向前推算到k亥h A和B来自式1.1，Q来自式1.3，滤波器的初始条件 在早先的引用中讨论过。K = P- Ht (HP Ht + R)-1(1.11)x = x_ + K(z H X )(1.12)P = (I KH) P(1.13)测量更新方程首先做的是计算卡尔曼增益K。注意1.11式和1.8式是相同的。k其次，便测量输出以获得匕，然后按1.12式(与1.7式相同)产生状态的后验估计。 最后按1.13式估计状态的后验协方差。计算完时间更新方程和测量更新方程，整个过程再次重复。上一次计算得到 的后验估计被作为下一次计算的先验估计。这种递归推算是卡尔曼滤波器最吸引 人的特性之一一一它比其它滤波器更容易实现：例如维纳滤波器Brown92，每 次估计必须直接计算全部数据，而卡尔曼滤波器每次只根据以前的测量变量递归 计算当前的状态估计。图1-2将表1-1和表1-2结合显示了滤波器的整个操作流程。山一札表L及表1置结合得到。和P、T为初始估计智1-2：卡尔曼波波器I：作原理图,测量更新（校正）（1）计算卡尔曼增益 虹电皑呻'顷由观测变虽4更新估计X- = n+4（L.-Hx（3）更新误差协方差卒（1-5"时间更新（预测）|（ 1 ）向前推算状态变量 厂 AM-""（2向前推算误差协方差 p=APk_e+Q滤波器系数及调整滤波器实际实现时，测量噪声协方淑一般可以观测得到，是滤波器的已知 条件。观测测量噪声协方差夫一般是可实现的（可能的），毕竟我们要观测整 个系统过程。因此通常我们离线获取一些系统观测值以计算测量噪声协方差。通常更难确定过程激励噪声协方差的己值，因为我们无法直接观测到过程 信号七。有时可以通过己的选择给过程信号“注入”足够的不确定性来建立一 个简单的（差的）过程模型而产生可以接受的结果。当然在这种情况下人们希望 信号观测值是可信的。在这两种情况下，不管我们是否有一个合理的标准来选择系数，我们通常（统 计学上的）都可以通过调整滤波器系数来获得更好的性能。调整通常离线进行， 并经常与另一个（确定无误的）在线滤波器对比，这个过程称为系统识别。在讨论的结尾，我们指出在Q和R都是常数的条件下，过程估计误差协方 差R和卡尔曼增益都会快速收敛并保持为常量（参照图1-2中的更新方程）。 若实际情况也如此，那么滤波器系数便可以通过预先离线运行滤波器计算，或者， 比如说，用Grewal93中的方法计算P的稳定值。k实际中，观测误差R尤其不易保持不变。例如，用我们的光电跟踪仪观察 挂在房间顶棚面板上的信号灯时，较近的信号灯会比较远的信号灯具有较小的观 测噪声。不仅是观测噪声会变化，有时过程激励噪声协方差2也会随着滤波器运 行而动态变化一一这样2变成了 2k 来适应不同的动态状态。例如，在跟踪三维虚拟环境中用户头部位置时，如果用户头部缓慢移动，我们会减小Qk的幅 度，如果移动开始快速变化，则增加幅度。在这些情况下，Qk的幅度要根据用 户的意图和模型的不确定性来选择。2扩展卡尔曼滤波器3卡尔曼滤波器实践：估计随机常数前两节我们讨论了离散卡尔曼滤波器和扩展卡尔曼滤波器的基本形式。在这 儿，我们给出一个简单的例子以帮助读者更好地理解卡尔曼滤波器的实现和性 能。Andrew Straw在http:/www.scipy.org/Cookbook/KalmanFiltering 上给出了使 用Python/SciPy的具体实现方法。过程模型在这个简单的例子里我们估计一个常数随机变量，比如电压。假设我们可以 测量这个常数的幅值，但观测幅值中掺入了幅值均方根（Root-Mean-Square， RMS）为0.1伏的白噪声（比如在模数转换器不是很准确的情况下）。下面的线 性差分方程描述了整个过程：x = Ax + Bu + wk-1k-1k气-1 * "kH x + v = x + v所以4 = 1；没有控制输入，所以u = 0；测量值z 沉l，Z -k过程的状态不随时间变化包含噪声的观测值是状态变量的直接体现，所以H= 1。（注意有些地方我们忽略了下标k，因为对应的系数在这个例子中为常数。）滤波器方程和参数时间更新方程为:A - A -xk = xk-1测量更新方程为:K = P( P+R)t = P /( p+R)x = x+ K (Z Hx)k k kkP = (I - K) P假设过程噪声方差Q非常小，Q = 10-5（也可以令Q = 0，但是一个小的非零常数可以方便地调整滤波器参数，下面将会证明）。再假设由经验我们知道随 机常数的真值具有标准正态分布，因此我们令滤波器的初始条件为零，即类似地，我们要选择尸幻1的初值尸o。如果确定初始状态估计n=。，可以 令户。=o。但因为初始状态估计n并不确定，令尸o=o可能会使滤波器一直产 生x = 0的结果。就像实验验证的那样，尸o的选择并不关键，几乎任何尸o丰o k都会使滤波器最终收敛。在这里我们令尸o = 1。模拟实验首先，我们令常数标量x =-0.37727（因为x是真值所以并没有人符号）。然 后产生5o个不同的观测值zk，其误差为正态分布，期望为。，标准偏移为o.1 （先 前我们假设观测值掺进了幅值均方根为o.1伏的白噪声）。我们本可以在滤波器 运行时产生这些观测值，但预先准备好这些观测值然后再使用在几组不同的模拟 中可以让对照更有意义。第一组实验中我们固定测量方差为R =（o.1）2= o.o1。因为这正好是预先产生 的观测误差的方差的真值，所以在响应速度和估计方差方面这组实验应该具有最 好的性能。这在与第二、三组实验的对比中更能显现出来。图3-1画出了第一组 实验的结果。实线代表随机变量的真值x =-0.37727,加号代表预先产生的观测噪 声，剩下的曲线是滤波器的估计结果。+ + * + noisy measure me nts a posteri estimate truth vslua203。Iteration4050图3-1第一组实验：R = （0.1）2=0.01。实线代表随机变量的真值x =-0.37727,加号 代表预先产生的观测噪声，剩下的曲线是滤波器的估计结果。前面讨论尸0的选择时我们提到过，只要尸0 0其取值并不是特别关键，因为最终滤波器总要收敛。下面的图3-2给出了每次重复迭代后的P值。第50次迭 k代后它已从最初的1下降到0.0002 （平方伏特）了。第50次迭代后初始误差协方差P已从最初的1下降到0.0002 （平方伏特）了。 k第一节“滤波器系数和调制”中我们简单地讨论了改变或调整系数2和R对 性能的影响。图3-3和图3-4显示了将R增大或减少100倍的结果。图3-3中观测方第二次实验：R= 1。滤波器对观测值的反应变慢，导致估计方差减小。图3-4中观测方差减小了100倍（也就钮=0.0001），因此滤波器收敛地更 快。对常数的估计相对比较直接，这样便清晰地显示了卡尔曼滤波器的工作性 能。特别是在图3-3中，估计值要比含噪声的观测值平滑很多，显著地表明了卡 尔曼滤波器的“滤波”特性。参考文献1 Brown92 Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random Signals and Applied Kalman Filtering, Second Edition, John Wiley & Sons, Inc.2 Gelb74 Gelb, A. 1974. Applied Optimal Estimation, MIT Press, Cambridge, MA.3 Grewal93 Grewal, Mohinder S., and Angus P. Andrews (1993). Kalman Filtering Theory and Practice. Upper Saddle River, NJ USA, Prentice Hall.4 Jacobs93 Jacobs, O. L. R. 1993. Introduction to Control Theory, 2nd Edition. Oxford University Press.5 Julier96 Julier, Simon and Jeffrey Uhlman. "A General Method of Approximating Nonlinear Transformations of Probability Distributions," Robotics Research Group, Department of Engineering Science, University of Oxford cited 14 November 1995. Available from http:/www.robots.ox.ac.uk/siju/ work/publications/Unscented.zip. Also see: "A New Approach for Filtering Nonlinear Systems" by S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, Proceedings of the 1995 AmericanControl Conference, Seattle, Washington, Pages:1628-1632. Available from http:/www.robots.ox.ac.uk/siju/work/publications/ACC95_pr.zip. Also see Simon Julier's home page at http:/www.robots.ox.ac.uk/siju/.6 Kalman60 Kalman, R. E. 1960. “A New Approach to Linear Filtering andPrediction Problems," Transaction of the ASMEJournal of Basic Engineering, pp. 35-45 (March 1960).7 Lewis86 Lewis, Richard. 1986. Optimal Estimation with an Introduction to Stochastic Control Theory, John Wiley & Sons, Inc.8 Maybeck79 Maybeck, Peter S. 1979. Stochastic Models, Estimation, and Control, Volume 1, Academic Press, Inc. Sorenson70 Sorenson, H. W.9 1970. “Least-Squares estimation: from Gauss to Kalman,” IEEE Spectrum, vol. 7, pp. 63-68, July 1970