卡尔曼滤波器的简单推导与直观理解-中文版-副本

1、卡尔曼滤波器的简单直观推导与直观理解摘要：本文给出了卡尔曼滤波器的一种简单又直观的推导过程， 目的是让那 些没有很强数学背景的学生学会使用这个有用的工具。该文中最复杂的数学要求 是理解两个高斯函数的乘积并对其进行化简。卡尔曼滤波器的提出至今已经有五十多年（补充：卡尔曼滤波器的提出时间 为1960年），但它仍是目前最重要也是最常用的数据融合算法之一。卡尔曼滤波器以鲁道夫卡尔曼的名字命名，其具有计算量小、递归方程简洁等优点，并且它 是一维高斯线性系统的最优估计器。卡尔曼滤波器的典型用途包括噪声数据平滑 和参数估计。应用包括全球定位系统的接收器、 无线电设备中的锁相环、笔记本 电脑触控板的输出等等。

2、从理论上讲，卡尔曼滤波器是一种在线性动力学系统中允许精确推演的算法， 该算法是一种类似隐马尔可夫的贝叶斯模型，不同的是隐变量的状态空间是连续 的，并且所有隐变量和观测量测均服从高斯分布（通常为多元高斯分布）。本讲义的目的是通过简单和直观的推导让对这些概念感到困惑或惧怕的人理解卡尔 曼滤波器的基础。相关工作卡尔曼滤波器1（以及其变形例如扩展卡尔曼滤波器2和无迹卡尔曼滤波 器3）是信息处理领域最著名和受欢迎的数据融合算法之一。 早期，卡尔曼滤波 器最著名的应用是在将尼尔阿姆斯特朗送至月球的阿波罗导航计算机中，并且（最重要的是）将他带了回来。目前卡尔曼滤波器已经应用在卫星导航设备、智 能手机和许多计

3、算机游戏中。卡尔曼滤波器的典型推导方法是使用最小均方误差估计器 4,该方法适合 数学功底较强的学生，但不适用数学功底较弱的学生。此处卡尔曼滤波器的推导 的关键是高斯分布的一条重要特性一一两个高斯分布的乘积是另一个高斯分布。对读者的要求本文并非为刚接触卡尔曼滤波器的学生提供一个全面的教程， 而是面向对卡 尔曼滤波器有所了解，但是数学功底不够强的学生。读者应该熟悉与卡尔曼滤波 器相关的一些基本符号和术语，比如状态矢量和协方差矩阵。本文面向那些需要也适用于那些已经对卡尔曼本文并非作为面向新手的全以简单直观的方式向其他人教授卡尔曼滤波器的人, 滤波器有一定的了解但是不完全理解其基础的人们。面而独立的教

4、育工具，因为这将需要一章而非几页内容能够讲述清楚问题描述卡尔曼滤波器的目标状态演变模型假设系统状态从k 1时刻演变至k时刻时服从方程Xt FtXt 1 BtUt Wt（1）where其中?Xt为状态矢量，包含系统在t时刻感兴趣的参数（例如位置、速度和方位 角）。?Ut为控制矢量，包含控制项（例如转向角、节气门设置、制动力）? Ft为状态转移矩阵，它是将t 1时刻系统的每个参数的影响作用于 t时刻 （例如，t 1时刻位置和速度都作用于t时刻），译者注：状态转移矩阵的作 用为系统参数的预测。? Bt是控制输入矩阵，其将矢量Ut中的每个控制输入参数作用于状态矢量 （例如，将油门设置的效果作用于系统的

5、速度和位置）? Wt为过程噪声，并假设过程噪声服从均值为零、协方差矩阵为Qt的多变量高斯分布系统的量测可以建模为Zt HtXt Vt其中? Zt为量测矢量? Ht为量测矩阵，将状态矢量中的参数映射至量测维? vt为量测噪声矢量，假设服从均值为零协方差为 Rt的高斯白噪声。在下面的推导中，将考虑一个简单的一维跟踪问题，具体的为一列火车沿着 铁路线行驶的一维跟踪问题（如图 1所示）。因此，我们可以考虑该问题中的一 些小例矢量和矩阵。状态矢量xt包含火车的位置和速度信息，即XtXt &带有场声的最恻Prediebon (Estimate)Measurement Noisy)FIG1 This fig

6、ure the至辟 104n undN con$idertiart4Idi it国雇示f所考虑的加龈踪系统火车驾驶员可以向系统施加刹车或者加速这样的输入，在此我们考虑的输入为向 系统施加的力ft和火车的质量m。这些控制信息以控制矢量ut的形式进行存储,ut工m时间t期间，力ft与火车的位置和速度的关系如下:% xti &i tft2m& &ift tm这些线性方程可以用矩阵的形式写为Xt1 tXt i%01%i2t2 tftm2t2 t通过与式（i）比较，我看可以发现i tFtand Bt0 i系统的状态Xt无法直接观测，卡尔曼滤波器提供了一种算法，通过联合系统模型 和带有噪声的量测函数能够得

