卡尔曼滤波器介绍

卡尔曼滤波器介绍摘要在I960年，REKalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。Kalman滤波器是一套数学等式，它提供了一种有效的以最小均方误差来估计系统状态的计算（递归的）方法。它在以下几方面是非常强大的：它支持过去、现在、甚至将来估计，甚至在系统准确模型也未知的情况下。本文的目的是提供一种对离散的Kalman滤波器的实用介绍。这些介绍包括对基本离散kalman滤波器、起源和与之相关的简单（有形）的带有真实数字和结果的描述和讨论。1、离散的kalman滤波器在I960年，REKalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。关于kalman滤波器一般方法的友好介绍可以在〔maybeck79〕的Chapter.1中找到，但是更完整部分的讨论能在〔Sorenson70〕中发现，它还包括许多有趣的历史解释。在〔Gelb74；Grewal93；Maybeck79；Lewis86；Brown92；jacobs93〕中有更多参考。估值过程Kalman滤波器解决估计离散时间控制过程的状态XWRn的一般性问题，定义线性随机差分方程-4分方程-4-=心卜1十也坠十叫・「（1」）其中，测量值ZURm，定义为=g+S.随机变量Wk和VK各自表示系统噪声和测量噪声，我们假定它们为相互独立的、白噪声且为正常概率分布p（w}^N（0,（2）T （1.3）P（.v）-Ar（U尺「在实际中，系统噪声协方差矩阵Q和测量噪声协方差矩阵R可能随过程和测量时间而改变，无论怎样，我们在这里假定它们是常量。在差分方程（1.1）中，nxn阶矩阵A与前一时刻（K-1）和当前时刻K相关，这里缺少传递函数或系统噪声。注意的是，在实际中，A可能随各自时刻改变，但这里我们假定其为常量，nxl阶矩阵R与非强制性输入UURi和状态x有关，在测量公式（1.2）中，mxn阶矩阵H与状态及测量值ZK有关，在实际中，H可能随各自过程或测量时刻而改变，这里假定它们是常数。滤波器计算初步我们定义X-URn（注意负号）为k时刻及系统k时刻以前数据的priori状态估计，定义KXK-URn在得到测量值ZK的k时刻的posteriori状态估计。我们这时定义前后两状态的估计误差为ck=xk-I*,and这时priori估计协方差为T6=倂咯1 〔I①并且posteriori估计协方误差为在推导kalman滤波器方程时，我们开始找至^Posteriori状态估计与priori估计XK-和实际测量值Z与预测值Hx-之差的加权的线性组合的公式，如式（1.7）。对于（1.7）的一些K k调整在下面的"滤波器的概率初步”中给出。(1.7)式（】.7）中（zk-Hxk）的差叫测量协方差或叫余数，这余数反映的是预测值Hxk与实际值Zk的不合。一个零余数意味着这两个数完全一致。式（1.7）中NXm阶矩阵选择Posteriori协方误差的最小增益或混合因子，这最小值可以获得：首先代式（1.7）到上面定义的ek,代入到（1.6）中，得到期望值，然后然后推导期望结果K的迹，并设其为0,最后解得K。对于更详细的看〔Maybeck79；Brown92；acobs93〕。最小化式（1.6）的结果K的一种形式如下Kk= +R}~X(l-«)HPkHT+R从（1.8）中，我们可以看到测量均方误差R趋于0时，增益K加权余数会越大，尤其li111Kl=H-1.另一方面，当Priori估计协方误差P一趋于0时，增益k加权余数越小，尤其Klim心=（）.&一（〕考虑加权K的另一种方法:当测量协方误差R趋于0时滇实测量值Zk越来越真实”这时，预测值Hx一越来越不真实，另一方面，当Priori估计协方误差P一趋于0时，真实测量值Zkk K越来越不真实，预测值Hx一越来越不真实。k滤波器概率初步式（1.7）的调整来源制约于在先刖测量值兀（Bayes准则）上Priori估计的概率。此时，我们足够指出：Kalman滤波器保持了分布状态的一、二阶矩。二&E［（耳-切（心-切叮=%式（1.7）的Posteriori状态估计反映了分布状态的均值（一阶矩）——这是在条件（1.3）和（1.4）同时满足的自然分布。Posteriori估计协方误差（1.6）反映分布状态的变化（二阶非中心矩），换之，卩（耳应）~"El订EL（心-耳）（耳-基）仃）二M耳，铁）•对于Kalman滤波器的更详细的概率初步，可以参考〔Maybeck79；Brown92；Jacobs93〕。离散Kalman滤波器算法我们从大体概述了一种包含离散Kalman滤波器形式的高级算法来开始这部分（看以前脚注）。在描述完它的高级目的之后，我们将在滤波器的本文集中到特定的公式和应用。Kalman滤波器是用反馈控制的形式来估计过程：在当时滤波器估计过程状态，然后在噪声测量值时获得反馈。比如，Kalman滤波器的等式有两组：timeupdate等式和measurementupdate等式。这timeupdate等式是当前状态之前的过程和获得下一时刻的Priori状态的估计协方误差。这measurementupdate等式反映的是反馈。如伴有新测量值的Priori状态估计和获得提高的Posteriori估计的组合。当measurementupdate被作为修正方程时，timeupdate也被作为原始等式。确实，最后的估计算法与解决数字问题的预测-修正算法相似，如下Figure1-1所示

