卡尔曼滤波器介绍.doc

卡尔曼滤波器介绍摘要在1960年，R.E.Kalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。Kalman滤波器是一套数学等式，它提供了一种有效的以最小均方误差来估计系统状态的计算(递归的)方法。它在以下几方面是非常强大的：它支持过去、现在、甚至将来估计，甚至在系统准确模型也未知的情况下。本文的目的是提供一种对离散的Kalman滤波器的实用介绍。这些介绍包括对基本离散kalman滤波器、起源和与之相关的简单(有形)的带有真实数字和结果的描述和讨论。1、离散的kalman滤波器在1960年，R.E.Kalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。关于kalman滤波器一般方法的友好介绍可以在maybeck79的 Chapter.1中找到，但是更完整部分的讨论能在Sorenson70中发现，它还包括许多有趣的历史解释。在Gelb74；Grewal93；Maybeck79；Lewis86；Brown92；jacobs93中有更多参考。估值过程Kalman滤波器解决估计离散时间控制过程的状态XRn的一般性问题，定义线性随机差分方程其中，测量值ZRm，定义为随机变量WK和VK各自表示系统噪声和测量噪声，我们假定它们为相互独立的、白噪声且为正常概率分布在实际中，系统噪声协方差矩阵Q和测量噪声协方差矩阵R可能随过程和测量时间而改变，无论怎样，我们在这里假定它们是常量。在差分方程（1.1）中，nn阶矩阵A与前一时刻（K1）和当前时刻K相关，这里缺少传递函数或系统噪声。注意的是，在实际中，A可能随各自时刻改变，但这里我们假定其为常量，nl阶矩阵R与非强制性输入URl和状态x有关，在测量公式（1.2）中，mn阶矩阵H与状态及测量值ZK有关，在实际中，H可能随各自过程或测量时刻而改变，这里假定它们是常数。滤波器计算初步我们定义XKRn（注意负号）为k时刻及系统k时刻以前数据的priori状态估计，定义XKRn在得到测量值ZK的k时刻的posteriori状态估计。我们这时定义前后两状态的估计误差为这时priori估计协方差为并且posteriori估计协方误差为在推导kalman滤波器方程时，我们开始找到Posteriori状态估计XK与priori估计XK和实际测量值ZK与预测值Hxk之差的加权的线性组合的公式，如式（1.7）。对于（1.7）的一些调整在下面的“滤波器的概率初步”中给出。式（1.7）中（ZKHxk）的差叫测量协方差或叫余数，这余数反映的是预测值Hxk与实际值Zk的不合。一个零余数意味着这两个数完全一致。式（1.7）中nm阶矩阵选择Posteriori协方误差的最小增益或混合因子，这最小值可以获得：首先代式（1.7）到上面定义的ek ，代入到（1.6）中，得到期望值，然后然后推导期望结果K的迹，并设其为0，最后解得K。对于 更详细的看Maybeck79；Brown92；Jacobs93。最小化式（1.6）的结果K的一种形式如下从（1.8）中，我们可以看到测量均方误差R趋于0时，增益K加权余数会越大，尤其另一方面，当Priori估计协方误差PK趋于0时，增益k加权余数越小，尤其考虑加权K的另一种方法：当测量协方误差R趋于0时，真实测量值ZK越来越真实，这时，预测值Hxk越来越不真实，另一方面，当Priori估计协方误差PK趋于0时，真实测量值Zk越来越不真实，预测值Hxk越来越不真实。滤波器概率初步式（1.7）的调整来源制约于在先前测量值ZK（Bayes准则）上Priori估计XK的概率。此时，我们足够指出：Kalman滤波器保持了分布状态的一、二阶矩。式（1.7）的Posteriori状态估计反映了分布状态的均值（一阶矩）这是在条件（1.3）和（1.4）同时满足的自然分布。Posteriori估计协方误差（1.6）反映分布状态的变化（二阶非中心矩），换之，对于Kalman滤波器的更详细的概率初步，可以参考Maybeck79；Brown92；Jacobs93。离散Kalman滤波器算法我们从大体概述了一种包含离散Kalman滤波器形式的高级算法来开始这部分（看以前脚注）。在描述完它的高级目的之后，我们将在滤波器的本文集中到特定的公式和应用。Kalman滤波器是用反馈控制的形式来估计过程：在当时滤波器估计过程状态，然后在噪声测量值时获得反馈。比如，Kalman滤波器的等式有两组：time update等式和measurement update等式。这time update等式是当前状态之前的过程和获得下一个时刻的Priori状态的估计协方误差。这measurement update等式反映的是反馈。如伴有新测量值的Priori状态估计和获得提高的Posteriori估计的组合。当measurement update被作为修正方程时，time update也被作为原始等式。确实，最后的估计算法与解决数字问题的预测修正算法相似，如下Figure 1-1所示Figure 1-1 不间断离散Kalman滤波器循环，Time update适时计算当前状态估计。Measurement update在那时通过真实测量值来调整设计估计。在Table 1-1和Table 1-2表示暂态和稳态方程再次注意，在Table 1-1计划中，无论Time update方程如何，状态和协方差估计从K-1状态到K状态。当Q来自式（1.3）是，A和B来自式（1.1）。滤波器的内部条件在早先的参考书中已经讨论了。在Measurement update期间，最初任务是计算Kalman滤波器的增益Kk。注意的是，当式（1.11）和（1.8）相同时，等式已经给出。下一步是根据真实计算过程来获得Zk。然后通过式（1.12）合并测量值来生成Posteriori状态估计。式（1.12）在这里是式（1.7）的完全重复。最后通过式（1.13）来获得Posteriori估计协方误差。每次Time update和Measurement update成对后，系统重复用以前的Posteriori估计过去计划或预测的新的Priori估值。这递归的本质是Kalman滤波器的一大特色它的实际应用比设计每次操作直接数据的Wiener滤波器的应用更为有效Brown92。在过去所有过去测量值的基础上Kalman滤波器递归的代替当前估计。下面的Figure 1-2提供了滤波器操作的完整图片，从Table 1-1和Table 1-2组合成前面图表Figure 1-1。滤波器参数和调整在滤波器的实际应用中，测量噪声协方差R通常先于滤波器操作之前测量。测量值协方误差R一般是实际的（可能的）因为我们能够测量过程，无论如何（当运行滤波器）为了决定测量噪声的变化我们一般能够得到离线例子测量值。系统噪声协方误差Q的测定一般是很困难的，因为我们不能直接得到观测估计过程。有时候相关简单的系统模型能产生可能的结果，如果通过选择Q它注入足够不确定进入过程。的确，在这种情况下，我们希望系统测量值是可信的。在另一种情况，无论我们是否选择一个有理数参数，时间前级滤波器参数（统计说）通过调整滤波器参数Q和R便能得到。这个调整经常离线操作，通常的在系统中，另一种（明显的）Kalman滤波器一般参考系统鉴定。Figure 1-2 Kalman滤波器操作的完整图片，Table 1-1和Table 1-2组合成前面图表Figure 1-1。在结束时，我们注意在Q和R是常数的条件下，估计协方误差PK和Kalman增益KK将快速稳定，然后保持常量（看Figure 1-2滤波器修正公式）。如果这种场合，这些参数能在Grewal93中通过离线运行滤波器或决定PK的稳态值来提前计算。测量协方误差（特别的）不能保持常数是通常情况。例如，当在我们的光电跟踪面板看到信号是，在靠近信号的测量值比远离信号的测量噪声将更小。同样，系统噪声Q在滤波操作变成Qk期间为了调整动态差有时候也会动态的改变。例如，在跟踪虚拟环境的使用过程情况下，如果目标移动慢，我们能够减小QK的量值，如果动态变化快，我们增加量值。在这种情况下，QK能够选择计算不确定的用户目的和用户模型。2、扩展的Kalman滤波器（EKF）估值系统正如上一节的描述，Kalman滤波器解决估计离散时间控制过程的状态XRn的一般性问题，定义线性随机差分方程。但是如果被估值系统或系统的测量值关系是非线性的，会发生什么变化呢？许多Kalman滤波器重要的或成功的应用已用于这种情况。线性Kalman滤波器的当前均值和协方差可以作为EKF的参考。在类似Taylor级数的时候，即使是非线性关系时我们也能围绕当前估计，通过系统的部分推导公式和测量公式计算估计来把估值线性化。为了如此，我们在本部分必须修改一些重要描述。我们再次假定系统有一个状态矢量XRn，但是，这个系统现在被定义为非线性随机差分方程。其中，测量值ZRm，定义为这里，随机变量WK和VK再次表示系统噪声和测量噪声。正如式（1.3）和（1.4）一样。在这种情况下，在差分方程式（2.1）中，线性函数f与上时刻状态K-1和当前时刻状态K有关。它包括驱动函数UK-1和零均值系统噪声Wk的参数。在测量等式（2.2）中，非线性函数h与状态XK和测量值ZK有关。在实际过程中，我们不知道每个时刻的WK和VK的独立值，然而，我们可以在没有WK和VK的状态下近似状态矢量和测量矢量，如下这里，Xk是Posteriori估计状态（从上一个时刻K开始）。重点注意：EKF和基本缺陷是在遭到各自非线性变换后，不同的随机变量的分布（连续情况下的密度）不再正常。在EKF是简单的接近线性最佳Bayes公式的特殊状态估值。 Julier et al.已经通过用非线性变换来优质正常分布来了发展了EKF变量Julier96滤波器计算初步为了估计非线性系统差分值和测量值的关系，我们重新写线性估计式（2.3）和(2.4)方程的控制方程,这里l XK和ZK是真实状态和测量矢量,l XK和ZK是由式(2.3)和(2.4)而得到近似状态和测量值矢量,l XK是K时刻的Posteriori估计状态,l 随机变量WK和VK表示在(1.3)和的(1.4)的系统噪声和测量噪声,l A是关于X的由 f 部分派生的Jacobian矩阵,定义为l W是关于 w 的由 f 部分派生的Jacobian矩阵,定义为l H是关于X的由 h 部分派生的Jacobian矩阵,定义为l V是关于 v 的由 h 部分派生的Jacobian矩阵,定义为在这种情况下,简单注意,我们不能用Jacobians的A，W，H，的时间下标，即使在各自时刻真正不同。现在我们为预测误差定义一个新符号，和测量余数，记得，在实际中，式2.7不能接近k，它便是真实状态矢量，例如，要估计的量。另一方面，式2.8不能接近k，它是用Xk估计真实测量值。用式（2.7）和（2.8）我们能写系统误差的控制方程，如下这里，k和k表示新的有零均值和协方差WQWT和VRVT并同带有Q和R的式（1.3）和式（1.4）一样的独立随机变量。注意的是等式（2.9）和等式（2.10）是线性的，从离散Kalman滤波器我们得到真得得到类似的差分方程和测量等式（1.1）和（1.2）。这种激励在式（2.8）用真实测量值余数Ezk和第二（假定的）Kalman滤波器来估计预测误差Exk由式（2.9）给出，然后这叫EK测量能连同式（2.7）被用来获得原始非线性系统的Posteriori状态估计，如下式（2.9）和（2.10）的随机变量有近似的下面可能的分布（看以前脚注）：给定一些ek的近似值和预测值为0，用来估计ek的Kalman滤波器等式是把式（2.12）代回（2.11），利用（2.8），我们可以看到，实际不用两个Kalman滤波器。式（2.13）在扩展的Kalman滤波器中用作Measurement update，其中XK和ZK来源于式（2.3）和式（2.4），Kalman增益KK来自带有测量协方差的特有代替式（1.11）。EKF完整等式如下Talble 2-1和Table 2-2所示。注意，我们用Xk代替Xk，并且保持了与以前上标负号的一致。现在我们给Jacobians A，W，H，V，附加下标k来标注他们在各个时刻的不同。如同基本的离散Kalman滤波器，在Table2-1中的Time update等式计算从前一时刻K-1到当前时刻K的估计状态和协方差。此外，式（2.14）的f来源于式（2.3），Ak和Wk是K时刻的系统Jacobians，QK是K时刻的系统噪声协方差。如同基本离散Kalman滤波器，Table 2-2中Measurement update等式修正了测量值Zk的估计状态和协方差。此外，式（2.17）的h来自式（2.4），Hk和V是K时刻的测量值Jacobians，Rk是测量噪声协方差（注意，现在R的下标允许随每个测量值而改变）。EKF的基本算法同线性离散Kalman滤波器Figure 1-1所示的一样，下面的合并了前面表格Figure 1-1和Table 2-1和Table 2-2等式的Figure 2-1提供了EKF算法的完整描述。Figure 2-1 合并了高级表格Figure1-1和Table2-1和Table2-2等式的EKF的完整描述EKF的重要特征是正常增大或放大相关测量数据的Kalman增益Kk等式中的Jacobians。例如，如果测量值Zk和测量状态通过h不是一对一的映射，JacobianHk将影响Kalman增益，以致于糨仅仅放大了影响因素的XK-h(XK,0)余数的部分。当然，如果测量值Zk和测量状态通过h都不是一对一的映射关系，你可以很快预测到滤波器是发散的，这种情况是不可观测的。3、Kalman滤波器的应用：估计随机常量在前两节中，我们描述了离散Kalman和扩展Kalman滤波器的基本形式，为了更好的了解滤波器的运算和性能，我们在这里举一个简单的例子。系统模型在这个简单例子中，我们估计一个随机常标量，例如，电压。假设，我们能够获得测量常数，但是测量值是被均方根为0.1的白噪声破坏（例如，从模拟到数字转换是不准确的）。在这个例子中，系统为线性差分方程其中，测量值ZKRl，并定义：在种状态不随时刻而变化，因此A=0。这里没有控制输入，因此u0。噪声测量值为直接状态，于是H=1。（注意，我们在许多地方没有考虑下标，这是因为在简单模型中，各参数均为常数）滤波器等式和参数Time update等式为和Measurement update等式为假设一个很小的系统变化，我们使Q=1e-5。（我们能够确定Q=0，但是为了更好的调整滤波器，假定一个很小但又不为0的值，下面我们会给出证明）。根据经验知道，随机常量的真实值有标准自然概率分布，于是我们定义滤波器常量为0，换句话说，工作前，我们使Xk-1=0。类似的，我们需要选择Pk-1的初始值，如果我们完全确定初始化状态估计X00是正确的，那么P0=0。然而，初始估计X0是不确定的，选择P0=0能起滤波器初始化和使Xk=0。于是证明，二者的选择是临界的，我们能够选择任何P00，最终，滤波器是收敛的，我们以P0=1开始。仿真开始，我们随机选择一个标量Z0.37727。（Z不是“hat”，因为它表示真实值）。然后，我们模拟50个不同的标准偏差为0.1的零自然误差分布的测量值Zk。（记得，我们假设测量值被均方根为0.1的白噪声破坏）。我们只有在同一准确测量情况下的一系列的50个仿真值能在滤波器循环内得到单独的测量值（例如，相同的测量噪声）。于是在不同参数的模拟的比较是很有用的。在第一次仿真时，我们确定了在R=