7、到系统状态的估计值 又。因此，状态矢量中感兴趣 参数的估计值由概率密度函数而不是离散的值给出。为了充分的表示高斯分布， 我们需要知道它们的方程和协方差，并且存储在协方差矩阵Pt中。Pt中主对角线 上的元素为状态矢量中相关元素的方差。Pt的副对角线提供状态矢量中元素之间 的协方差。在建模良好的一维线性系统中，量测误差为零均值的高斯分布，卡尔 曼滤波器已经被证明为最优估计器5。在本文的剩余部分，我们将推导卡尔曼滤 波器的方程，该方程能够联合先验知识、系统模型预测和带噪声的量测来递归计算又。卡尔曼滤波算法包括两个步骤：预测和量测更新。标准卡尔曼滤波器的预测 步骤的方程为?t|t i Pt|t iFt

8、?ti|tiBtUtFPt i|t iFtTQtEXtXt|t iXt%1t i (3),下面对方程其中Qt为过程噪声协方差矩阵。在上面的讨论中推导了方程 进行推导。未知的真实值Xt与预测值Xt-的方程可以表示为计算式(3)和(1)的差，可得|t ixtxt|t i = FtPt|t i EXt i * it iXtWtTXtXt|t iFt Xt iXt it iWtFtXt iXt itTiWtFtE殁 iXt it iTXt iXt it iFtTFtEXt i it iTwtE Wt Xt iT FtTTE WtWt因为状态估计误差和过程噪声不相关，所以有FtEXt iXt it iT

9、WtE wtTXt i X it iFtTPt|t iFtE Xtit itiXt iTXt i|t iFtTTWtWtPt|t iFtPt it iFtTQt测量更新方程为?t |tXt|t iKt ZtH tXt|t iPt|tPt|tiKtHtPt|ti(5)(6)where其中de两个高斯分KtPt|t iHT HtPt|t iHT Rt在论文的剩余部分，我们将利用第一原理(相关工作部分中提到 布的成绩还是一个高斯分布)推导量测更新方程 (5)(7)式。解决方法|FIG2 The inFilial knowledge st the system at time t = Oi The r

10、ed Gaussian di&lribLitiDn represents the pdf providing thE lirirtbal conf ide ncB in the KlimatE of the position of the tram. The arrow pointing to the right repreuntB ihe known initial vekichy of tht TrMrv & 最堆t印H的M蛤状思*虻色不厮野乐界:示的t奔r!画数过供了火4忡居曲叶的上龙胃仁支，他人次云火1酉打轴“向|FIG3 HttFkF. lliv prodiLtion of Iha

11、hull on of lilt trjin Jit tirna-1 = 1Jnd Un 加廿 3 of uncerljilnty in thml pvditlion ii ihowmi. The ronfldMce in the know/dgR of th哥 pan of tlw tralmdcrpa4ed, a$ ww ar not cerrairii if th train hx und#rgon anyaccelerations or川的atiun等 in the intervening period from r = 0 B t = 1图3这里绘(Hft二l财X的火隼粒宣BfflPB

12、JHI的不/定性.火车位置的K值度降怅J 这是一惶我ri不确定火隼从20到,1时菊是否遥行AG4| Shows the rrieauremenrt of the kjcaTion of the train at tiinie r = 1 and the Kewell of unceFtainty in that noisy measuremert,. rprHentd by th凿 b4ue Gaussian pdf. The ccmbimed knciM叁dge of ttiii system is. provided by muftipiyMg these two pdft BgethM国

