卡尔曼滤波器介绍.doc

word卡尔曼滤波器介绍摘要在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的调整来源制约于在先前测量值ZKBayes准如此上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=(0.1)2=0.01时的测量协方差。因为这是真实测量协方误差，我们根据平衡响应和估计方差来预测最优特性。在第二次和第三次仿真中，将有更多的证据。Figure3-1描述了第一次仿真的结果。随机常量x0.37727的真实值已经在实线上给定了，噪声值为“标记，滤波器估计仍保持曲线。Figure3-1第一次仿真：R=(0.1)2=0.01。随机常量x0.37727的真实值已经在实线上给定了，噪声值为“标记，滤波器估计仍保持曲线。当考虑到上面选择P0时，我们提到：这选择在P00时候不是临界的，因为滤波器最终会收敛。下面Figure 3-2中，我们画出了相对于重复的PK的值。通过第50个重复，它解决了从1到近似0.0002的最初选择。Figure 3-250个重复后，我们最初协方误差PK从1到0.0002的调整。在“滤波器参数和调整那节中，我们简要讨论了为了获得不同滤波器特性而改变或调整参数Q和R。在下面Figure 3-3和Figure 3-4 中，我们能看到当R各自以100的因子增加或减小时，滤波器是如何改变的。在Figure 3-3中，滤波器告诉测量方差是大100次例如，R=1。于是假定测量值为慢的。Figure 3-3第二次仿真：R=1。滤波器响应测量较慢，导致减小估计方差。在Figure 3-4中，滤波器说明：测量方差小于100次例如：R=0.0001于是假定噪声测量值是非常快。Figure 3-4第三次仿真：R=0.0001。滤波器快速响应测量值，增加估计方差。虽然，常数的估计是相对直接的。它明显的证明Kalman滤波的运转。特别，Figure 3-3中，Kalman滤波是明显的，因为估计出现比噪声测量明显平滑。12 / 12