TimeUpdakCPmdiE)MeAsuremenlUpdile

("Corrttl''}TimeUpdakCPmdiE)MeAsuremenlUpdile

("Corrttl''}Figure1-1不间断离散Kalman滤波器循环，Timeupdate适时计算当前状态估计。Measurementupdate在那时通过真实测量值来调整设计估计。在Table1-1和Table1-2表示暂态和稳态方程Table1-1:DiscreteKalmanfiltertimeupdateequations.一L=Ah_1+Bn上_、 (1.9)Pk=APk_,AT+Q (LIO)再次注意，在Table1-1计划中，无论Timeupdate方程如何，状态和协方差估计从K-1状态到K状态。当Q来自式(1.3)是，A和B来自式(1.1)。滤波器的内部条件在早先的参考书中已经讨论了。Table1-2:DiscreteKalmanfiltermeasurememupdateequations.k=P'kHT(HP'kH7+/!)"1(l」l)耳=xk^Kk(zk-Hxk}(1-12)耳=(I~KkH)p-k(1-13)在Measurementupdate期间，最初任务是计算Kalman滤波器的增益％。注意的是，当式（1.11）和（1.8）相同时，等式已经给出。下一步是根据真实计算过程来获得Zk。然后通过式（1.12）合并测量值来生成Posteriori状态估计。式（1.12）在这里是式（1.7）的完全重复。最后通过式（1.13）来获得Posteriori估计协方误差。每次Timeupdate和口Measurementupdate成对后，系统重复用以前的Posteriori估计过去计划或预测的新的Priori估值。这递归的本质是Kalman滤波器的一大特色——它的实际应用比设计每次操作直接数据的Wiener滤波器的应用更为有效〔Brown92〕。在过去所有过去测量值的基础上Kalman滤波器递归的代替当前估计。下面的Figure1-2提供了滤波器操作的完整图片，从Table1-1和Table1-2组合成前面图表Figure1-1。滤波器参数和调整在滤波器的实际应用中，测量噪声协方差R通常先于滤波器操作之前测量。测量值协方误差R—般是实际的（可能的）因为我们能够测量过程，无论如何（当运行滤波器）为了决定测量噪声的变化我们一般能够得到离线例子测量值。系统噪声协方误差Q的测定一般是很困难的，因为我们不能直接得到观测估计过程。有时候相关简单的系统模型能产生可能的结果，如果通过选择Q它注入足够不确定进入过程。的确，在这种情况下，我们希望系统测量值是可信的。在另一种情况，无论我们是否选择一个有理数参数，时间前级滤波器参数（统计说）通过调整滤波器参数Q和R便能得到。这个调整经常离线操作，通常的在系统中，另一种（明显的）Kalman滤波器一般参考系统鉴定。

TimeLpddteCTredkl't（I）Cc^npiLtcIheKalmanguinKkTimeLpddteCTredkl't（I）Cc^npiLtcIheKalmanguin⑴Projectthestaleahead瓦=曲-1+B叫⑴Projectthestaleahead瓦=曲-1+B叫.（.2）Upilulcc-[Lmaiewill】jnua->uleinem:;.Initialestimates1'ur'：*.,lliilIPk_）Figure1-2Kalman滤波器操作的完整图片，Table1-1和Table1-2组合成在结束时，我们注意在Q和R是常数的条件下，估计协方误差PK和Kalman增益Kk将快速稳定，然后保持常量（看Figure1-2滤波器修正公式）。如果这种场合，这些参数能在〔Grewal93〕中通过离线运行滤波器或决定P/勺稳态值来提前计算。K测量协方误差（特别的）不能保持常数是通常情况。例如，当在我们的光电跟踪面板看到信号是，在靠近信号的测量值比远离信号的测量噪声将更小。同样，系统噪声Q在滤波操作一变成Qk——期间为了调整动态差有时候也会动态的改变。例如，在跟踪虚拟环境的使用过程情况下，如果目标移动慢，我们能够减/」Q的量值，如果动态变化快，我们增K加量值。在这种情况下，qk能够选择计算不确定的用户目的和用户模型。2、扩展的Kalman滤波器（EKF）估值系统正如上一节的描述Kalman滤波器解决估计离散时间控制过程的状态XER.的一般性问题，定义线性随机差分方程。但是如果被估值系统或系统的测量值关系是非线性的，会发生什么变化呢？许多Kalman滤波器重要的或成功的应用已用于这种情况。线性Kalman滤波器的当前均值和协方差可以作为EKF的参考。在类似Taylor级数的时候，即使是非线性关系时我们也能围绕当前估计，通过系统的部分推导公式和测量公式计算估计来把估值线性化。为了如此，我们在本部分必须修改一些重要描述。我们再次假定系统有一个状态矢量XERn,但是，这个系统现在被定义为非线性随机差分方程。S— "上_1?A'—1 ' (-•I)其中，测量值zeRm，定义为这里，随机变量WK和VK再次表示系统噪声和测量噪声。正如式（1.3）和（1.4）一样。在这种情况下，在差分方程式（2.1）中，线性函数f与上时刻状态K-1和当前时刻状态K有关。它包括驱动函数人車和零均值系统噪声Wk的参数。在测量等式（2.2）中，非线性函数1与状态XK和测量值zK有关。在实际过程中，我们不知道每个时刻的wk和vK的独立值，然而，我们可以在没有wK和VK的状态下近似状态矢量和测量矢量，如下九=川卜J"and:.k=灿片（2.4）这里，Xk是Posteriori估计状态（从上一个时刻K开始）重点注意：EKF和基本缺陷是在遭到各自非线性变换后，不同的随机变量的分布（连续情况下的密度）不再正常。在EKF是简单的接近线性最佳Bayes公式的特殊状态估值。Julieretal.已经通过用非线性变换来优质正常分布来了发展了EKF变量〔Julier96〕

滤波器计算初步为了估计非线性系统差分值和测量值的关系，我们重新写线性估计式(2.3)和(2.4)方程的控制方程，(2.5)(2-6)'严耳+川U-1-耳—1)+Wh氐—]讥"耳+H(叫—耳)+"切(2.5)(2-6)xK和zK是真实状态和测量矢量,XK和ZK是由式(2.3)和(2.4)而得到近似状态和测量值矢量,XK是K时刻的Posteriori估计状态,随机变量WK和VK表示在(1.3)和的(1.4)的系统噪声和测量噪声,A是关于X的由f部分派生的Jacobian矩阵,定义为W是关于w的由f部分派生的Jacobian矩阵,定义为IVIIVIiI*［厂］=齐石H是关于X的由h部分派生的Jacobian矩阵,定义为H^.J\V是关于v的由h部分派生的Jacobian矩阵,定义为⑴严)•在这种情况下简单注意，我们不能用Jacobians的A,W,H,的时间下标，即使在各自时刻真正不同。现在我们为预测误差定义一个新符号，L三*一耳， （2・7）和测量余数，记得，在实际中，式2.7不能接近Xk,它便是真实状态矢量，例如，要估计的量。另一方面，式2.8不能接近Xk,它是用Xk估计真实测量值。用式（2.7）和（2.8）我们能写系统误差的控制方程，如下L"（乜-1-H-1）+Ej 29）这里，.和nk表示新的有零均值和协方差WQWt和VRVt并同带有Q和R的式（1.3）和式（1.4）一样的独立随机变量。注意的是等式（2.9）和等式（2.10）是线性的，从离散Kalman滤波器我们得到真得得到类似的差分方程和测量等式（1.1）和（1.2）。这种激励在式（2.8）用真实测量值余数Ezk和第二（假定的）Kalman滤波器来估计预测误差Ex“由式（2.9）给出，然后这口电疋测量能连k K同式（2.7）被用来获得原始非线性系统的Posteriori状态估计，如下以二耳+纭 （2式（2.9）和（2.10）的随机变量有近似的下面可能的分布（看以前脚注）：師恳1）f）Uk）-N^WQkWr）P{}]k）-N（S\VRkVT）

给定一些ek的近似值和预测值为0,用来估计ek的Kalman滤波器等式是(212)把式（2.12）代回（2.11）,利用（2.8）,我们可以看到，实际不用两个Kalman滤波器。式（2.13）在扩展的Kalman滤波器中用作Measurementupdate，其中X和z来源于KK式（2.3）和式（2.4）,Kalman增益耳来自带有测量协方差的特有代替式（1.11）EKF完整等式如下Talble2-1和Table2-2所示。注意，我们用Xk-代替Xk，并且保持了与以前上标负号的一致。现在我们给JacobiansA,W,H,V,附加下标k来标注他们在各个时刻的不同。l^blc2-1:EKFLimenpihLccqLinLions:.忑二八lW-lM忑二八lW-lM(2.I4J如同基本的离散Kalman滤波器在Table2-1中的Timeupdate等式计算从前一时刻K-1到当前时刻K的估计状态和协方差。此外，式（2.14）的f来源于式（2.3）,Ak和Wk是K时刻的系统Jacobians,qk是K时刻的系统噪声协方差。liblc2-2:EKFmeas:LiremeritLipdaLce屮LMiiw心=户网“疋戶"叫叫U厂电=电十肌㈡-加仏⑴］◎◎=U—K皿珥(2.ISJ如同基本离散Kalman滤波器，Table2-2中Measurementupdate等式修正了测量值Zk的估计状态和协方差。此外式2.17的h来自式;2.4）Hk和V是K时刻的测量值Jacobians,Rk是测量噪声协方差（注意，现在R的下标允许随每个测量值而改变）EKF的基本算法同线性离散Kalman滤波器Figure1-1所示的一样，下面的合并了前面表格Figure1-1和Table2-1和Table2-2等式的Figure2-1提供了EKF算法的完整描述。iircmentLlpdjiteii