13、I火里花厂1时划的位置用以覆置Hl不*出性程度用色的*%概率密度晶独入示.聚统的取舍信息由两个ndf的垂粗根0UfF(G5| Shows the rww pdf (grrap) generated by muhiply ing tfrie pdfs aswaateid wilii the preditrion and meBsunemerit of the trains I outran at tim t = 1B Thh new pdf provbdK the best estimate of tho location of tha traia, by fusing the data fro

14、m the predialon and the meanremenl.由t=l明刎或启位置的榛器I耶拒阳人息宰和li工H ：娓枳生或新晌粗窣密度国Ik (M色j .上吸新的喉备甯r班轮的il旃府Hi剖机员用肥令赛三火中位置的眼惺估i I .在此，通过考虑一个简单的以为跟踪问题，具体为一列火车行驶在一条铁路上,车位置的最佳估计值（或者更精确地说，是安装在火车车顶上的无线电天线的位置）。可用的信息来源有两个：1）基于上次获得的火车位置和速度的预测值以及2）部署在轨道侧的无线电测距系统对火车位置的测量结果。预测和量测信息的组合能够提供火车位置的最可能的估计值。该系统如图1所示已知一定精度的系统的初始

15、（t 0秒）状态，并如图2所示。火车的位置由高斯概率密度函数给出。我们能够根据t 0秒已知的火车的位置和速度信息以及火车最大的加速度来估计在下一时刻（t 1秒）火车最大可能出现的新位置。实际飞照（吕计Pr&cietioni (Esiirraiej /M1的噪声) MaiwiUFunMNMy)对卡尔曼滤波器的量测更新方程进行推导在每个量测到来时，我们希望知道火谕辑估汁)Prediction (Etmate用茗(带有唤力1Measuwniil (No6y)/zPredielion (Estimate)中，我们可能对驾驶员在刹车或者油门上的控制有所了解。无论如何，我们都可以火车的新位置，在图3中用新

16、的高斯概率密度函数进行表示， 该函数拥有新的 均值和方差。数学上，该步骤由式（1）表示。图中，方差的增加表示与t 0时 刻相比，位置估计的精度有所降低，这是因为从t 0到t 1任何加速或者减速的 不确定性引起的火车噪声增加引起的。在t 1时刻，我们还使用无线电定位系统对火车的位置进行了量测，如图 4 中蓝色部分的高斯概率密度函数所示。 通过结合预测和量测信息，可以对火车的 位置做出最佳估计。两者的结合方法可以通过两个概率密度函数的乘积获得， 如 图5所示。利用高斯函数的一个关键性质：两个高斯函数的乘积是另一个高斯函数。这 是至关重要的，因为它允许随着时间的增长而增加无数的高斯 pdf,但是结果

17、函 数的复杂度或项数不会增加；在每个时间段之后，新的 pdf都完全由高斯函数表 示。这是卡尔曼滤波的递归特性能够简洁的关键所在。图中所描述的过程下面给出去数学形式以推导卡尔曼滤波器的量测更新方 程。图3中红色高斯函数表示的预测概率密度函数可以表示为方程Vi r；2(8)图4中蓝色高斯函数表示的量测概率密度函数可以表示为方程2V2 r；这两个概率密度函数提供的信息通过将两者相乘得到，即同时考虑预测和量测 （如图5所示）。因此，新的概率密度函数表示的融合信息来自预测和量测，表示为当前时刻系统的最优估计，并由两个高斯函数的乘积给出，即 TOC o 1-5 h z 22r 1r 2(10)12 121

18、2 fe 1e 2212222r 12 12得到的新函数可以重新写成高斯形式yfused r ； fused , fused2r fused2 2 fused2fused(11)其中fused2212212212212112212(12)并且22212fused 22124(13) TOC o 1-5 h z 2112212关于式(11)至(13)的推导过程可以参考文献6。最后两个方程表示卡尔曼滤波器算法的更新步骤，具体的如下文所示。然而，为 了呈现更一般的情况，我们需要考虑扩展此示例。在上面的例子中，假设预测和量测是相同的坐标系中以相同的单位进行的。这会产生一对特别简洁的方程式， 分别代表预

19、测和两个更新步骤。然而，需要注 意的是，在实际中往往需要一个函数将预测和量测映射到同一域中。 本文中例子 的一个现实扩展为，火车的位置的预测直接沿着铁路线并且单位是米， 但是行驶 时间的以秒为单位记录。为了使预测和量测的概率密度函数相乘， 必须将一个概率密度函数转换至另一个概率密度函数的域，标准的做法是通过转换矩阵H t将预测映射到量测域。现在，我彳门回顾式(8)和(9), y1和y2不在表示沿着铁路线行进的米数，我们考虑分别y2表示从x 0处无线电天线发出的电磁波传播至火车所在位置时所用 的秒数。空间预测概率密度函数通过光速c来缩放函数从而转换至量测域。因 此，方程(8)和(9)必须重新为2

20、r c2y s; 1,(14)2 1 c以及(15)其中，两个分部现在都定义在了量测域， 射频信号沿着时间轴传播，并且量测单 位是秒。按照之前的推导我们现在发现fusedc211-2-ccfused -21c21c(16)替换 H 1. c and K H 12 : H2 12可得fusedi+K(17)同样，融合的协方差估计变为2 fused2 c41c21c2fused21c212-2cKH 12(18)现在，我们可以将标量推导得出的某些项与卡尔曼滤波算法中使用的标准向量和 矩阵进行比较：fused12 fused21222HXt|t :数据融合后的状态向量?t|t 1:数据融合之前的状态

21、向量，即预测心：数据融合后的协方差矩阵（置信度） 卜Pt|t 1 :数据融合前的协方差矩阵（置信度）Zt:量测矢量Rt:量测噪声协方差矩阵Ht:用于将状态向量参数映射到测量域的转换矩阵H 21口 2 21 2Kt %K；也巳K；Rt :卡尔曼增益。H 12现在可以很容易地看到标准卡尔曼滤波器方程与上面推导的（17）和（18）的关系： H12fused 122221H 12?t|tXt|t 1 K t ztHt?t|t 12 fusedH2221Pt|t Pt|t 1 KtHtPtt 1结论卡尔曼滤波器可以通过一个简单的推导 （涉及标量数学、基本代数操作）和一个 易于理解的思想实验来教授。这种方法应该允许对数学缺乏兴趣的学生以一种直 观的方式理解卡尔曼滤波器的数学核心，并通过高斯函数的唯一乘法特性能够理 解该滤波器的递归特性。参考文献R. E. Kalman, A new approa