配套课件-目标跟踪系统中的滤波方法

第1章绪论1.1滤波方法在目标跟踪系统中的地位和作用1.2状态估计和融合方法的研究进展及现状1.3目标跟踪滤波性能评价准则1.4本书内容安排1.1滤波方法在目标跟踪系统中的地位和作用

目标跟踪是人们运用各种观测和计算手段，实现主体对被关注运动客体的状态建模、估计、跟踪的过程。随着航空、航天、航海事业的不断发展以及现代战争信息化、网络化特征的日益凸显，对海底、海面、陆地、空中和太空中目标跟踪技术的精确性和实时性的要求在不断提高。毋庸置疑，该技术在国防安全领域中发挥着重要作用。此外，在民用领域，目标跟踪技术也得到广泛应用，例如空中交通管制、机器人、视频监控以及存在于制造业中的工件定位等。一般意义下的目标跟踪技术通常包括三个部分：数据关联、状态估计及融合、航迹管理[1]，如图1.1所示。在多目标多传感器测量环境下，数据关联的作用很重要，不正确的数据关联直接导致跟踪精度降低甚至丢失目标。数据关联从所关联数据的类型的角度可做如下划分：量测和量测关联、量测和局部估计关联以及航迹和航迹关联。数据关联的难度体现在传感器存在漏检、虚警以及在测量值比较密集情况下的关联。Sittler最早提出数据关联的概念，Singer和BarShalom对数据关联理论的研究和发展做出过重大贡献。最为重要的数据关联方法包括最近邻方法、概率数据关联、联合概率数据关联、多假设方法以及上述方法的改进算法等。航迹管理包括航迹起始、航迹终止、航迹维持等。图1.1目标跟踪系统

1.2状态估计和融合方法的研究进展及现状

1.2.1信息融合技术

信息融合通俗地讲，是将不同来源、不同模式、不同媒质、不同时间、不同地点、不同表示形式的信息进行综合，最后得到对被感知对象的更精确描述。信息融合与数据融合虽属两个不同的概念，但两者很类似，人们所指的信息融合一般是指数据的融合，因此本书对这两个概念不加区分。由于多传感器测量信息具有冗余性和互补性，因此采用有效的融合方法可以得到更加可靠和更加准确的信息。冗余性可以增加系统的健壮性，如果传感器网络中有部分节点遭到破坏，则可以利用其它正常工作的传感器所携带的相同信息进行弥补。互补性可以提高信息的准确性，可以充分利用不同传感器的测量特征，从而获取被观测目标的更准确、更全面的信息。根据美国国防联合指导实验室(theUSJointDirectorsofLaboratories，JDL)的定义，信息融合是组合数据的过程，该过程能够进一步优化估计和预测[3]。也就是说，如果能够有效地利用多源信息，则能够在一定程度上获取更加可靠、更加精确的估计和预测信息。为了说明信息融合的特征，我们给出一个典型的信息融合例子，如图1.2所示。在目标跟踪过程中，使用雷达(Radar)、可见光或红外(EO/IR)和电子支援设备(ESM)对目标进行观测，然后进行融合。可以看出，不同类型的测量设备在探测性能、运动参数估计和识别能力方面是不完全相同的。也就是说，这些传感器的性能具有互补性，所获得的测量信息也相应地具有互补性，使用有效的融合方法进行融合，能够获得各项指标都较好的总体性能。此外，以各种途径获得的数据之间存在冗余，当一部分数据被破坏或者丢失之后，可以通过其它传感器数据对该部分数据进行恢复，因此提高了系统的健壮性。图1.2信息融合举例信息融合在军事上涵盖的范围是比较广泛的，从最底层的数据预处理到战场中的指挥决策，全都包含在内。JDL实验室对信息融合进行了分层，给出了各层的作用范围框架，如图1.3所示。下面对每一层的定义简单加以说明。图1.3信息融合分层模型[3]第0层——次目标数据估计：在对有偏的信号或者像素层数据进行数据关联和特征化的基础上，对信号(即目标观测向量)进行估计和预测。

第1层——目标估计：在有偏的量测和航迹关联的基础上，对实体状态进行估计和预测，包括连续状态估计和离散状态估计。

第2层——态势估计：估计和预测实体之间的关系，包括军力结构和交叉军力关系、通信和感知影响、自然条件等。

第3层——影响估计：战争各方的计划或者对其估计(或预测)的行为对态势影响的估计和预测，包括多个参战方行动计划的相互作用等。第4层——过程优化：自适应地获取和处理数据，以便支持战争目的。该处理涉及到计划和控制，不是估计。该层任务主要是根据各层结果进行判断，指定有利于我方的部署，并将任务分配给各种资源，最终达到目前态势和预测态势有利于我方的目的。

然而上述分层模型只是一个框架，不够具体。赵宗贵在此基础上结合军事实例对该模型涵盖的内容进行了细化，如图1.4所示。

信息融合技术涉及的范围尽管很广泛，但是现有的研究成果主要集中在低层(也就是第0层和第1层)。这主要是因为高层需要具体的实例环境以及相关的军事理论和实践知识，大部分理论研究者缺乏此类专业知识，因此，就公开的文献来看，针对高层信息融合的研究开展得相对较少[5]。如果按照JDL分层理论，本书的研究范畴属于第1层次。

JDL划分的第1层次中一个重要的研究内容就是运动目标跟踪。目标的精确定位和运动估计在现代战场上对决定战场态势具有重要作用。因此，近半个世纪以来，世界各国尤其是发达国家，对信息融合的基础理论研究和工程实践都非常重视。在这期间，出现了众多理论研究成果和专著。图1.4信息融合分层在军事中的实例[4]最具有影响的专著有Shalom等人的文献[1]和文献[6]、Blackman等人的文献[7]以及Hall的文献[8]等。还有许多优秀的相关专著极大地推进了信息融合技术的不断发展[9-10]。我国在上世纪80年代中期开始数字雷达处理技术的研究，也出现了多部信息融合领域的著作[11-18]。尤其是2009至2010年，涌现出多部信息融合和目标跟踪领域的专著[19-25]，这些新著作必将会为信息融合技术在我国科研人员中的普及发挥重要作用。

在信息融合领域做出过巨大贡献的有如下几位：BarShalomY.、SamuelS.Blackman、Farina和李晓榕(LiX.R.)等人，他们长期活跃在信息融合领域，不断地推动并引导信息融合技术的发展。国内也展开了一系列研究，主要研究机构有上海交通大学、国防科技大学、杭州电子科技大学、海军航空工程学院、西安交通大学、西北工业大学和西安电子科技大学等单位。信息融合领域的重要刊物包括《IEEETransactionsonAutomaticControl》、《IEEETransactionsonAerospaceandElectronicSystems》、《IEEETransactionsonSignalProcessing》和Elsevier出版集团的《Automatica》等。信息融合国际会议(InternationalConferenceonInformationFusion)是该领域的学术盛会。1.2.2目标跟踪技术

使用状态空间估计方法对目标进行跟踪，首先要对目标的运动模型和传感器的测量模型建模。测量模型根据传感器的性质容易确定，然而，目标的运动模型事先难以确定。为了简化问题，人们对常见的理想状态下的目标运动进行建模[26]。简单的目标运动模型包括匀速运动、匀加速运动、零均值一阶马尔可夫模型、均值自适应加速模型、已知转弯角速度的常速度转弯运动和未知转弯角速度的常速度转弯运动等。在基于模型的目标跟踪过程中，对目标运动情况的建模十分重要，模型的好坏直接决定着跟踪的效果。然而在实际应用中，目标的运动模型往往是未知的，而且在现代战场上，目标在运动过程中会做各种各样的运动模式变换，以摆脱对方的追踪和打击。如果仅仅使用已知的模型进行状态估计，效果就会很差，在这种情况下，人们对机动目标的跟踪产生了浓厚的兴趣。所谓机动目标，是指目标的运动模式不断变换，跟踪系统无法准确知道当前目标的运动模型。李晓榕及其合作者十分关注机动目标跟踪方法的研究和该领域的进展，并且分主题做了相关的综述。1.2.3状态估计技术

在目标跟踪过程中，传感器测量信息不仅包含所需信号，同时也包含随机观测噪声和干扰信号。估计是指通过对一系列带有观测噪声和干扰信号的实际观测数据进行处理，从中得到所需要的各种参量的估计值的过程。通常估计问题可以分为两类：一类是系统的结构参数部分或全部未知，有待确定；二是系统中的部分或全部状态变量不能直接测得。这两类问题通常称为参数估计和状态估计。两者的区别在于：参数估计是不随时间变化或只随时间缓慢变化的随机变量；状态估计是随时间变化的随机过程。对于状态估计的国外专著如文献[41-43]，国内也涌现了一系列著作可供参考[44-48]。根据状态向量和观测向量在时间上存在的不同对应关系，状态估计问题可以分为预测、滤波和平滑。预测是滤波的基础，滤波是平滑的基础，因此下面仅针对主要的滤波算法进行讨论。假设xk|j表示根据j时刻和j时刻以前的测量值对k时刻的状态xk作出的估计，则按照k和j的不同对应关系，状态估计可作如下划分：

(1)当k=j时的估计过程称为滤波，即依据过去直到现在的观测值来估计现在的状态；

(2)当k>j时的估计过程称为预测，即依据过去直到现在的观测值来预测未来的状态；

(3)当k<j时的估计过程称为平滑，即依据过去直到现在的观测值来估计过去的历史状态。^1.2.4估计融合技术

如果同时有多个传感器对目标进行测量，则需要用到融合技术。根据观测节点是否具有状态估计能力，可以将其分为集中式、分布式和混合式三种结构[80，81]。集中式结构如图1.5(a)所示，N个传感器对目标进行观测之后获得量测值，然后将量测值送入融合中心，由中心处理器对这些量测信息进行统一处理。分布式结构如图1.5(b)所示，每个传感器带有可以进行局部估计的处理器，当传感器观测到目标信息之后，局部传感器直接进行处理，将得到的局部估计结果送入融合中心后，采用相应算法进行统一处理。前一种结构需要传输的信息量大，对中心节点的处理能力要求高，然而由于原始数据能够得到统一处理，因此，其处理结果精度高。后一种结构网络传输负载小，对中心节点的处理能力要求低，但是其处理结果精度也低。混合结构性能则介于上述两种结构之间。集中式结构下的融合算法较为简单，常见的有扩维融合和序贯融合两种方法。而分布式结构下，人们已经提出了多种融合算法。

凸组合航迹融合算法(ConvexCombinationtracktotrackFusion，CCF)[80]不考虑各传感器局部估计误差之间的相关性。当局部航迹都是传感器航迹并且不存在过程噪声，并且各传感器在初始时刻的估计误差也不相关时，简单凸组合算法是最优的。该算法只对各个局部传感器的估计及其协方差矩阵进行处理，需要传送到中心节点的信息量少，与其它航迹融合算法相比运算量小。图1.5融合结构最优航迹融合算法(OptimaltracktotrackFusion，OF)[82]考虑了局部估计误差之间的相关性，对航迹的过程噪声有较好的抑制性。该算法性能和集中式扩维融合算法性能相同，所以常常被作为衡量分布式航迹融合算法性能的基准，但是该算法融合过程中需要的数据相对较多。在过程噪声较小且数据采样率较高的条件下，可以使用一种称之为小航迹(TrackletFusion，TF)[83-85]的航迹融合算法。简单凸组合融合算法在忽略噪声相关的条件下利用状态估计的误差协方差对局部估计进行加权求和。BarShalomCampo算法尽管考虑了噪声相关情况，但解决的并不彻底[86]。类似的还有最大后验概率法[87]、协方差交叉法[88-93]、联邦滤波器法[94-96]等。其中联邦滤波器法较为突出，通过放大各个噪声分量来近似消除噪声之间的相关，从而获得了一种近似方法。该方法被美国宇航局作为标准算法，应用在航空航天领域的估计融合过程中。此外，李晓榕创建的最优线性估计融合方法将集中式、分布式和混合式结构统一到一个算法框架下，具有良好的应用参考价值。多传感器的融合结果理论上要优于单个传感器估计，估计精度提高的幅度不但与融合算法有关，而且与传感器量测噪声之间的相关性有关。文献[97]讨论了在不同测量噪声相关性条件下的部分航迹融合算法的误差性能。

1.3目标跟踪滤波性能评价准则

在目标跟踪滤波过程中，往往需要对滤波的准确程度进行评估，以判断所采用算法的优劣。由于单目标系统和多目标系统的性能评估准则存在较大差异，因此，本节分别给出常用的滤波性能评价准则。

1.3.1单目标跟踪滤波性能评价准则

1.位置分量的误差适配百分比

位置分量的误差适配百分比(PercentageFitError，PFE)是目标位置估计和真实位置之差的范数与真实位置范数之比。当目标真实位置和估计位置重合时,该值为0;当目标的估计位置偏离真实位置时，该值增大。在对比各种算法性能时,PFE值最小的算法精度是最好的。误差适配百分比定义如下:

在实际应用中，状态向量中的真实分量是未知的，此时可以计算量测轨迹和预测轨迹或者估计轨迹之间的范数来替代真实分量。(1-1)

2.位置分量的均方根误差

位置均方根误差(RootMeanSquareError，RMSE)是指目标真实的和估计的x、y和z方向位置的均方根误差。该值是一个标量，如果目标真实位置和估计位置重合，则其值为0；如果估计位置偏离真实位置，则其值增大。在比较各种滤波算法性能时，该值越小，表明算法滤波精度越高。位置均方根误差定义如下：(1-2)

3.位置分量的和方根误差

位置分量的和方根误差(RootSumSquareError，RSSE)是指目标真实的和估计的x、y和z方向位置的误差平方之和的平方根。该值是离散独立变量，可以用图来表示不同时刻的误差结果。这些结果是一个误差序列，如果目标估计位置和真实位置相同，则相应时刻的和方根误差值为0；如果估计位置偏离真实位置，则该值增大。在比较各种滤波算法性能时，该值达到最低值的时间越短，说明算法收敛得越快，性能越好。位置分量的和方根误差定义如下：(1-3)采用类似的式子可以计算目标速度和目标加速度的和方根误差。

4.带有理论边界的状态误差

具有理论边界的状态误差计算式为，通过该准则(表达式)可将状态误差(可以是位置、速度、加速度、Jerk甚至Surge分量)和其理论边界画在曲线图上。理论边界等于协方差对角线元素值平方根的2倍。如果整个计算到的误差曲线的95%都处于该边界内，就意味着状态估计的精度良好。若x方向位置的状态误差计算为，则对应的理论边界(上下界)为。

5.带有理论边界的新息序列

带有理论边界的新息序列(或称量测残差)计算式为，该式的理论边界为。将新息序列及其理论边界一起显示到曲线图中，其中S是从卡尔曼滤波器输出的新息协方差矩阵。理论边界的值等于新息协方差矩阵的对角线元素值的平方根的2倍。如果误差都位于边界之内，就意味着跟踪器性能良好。

6.归一化估计误差平方

归一化估计误差平方计算为

(1-4)

该表达式利用状态向量的协方差矩阵对状态误差平方做归一化。一般情况下，其均值等于状态向量的维数。如果该值位于理论边界之内，则意味着滤波器表现良好。该值的理论边界下界(LowerBound)算式为；该值的理论边界上界(UpperBound)算式为。其中，自由度p为NxNMCS;Nx是状态向量的元素个数，NMCS是蒙特卡罗(MonteCarlo)仿真次数，χ是卡方分布符号。

7.具有理论边界的归一化新息平方

归一化新息平方计算式为vS－1vT，该值利用新息的协方差对新息平方进行归一化。一般来说，其均值等于量测向量的维数。如果该值位于理论边界之内，则意味着滤波器表现良好。该值的理论边界下界算式为；该值的理论边界上界算式为。其中，自由度p为NzNMCS；Nz是量测向量的元素个数。1.3.2多目标跟踪滤波性能评价准则

1.圆丢失概率

圆丢失概率(CircularPositionErrorProbability，CPEP)[98]用于评价目标跟踪丢失率的情况。假设k时刻实际和估计的多目标状态集合为Xk和Xk，则圆丢失率定义为^(1-5)

2.Hausdorff距离

Hausdorff距离[99]是数学中常用的度量两个集合之间距离的方法。其优点在于能够较好地反映估计结果的局部性，但是它对目标个数的估计误差不敏感。对于目标的真实状态集X和估计状态集X，Hausdorff距离定义为^(1-6)

3.Wasserstein距离

Hoffman和Mahler建议采用统计学中的Wasserstein距离[99]来定量地度量集合X和X之间的距离。对于目标真实状态集X和估计状态集X，Wasserstein距离的定义为^^(1-7)

4.最优子模型分配距离

最优子模型分配距离(OptimalSubpatternAssignment，OSPA)[100]是一种可用来衡量集合之间差异程度的误差距离。该距离建立在Wasserstein距离的基础上，对Wasserstein距离度量集合之间的差异有所改善。

集合X={x1，x2，…，xm}和集合X={x1，x2，…，xn}之间的距离定义为

如果n≥m，则^^^^(1-8)如果n≤m，则(1-9)此外，当p→∞时，有(1-10)其他1.3.3时间复杂度评价准则

算法的时间复杂度是指执行算法所需要的计算工作量。为了比较同一个问题的多种算法的时间复杂度高低，可以采用算法在执行过程中所需要的基本运算的次数来衡量[101]。考虑到算法执行中的基本运算次数与实际求解的问题的规模有关，因此，算法所执行的基本运算次数是问题规模的函数，即A(n)=f(n)，其中n为问题的规模。

即便在同一问题规模的情况下，算法的时间复杂度也不是一成不变的。若算法执行所需要的基本运算次数取决于某一特定输入，则需要采用其它的方法进行度量。此时，常用的度量方法有平均性态法和最坏情况法。平均性态法是指用各种输入情况下所需要的基本运算次数的加权平均作为该算法时间复杂度的评价标准。算法的平均性态定义为(1-11)其中：x表示算法的输入；p(x)表示输入为x的概率；t(x)表示算法在输入为x时所需要的基本运算次数；Dn表示当问题规模为n时，算法所有可能的输入集合。最坏情况法采用算法执行需要的最大基本运算次数来表示算法的时间复杂度。最坏情况复杂度定义为

，其中参数含义同式(1-11)。在实际应用中，考虑到时间复杂度计算中基本运算次数分析的困难，通常把计算机仿真过程中算法运行的平均时间作为度量标准。此时，计算公式为，其中：Tistart表示算法进行第i次仿真时的开始时间，Tiend表示算法进行第i次仿真时的结束时间，M表示算法进行蒙特卡罗仿真总次数。

1.4本书内容安排

第1章介绍本书的研究背景和意义，论述了信息融合中的目标跟踪技术的研究进展及现状，对目标跟踪系统中的滤波算法做了简要回顾。第2章讨论卡尔曼滤波算法和基于卡尔曼滤波的非线性滤波算法。第3章讨论粒子滤波算法。第4章对具有等式约束的滤波算法进行研究，在对具有约束的滤波算法进行回顾的基础上，提出了两种与约束有关的算法，即：等式状态约束下的粒子滤波算法和基于UT变换的收缩迭代非线性约束滤波算法。第5章研究了无序量测条件下的滤波问题，提出了一种基于UT变换的单步滞后无序量测算法。第6章研究了变分贝叶斯自适应卡尔曼滤波算法，提出了一种新的双重迭代VB_AKF算法;此外，在上述算法的基础上，结合扩维集中式融合和序贯集中式融合方法，提出了两种新的多传感器数据融合算法，即：基于VB_AKF的扩维集中式融合算法和基于VB_AKF的序贯集中式融合算法。第7章讨论丢包条件下的滤波方法，对近年来提出的具有丢包条件下的滤波算法进行讨论，提出了一类非线性系统中具有丢包情况的滤波算法。第8章针对估计融合方法进行研究，对多种RTS平滑算法进行讨论，在此基础上，提出了一种基于分段RTS的凸组合估计融合算法。第9章研究了几种非线性滤波算法在目标跟踪中的应用。第10章是相关的数学预备知识。第2章卡尔曼滤波和非线性系统滤波方法2.2卡尔曼滤波算法2.3扩展卡尔曼滤波算法2.4不敏卡尔曼滤波算法2.5积分卡尔曼滤波算法2.6容积卡尔曼滤波算法2.7傅立叶厄米特卡尔曼滤波算法2.8中心差分卡尔曼滤波算法2.9小结

2.2卡尔曼滤波算法

2.2.1状态空间模型

对于离散状态空间模型或者贝叶斯非线性滤波模型，一般都包含有下列条件概率分布函数(2-1)该模型假定符合马尔科夫理论，因此具有以下两个性质：

(1)状态向量的马尔可夫性质。状态值序列{xk:k=0，1，2，…}构成了一个马尔科夫序列，如果状态值是离散的，那么序列是一个马尔科夫链。符合该性质，意味着k时刻的状态估计xk只与k－1时刻的状态值有关，而与k－1时刻之前(k－2，k－3，…，1)的状态值无关，即

(2-2)同样，k－1时刻的状态估计值也只与k时刻的状态值有关，而与k时刻之后(k+2，k+3，…，T)的状态都无关，即(2-3)

(2)量测向量的条件独立性。当前时刻的量测值yk与当前时刻的状态值xk有关，而与k时刻之前(k－1，k－2，…，1)的量测值和状态值无关，即(2-4)2.2.2最优滤波方程

最优滤波就是在给定历史量测值的情况下，计算出每一时刻状态向量xk的边缘后验分布值p(xk|y1:k)。

定理2.1

计算预测分布p(xk|y1:k－1)及滤波分布p(xk|y1:k)的递归方程由下述贝叶斯滤波方程给出。

(1)初始化。递归是从先验分布p(x0)开始的。

(2)预测。状态向量xk在第k步的预测分布可以通过ChapmanKolmogoro方程获取(2-5)

(3)更新。当获得第k步的量测值yk后，后验分布可由贝叶斯规则求取(2-6)其中归一化常数Zk为(2-7)证明：在给定y1:k－1的情况下，xk和xk－1的联合分布值计算如下(2-8)考虑到系统的马尔可夫特性，在上式推导过程中丢弃了历史量测值y1:k－1。给定y1:k－1时，xk的边缘分布可以通过对式(2-8)在xk－1处求积分来获取，也就是使用ChapmanKolmogoro方程(2-9)如果xk－1是离散的，那么式(2-9)中的积分变为在xk－1处的累加和。在给定y1:k的条件下，xk的分布可以通过贝叶斯规则进行计算(2-10)其中，归一化常数项如式(2-7)所示。式(2-10)在推导过程中丢掉了历史量测数据y1:k－1，这是因为，在给定xk条件下yk独立于历史量测信息y1:k－1。2.2.3卡尔曼滤波

卡尔曼滤波是一种离散状态空间模型下的线性最小方差估计。系统的动态方程和量测方程都是线性高斯的，通常表示为(2-11)(2-12)其中，xk∈Rn表示状态向量，yk∈Rm表示量测向量，qk－1~N(0，Qk－1)和rk－1~N(0，Rk)分别是服从高斯分布的过程噪声序列和量测噪声序列，矩阵Fk－1是动态模型的转移矩阵，Hk是量测模型矩阵。假设先验分布也符合高斯分布，即x0~N(x0|0，P0)，则可将上述模型以概率形式表示为(2-13)(2-14)线性滤波模型(2-11)、(2-12)的最优滤波方程可以通过下列近似方法获取，其结果都符合高斯分布，即(2-15)(2-16)(2-17)上述分布中的参数可以根据下面的卡尔曼滤波器的预测和更新步骤计算。预测步骤为(2-18)(2-19)更新步骤为(2-20)(2-21)(2-22)(2-23)(2-24)初始状态向量值先验分布为x0~N(x0|0，P0)，其中给出了状态向量的初始均值和方差。卡尔曼滤波方程可以通过如下步骤进行推导:

(1)给定y1:k－1时，xk和xk－1的联合分布为(2-25)其中(2-26)(2-27)xk的边缘分布为(2-28)(2-29)(2-30)其中

(2)yk和xk的联合分布如下

(2-31)其中(2-32)(2-33)(3)xk的条件分布为(2-34)其中(2-35)(2-36)

2.3扩展卡尔曼滤波算法

2.3.1泰勒级数展开

考虑将高斯随机变量x转换成另外一个随机变量y(2-37)(2-38)随机变量y的概率密度表示为(2-39)其中，|J(y)|是逆变换g－1(y)的雅可比矩阵的行列式。然而，由于g往往是非高斯的，该分布难于直接获得。对y的分布做一阶泰勒级数近似的方法如下。若令x=m+δx,其中δx~N(0，P)，那么就可得函数g的泰勒级数展开为(2-40)其中：Gx(m)是一个雅可比矩阵，其各个元素定义为(2-41)G(i)xx(m)是一个Hessian矩阵(2-42)ei=[0…010…0]T是一个单位矩阵，只有在位置i的值为1,其余位置的元素值都为0。也就是说，在坐标轴i的方向上是单位向量。也可以取泰勒级数展开式的前两项对非线性函数进行近似，即(2-43)计算该式在x=m时的期望值为(2-44)于是协方差近似计算为(2-45)向量x和y之间的互协方差也很重要，它可以通过下面的增广变换来获得(2-46)(2-47)其均值和协方差为(2-48)对于扩展卡尔曼滤波方程的推导，需要一个一般化的变换形式(2-49)(2-50)其中，q和x相互独立，x和y联合分布的定义如前所述。

(1)含加性噪声的线性化近似。对x和随机变换y=g(x)+q的联合分布的线性化为(2-51)其中(2-52)(2-53)(2-54)

Gx(m)是函数g关于状态向量x的雅可比矩阵，当x=m时，矩阵元素根据下式计算(2-55)

(2)含非加性噪声的线性化近似。如果滤波模型中的过程噪声是非加性的，即有(2-56)其中x和q是不相关的随机变量。均值和方差可以通过将方程中的增广向量(x，q)替换为向量x来获取。正如方程(2-56)所示的，其增广变换的均值和方差给出如下(2-57)(2-58)上述近似值的计算可以通过下面的规则求取。

对于x和y=g(x，q)的联合分布，其线性化近似为(2-59)其中(2-60)(2-61)(2-62)Gx(m)是一个函数g关于变量x的雅可比矩阵，当x=m，q=0时，该矩阵元素为(2-63)相应地，Gq(m)也是一个函数g关于噪声变量q的雅可比矩阵，该矩阵元素为(2-64)

(3)含有加性噪声的二阶线性化近似。二阶线性化近似形式为(2-65)其中(2-66)(2-67)(2-68)其中，Gx(m)是形如式(2-55)的雅可比矩阵，而G(i)xx(m)是当x=m时的Hessian矩阵，即(2-69)ei是一个单位向量，只在位置i取值为1，其余位置取值均为0。2.3.2扩展卡尔曼滤波

1.带有加性噪声的一阶扩展卡尔曼滤波过程

假定过程噪声和量测噪声都是加性噪声，扩展卡尔曼滤波系统模型如下

xk=f(xk－1)+qk－1

(2-70)

yk=h(xk)+rk

(2-71)

其中：x∈Rn是状态向量，y∈Rm是量测向量，qk－1~N(0，

Qk－1)和rk~N(0，Rk)分别是高斯过程噪声及高斯量测噪声。f是动态模型函数，h是量测模型函数。需要指出的是，函数f，h有时也将离散的时间变量k作为参数。

预测步骤为(2-72)(2-73)更新步骤为(2-74)(2-75)(2-76)(2-77)(2-78)

2.带有非加性噪声的一阶扩展卡尔曼滤波

具有非加性噪声的扩展卡尔曼滤波系统模型为(2-79)(2-80)其中，qk－1~N(0，Qk－1)和rk~N(0，Rk)分别是过程噪声和量测噪声。该模型在形式上更具有一般性。预测步骤为(2-81)(2-82)更新步骤为(2-83)

(2-84)(2-85)(2-86)(2-87)其中，Fx(m)，Fq(m)，Hx(m)和Hr(m)都是雅可比矩阵，矩阵元素分别为(2-88)(2-89)

3.带有加性噪声的二阶扩展卡尔曼滤波

类似地，利用泰勒级数二阶展开近似方法，可以获得二阶扩展卡尔曼滤波。下面给出算法步骤。

预测步骤为(2-90)(2-91)

更新步骤为(2-92)(2-93)(2-94)(2-95)(2-96)其中，矩阵Fx(m)和Hx(m)已经在式(2-88)及式(2-89)中给出。矩阵F(i)xx(m)和H(i)xx(m)是相应的Hessian矩阵，矩阵元素为(2-97)(2-98)非加性的情况可以通过同样的过程获得，这里不再赘述。与其它非线性滤波相比，EKF的优点就是在实现方面相对比较简单，时间复杂度低。构建非线性化系统的近似线性化是解决非线性系统滤波问题的常规做法，且易于理解和接受。其缺点是EKF要求非线性系统的量测模型函数和动态模型函数是可微分的。EKF虽然被广泛用于解决非线性系统的状态估计问题，但其滤波效果在很多复杂系统中并不能令人满意。模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波发散。另外，在许多实际应用中，模型的线性过程比较繁琐，而且也不容易得到。这就迫使人们不断寻求新的非线性滤波算法。

2.4不敏卡尔曼滤波算法

2.4.1不敏变换

不敏卡尔曼滤波是在不敏变换的基础上发展起来的。不敏变换的基本思想是由Julier等首先提出的，是用于计算经过非线性变换的随机变量统计的一种新方法。该方法不需要对非线性状态和量测模型进行线性化，而是对状态向量的概率密度函数进行近似。近似化后的概率密度函数仍然是高斯的，但它表现为一系列选取好的西格玛采样点。

假设x为一个nx维随机向量，

为一非线性函数，并且y=g(x)。x的均值和协方差分别为和P。计算UT变换的步骤可简单叙述如下。

Step1：首先计算2nx+1个西格玛采样点χi和相应权值ωi

(2-100)(2-101)其中，[]i表示矩阵的第i列，nx为状态向量的维数，λ是一个尺度参数，定义如下

λ=α2(nx+κ)－nx

(2-101)

式中参数α，κ决定西格玛点以均值为原点的分散程度。

Step2：每个西格玛采样点通过非线性函数传播，得到

yi=g(χi)，i=0，…，2nx

(2-102)

Step3：y的估计值和协方差估计如下(2-104)(2-103)2.4.2不敏卡尔曼滤波

假设非线性系统中的状态向量的初始均值和协方差分别为。

UKF算法中的初始状态向量则是由原始状态向量、过程噪声以及量测噪声三者组成的扩维向量，其初始值和协方差定义如下(2-106)(2-105)其中Q和R分别是过程噪声和量测噪声的协方差。UKF算法过程如下。

Step1：用式(2-100)计算西格玛点的权值，用下式计算西格玛点(此处把所有西格玛点集看做一个向量)(2-107)(2-108)

Step2：西格玛点的一步预测Step3：状态预测(2-109)(2-110)Step3：计算量测预测采样点(2-111)

Step4：估计量测预测值(2-112)Step5：估计新息协方差矩阵(2-113)Step6：估计互协方差矩阵(2-114)Step7：计算增益矩阵(2-115)

Step8：状态更新(2-116)Step9：协方差更新(2-117)

2.5积分卡尔曼滤波算法

2.5.1高斯厄米特积分准则

考虑一个在区间(a，b)上的加权可积函数g(x)，其积分表示为(2-118)其中W(x)是加权函数。采用m点数值积分对式(2-118)进行近似计算(2-119)式中ξi是积分点，ωi是相应的权值。假设一个随机变量x具有高斯密度N(x;0，1)，那么函数g(x)的期望可以近似为(2-120)其中积分点ξi和相应权值ωi的计算运用了正交多项式和三对角矩阵之间的关系。假定J是一个对称三对角矩阵，具有0对角元素，J矩阵的其它元素值通过下式计算(2-121)此处m是积分点个数，通常取3或者5。积分点，其中εi是J的第i个特征值；相应的权值ωi=(νi)21，其中(νi)1是J的第i个特征向量的第一个元素。对于服从高斯分布的随机向量x，其高斯密度函数为P(x)=N(x;0，)，为nx×nx单位矩阵。由于向量x中各个元素互不相关，式(2-120)可以扩展到多维积分公式(2-122)其中，2.5.2积分卡尔曼滤波

通过线性回归点(即高斯厄米特积分点)，可以确定高斯密度。假设状态的后验概率可以用高斯分布来近似，可利用高斯厄米特积分点线性回归系统的状态方程和观测方程，得到系统的先验和后验概率估计。积分卡尔曼滤波算法步骤总结如下。

预测步骤为(2-123)(2-124)(2-125)(2-126)更新步骤为(2-127)(2-128)(2-129)(2-130)(2-131)(2-132)(2-133)(2-134)

2.6容积卡尔曼滤波算法

2.6.1球面径向规则

高斯分布下的非线性滤波问题可以归结为一个积分计算问题，其中被积函数都是非线性函数和高斯密度乘积的形式，定义如下积分形式(2-135)其中被积函数定义在笛卡尔坐标系中。为了对该积分形式进行数值化，采取如下策略，将式(2-135)转换成更常用的球面径向形式，并采用新的球面径向规则进行数值积分。

1.球面径向变换

在球面径向变换中，把笛卡尔向量x∈Rn转换成径向标量r和方向向量y的乘积，即：令x=ry，且yTy=1，则可得xTx=r2，r∈[0，∞]。式(2-135)在球面径向坐标系中可以写为(2-136)式中:Un是球形的表面，定义为Un={y∈Rn|yTy=1};σ(·)是Un中的表面量测或面积元素。在单位权重函数w(y)=1时，令(2-137)则式(2-137)变为径向积分(2-138)球面径向积分利用后续将要介绍的球面容积规则和高斯积分规则分别进行数值计算，为了便于介绍上述规则，首先引入如下符号和定义。

满足如下两个条件的容积规则称为完全对称的：①若x∈D则有y∈D，其中y由x通过置换或者符号变换得到；②若x∈D，y∈D，则w(x)=w(y)，即集合中关于圆点对称的元素的权值相等。

例如，实数集R是一个一维全对称集，若x∈R则必有－x∈R并且有w(x)=w(－x)。对于n维对称空间中的点u=(u1，u2，…，ur，0，…，0)∈Rn，若ui≥ui+1>0，i=1，2，…，r－1，则点u为一个生成元。把全对称集中的生成元u=(u1，u2，…，ur，0，…，0)简记为[u1，u2，…，ur]。若生成元中的各分量互不相同，则生成元的全对称集有个元素。例如，[1]∈R2表示二维空间的点集：，该点集的生成元为。我们用[u1，u2，…，ur]i表示全对称集[u1，u2，…，ur]中的第i个点。

2.球面容积规则

根据不变量理论，假定一个三阶球面容积规则的最简单形式为(2-139)由[u]i经过置换和符号变换得到的点集是不变量。对于单项式，若为奇数，则式(2-139)式可以准确积分。对于所有三阶以下的单项式，只需要考虑单项式处于2的情况。因此，对于单项式f(y)=1和f(y)=y21，需要找出未知参数u和w，使其满足全对称容积规则

(2-140)(2-141)当f(y)=1时，当f(y)=y21时，

3.径向规则

接下来介绍如何利用高斯求积分法求解径向积分。高斯积分法是已知计算一维积分最为有效的数值积分方法。m点的高斯积分可以很好地逼近2m－1阶多项式，即(2-142)其中，w(x)是[a，b]上已知的非负权函数，未知积分点{xi}和相应的权值{wi}具有唯一的解。比较式(2-136)和式(2-142)，我们得到区间[0，∞]的权函数为w(x)=xn－1exp(－x2)。为了把这种积分转变成熟悉的形式，做变量代换，令t=x2，则有(2-143)对于一阶高斯规则，有f(t)=1，t;f(x)=1，x2。但是它对于奇数阶多项式f(x)=x，x3并不适用。但若把径向规则与球面规则相结合求解式(2-135)，则所有的奇数阶多项式的积分为零。这是因为对于球面规则(如式(2-136))，所有奇数阶多项式在对称空间的积分为零，因此一阶通用高斯-拉格朗日规则数值积分需要一个点和相应的权值。因此有~(2-144)

4.球面径向规则

为了将球面规则和径向规则结合起来，并将球面径向规则用于高斯加权积分，下面需要介绍两个有用的定理。

定理2.2

利用mr点高斯求积分规则对径向积分进行数值化计算(2-145)利用ms点球面规则对球面积分进行数值化计算(2-146)于是得到一个ms×mr点球面径向积分规则(2-147)

证明：求容积法规则可以设计成适用单项式的子空间，考虑如下被积函数(2-148)其中{di}是正整数。因此，有如下积分(2-149)此时，假定上式中的被积函数是一个d阶单项式，即，作球面径向变换得(2-150)把上式分解成为一个径向积分和一个球面积分(2-151)运用数值规则，得到(2-152)证毕。对于被积函数是阶数不超过d的任何单项式，上述定理均适用。定理2.3

令函数w1(x)和w2(x)分别为w1(x)=exp(－xTx)和w2(x)=N(x;μ，Σ)，对于满足的平方根矩阵，可以得到(2-153)证明：考虑式(2-153)左侧。因为Σ是一个正定矩阵，故做矩阵分解，令，可以得到(2-154)证毕。对三阶的球面径向规则，mr=1和ms=2n，共需要2n个容积点。根据定理2.2和定理2.3，可用三阶球面径向规则计算标准的高斯加权积分，如下式(2-155)其中(2-156)使用求容积点{εi，wi}数值化计算积分，就得到了CKF算法。需要指出的是，上面的容积点定义在笛卡尔坐标系。2.6.2容积卡尔曼滤波

应用统计线性回归方法，可得容积卡尔曼滤波算法的状态更新步骤和量测更新步骤如下[12，13]。

预测步骤为：

Step1：采用Cholesky或者奇异值方法分解协方差(2-157)Step2：计算容积点(i=1，2，…，m)(2-158)其中m=2nx。

Step3：计算预测状态容积点(2-159)

Step4：估计预测状态(2-160)Step5：估计预测协方差(2-161)更新步骤为：Step1：预测协方差矩阵进行分解(2-162)Step2：计算容积点(i=1，2，…，m)(2-163)Step3：计算预测量测容积点(2-164)

Step4：估计预测量测(2-165)Step5：估计新息协方差矩阵(2-166)Step6：估计互协方差矩阵(2-167)Step7：计算卡尔曼滤波增益(2-168)Step8：估计状态更新(2-169)

tep9：估计状态协方差(2-170)2.7傅立叶厄米特卡尔曼滤波算法

2.7.1傅立叶厄米特级数展开

本节介绍傅立叶厄米特级数如何应用于高斯随机变量的非线性变换，以及如何看待它作为统计线性化的一般形式。

对于随机变量x~N(m，P)，考虑统计线性化状态模型

y=g(x)

(2-171)

假设其近似化形式为

g(x)≈b+Aδx

(2-172)

通过最小化表达式E[‖g(x)－b－Aδx‖2]获得最优矩阵A和向量b，其中δx=x－m。我们试图将其一般化，使得它能进行线性化近似。使用一个p阶多项式进行近似

(2-173)通过扩展上述表达式并且令其导数为零，就可以确定最优多项式的系数。该方法对于高阶多项式的求解就会变得复杂。所幸的是，依据希尔伯特空间理论可以让多项式的求解变得简单。通过给标量函数g和f定义内积(2-174)其中x~N(m，P)。通过定义下列范数确定一个希尔伯特空间函数(2-175)希尔伯特空间理论表明，存在一个与希尔伯特空间对应的正交多项式基。结果证明这些多项式基函数是尺度多元厄米特多项式，定义为(2-176)其中L是一个满足P=LLT的矩阵，且有(2-177)这样，就可以将一个满足条件〈gi，gi〉<∞的任意向量函数g(x)扩展为一个傅立叶厄米特级数，形式为(2-178)需要指出的是，因为H[0，…，0](x;m，P)=1，级数中的零阶项恰好就是函数的期望E[g(x)]。利用基函数的正交性可以得到外积期望之和(2-179)在不考虑零阶级数的情况下，我们可以得到g(x)协方差的表达式(2-180)由希尔伯特空间理论，可知对函数g(x)关于||·||2的最优p阶多项式近似，也即对式(2-173)的多项式展开，可以通过p阶厄米特多项式的正交投影得到。那么通过对级数(式(2-178))进行p阶级数截断就可以得到最优p阶多项式近似。现在考虑使用数值计算的方法来求取级数的系数，然后选取其中的零阶项作为均值，并通过上述对级数截断的方法计算协方差的近似值。然而，用这种方法计算协方差并不是一个好的选择，因为使用同样的数值方法我们可以直接获取协方差的值。实际上，依据下述结果，可以采用一种替代的方法获取傅立叶厄米特级数系数，即

(2-181)上述公式是统计线性化方法导数的一般形式。由此，傅立叶厄米特级数就可以用下面的形式表达(2-182)且其协方差为(2-183)结果证明我们甚至不需要计算系统导数的期望值，因为可以使用以下方法求解。假定对于任意的m，P，使用下列形式来计算期望值

(2-184)接下来，有(2-185)为了获得扩维函数g(x)=(x，g(x))的联合均值和协方差，同样使用上述结果，并计算~(2-186)其中。从而得到下面的具有加性变换的傅立叶厄米特级数近似规则。对于x~N(m，P)，q~N(0，Q)，随机变量x和非线性变换y=g(x)+q的联合分布的傅立叶厄米特级数为(2-187)其中，，(2-188)且有如下定义(2-189)(2-190)矩阵G定义如下^(2-191)当级数在截断时，上述近似形式的均值及互协方差总是很精确的，协方差也可以精确到p阶多项式。一阶近似等同于统计线性化近似方法。用与上述类似的方法也可以获得非加性变换的傅立叶厄米特近似过程。2.7.2傅立叶厄米特卡尔曼滤波

通过傅立叶厄米特级数展开，我们得到了傅立叶厄米特卡尔曼滤波算法[14]。该算法是统计线性化滤波的一般化形式。为了实现这种滤波算法，需要下述表达式的近似形式(2-192)(2-193)下面的式子同样需要近似形式(2-194)(2-195)带有加性噪声的傅立叶厄米特卡尔曼滤波的状态预测步骤和更新步骤如下:

预测步骤为(2-197)更新步骤为(2-198)

(2-199)(2-200)(2-201)(2-202)其中矩阵H

的元素定义为^(2-203)

2.8中心差分卡尔曼滤波算法

2.8.1Stirling插值公式

Stirling插值可以解决任意函数的多项近似问题。首先考虑单变量函数情况，然后再将其扩展到多维情况。如果函数f是解析函数，将其围绕着x=x做泰勒级数展开－(2-204)一种最常用的近似方法就是对上述级数进行有限项的截断。保留的项数越多，得到的近似也就越精确。类似地，可以通过插值公式来获取多项式的近似形式。这些插值公式通常不要求对函数求导，而是对有限个函数进行计算。因此，该方法可能会更容易获取非线性函数的线性近似形式。下面考虑一种称为Stirling的差值公式。首先定义两个操作符δ，μ如下(2-205)其中h是选定的区间长度。在x=x处，采用Stirling插值公式进行多项式逼近，得到－(2-206)(2-207)通常－1<p<1。考虑到我们要着重介绍一阶和二阶多项式逼近，因此将式(2-207)简化为(2-208)其中(2-209)(2-210)式(2-208)可以解释为把求导项替换为中心差分项的泰勒近似。为了评估多项式的逼近程度，用展开的泰勒级数代替和，得到(2-211)可以看出式(2-211)右边的前3项和区间长度h无关。

现在考虑多维的情况。设x为一个向量，x∈Rn，y=f(x)，有多种不同的方法可以将插值公式扩展成为多维的，但是在这之前，我们首先想到多维泰勒级数展开形式。当x=x时，函数f的泰勒级数展开为－(2-212)其中，算子DiΔxf表示为(2-213)上述算子同样可以表示为

(2-214)那么，采用多维插值公式对函数进行二阶多项式截断可得(2-215)其中，差分算子DΔxf和D2Δxf可分别表示为~~(2-216)(2-217)式中δp是局部插分算子，且有(2-218)式中ep表示第p个元素为1的单位向量。均值运算符μ可以做类似扩展。式(2-215)仅仅是一个插值公式向多维扩展的例子，为了说明如何推导其它形式的扩展，首先介绍对x的线性化变换

z=S－1x

(2-219)函数f定义如下~(2-220)函数f

和f有着相同的泰勒级数逼近形式。而采用式(2-215)多项式插值公式，f，f显然得不到相同的结果。这是因为~~(2-221)2.8.2中心差分逼近

随机向量x的均值和协方差分别定义为(2-222)(2-223)然后再定义(2-224)(2-225)由于f是非线性函数，不可能计算出其精确的均值，通常情况下采用一阶或二阶多项式逼近形式代替。此处，我们用插值公式(2-215)来获取f的多项式近似形式。首先把协方差矩阵的Cholesky分解因子作为变换矩阵(2-226)该变换有时也是对x进行随机解耦，使得z的各个元素之间相互无关，即(2-227)

1.一阶近似

首先对函数使用一阶截断插值公式进行逼近(2-228)根据定义有E[Δz]=0，则式(2-228)的期望为(2-229)如前所述，由于Δz均值为0，因此一阶项可以忽略。此外，有E[ΔziΔzTj]=0，i≠j。一阶估计协方差即式(2-224)推导为(2-230)考虑到

，式中sx，p表示对协方差矩阵Sx进行Cholesky分解后的因子的第p列，则式(2-230)可以表示为(2-231)同样，我们可得到一阶估计互协方差矩阵为(2-232)上式也同样可以写为(2-233)可以看出，区间长度对于一阶估计均值没有影响，但是却对一阶协方差和互协方差有影响。分析表明，区间长度h最优值的选取受到Δz分布的影响。可证明h与其峰值存在关系h2=σ4。

2.二阶近似

采用二阶截断插值公式，可以得到f的更加精确的均值和协方差的逼近值。其二阶截断插值公式为~(2-234)由于Δz的均值为零，且其各个元素是互不相关的，可以得到f的二阶估计期望为~(2-235)上式可被写为(2-236)可以获取协方差的估计值为(2-237)那么，可以得到二阶协方差估计值为(2-238)式(2-238)中的奇数矩阵均值为零，它的第一项已经在一阶近似中研究过，现在就来看它的第二项和第三项。第二项由如下三项构成(2-239)(2-240)(2-241)第三项由如下两项构成(2-242)(2-243)式(2-240)和式(2-243)是相等的，两者相互可以抵消。另外，考虑到随着向量z维数的增加，式(2-241)的计算量也会大大增加，为了降低计算量，将其忽略。再者，也是因为利用二阶插值公式无法计算出所有的四阶矩，就只能用三阶矩逼近函数f。因此，二阶协方差近似式为(2-244)考虑到σ2=1，且令h2=σ4(对于高斯分布σ4=3)，上式可以表示为(2-245)由于(2-246)即σ4≥σ22对任何分布都存在，因此可以选择h2≥1。显然，这意味着协方差估计总是半正定的。二阶互协方差估计Pxy的推导为(2-247)2.8.3中心差分卡尔曼滤波

考虑非线性系统模型(2-248)(2-249)假定vk，wk是独立同分布的，并独立于当前和过去的状态，且有vk~(vk，Q(k))，wk~(wk，R(k))。状态向量xk的一步预测的均值和协方差定义为－－(2-250)(2-251)其中，Yk－1是一个包含有过去量测值的矩阵，即(2-252)为方便起见，状态估计的更新假定为线性的。令状态估计的估计误差最小化，可得(2-253)(2-254)其中(2-255)(2-256)(2-257)相应地，状态向量更新后的协方差矩阵为(2-258)采用一阶多项式近似方法，得到的状态转移和量测方程如下(2-259)

(2-260)其中(2-261)(2-262)在上述近似方程的基础上，可以获得下列滤波步骤。时间更新步骤为(2-263)(2-264)(2-265)量测更新步骤为(2-266)(2-267)(2-268)在接下来的部分，将会介绍采用插值公式的方法来获取非线性系统的状态估计值。

1.一阶CDKF

下面我们将运用之前所介绍的一阶中心差分逼近方法来获取非线性系统的状态估计值。首先，引入以下四个Choleskey分解(2-269)令表示的第j列，其它类似向量也采用这样的方法表示。四个包含差分因子的矩阵定义为(2-270)(2-271)(2-272)(2-273)考虑一个包含有状态向量和过程噪声的扩维状态向量为(2-274)由于假定过程噪声独立于状态向量，那么Δ的协方差为(2-275)

是随机解耦的，引入z，使得。那么，不难看出状态估计问题可以映射到如前所述的一般化的向量函数f(z)上。使用式(2-229)进行状态向量的一步更新~(2-276)在式(2-231)的基础上，通过应用式(2-270)~式(2-273)定义的矩阵，协方差更新为

(2-277)由于vk、xk相互独立，协方差更新可以写为两个矩阵积之和的形式。上述计算过程中，协方差矩阵可能会出现非对称和非正定性，从而带来数值计算问题。与卡尔曼滤波类似，通常采用因子分解的方法加以解决。由于式(2-277)的计算是两个二次项的和，因而不会出现数值计算问题。但是，在量测更新过程中需要用到分解因子，通过对下面复合矩阵的Choleskey分解获取分解因子(2-278)由于该矩阵是矩形矩阵，为了后面的使用，需要将其转换为方阵Choleskey因子，这可以通过Householder三角化方法获得。

量测估计值为(2-279)相应的复合矩阵为(2-280)式(2-280)是式(2-279)误差协方差的Cholekey因子，即(2-281)对于矩阵，需要采用Householder三角化方法将其都转换为方阵。为了计算状态向量和量测估计之间的互协方差矩阵，使用式(2-233)中的结果得

(2-282)依据式(2-253)，卡尔曼增益为(2-283)依据式(2-254)，状态向量更新为(2-284)后验协方差可以通过式(2-258)更新，然而为了避免可能出现的数值计算问题，可以直接使用Cholekey分解因子进行更新，则有下式(2-285)因此，量测更新可以表达为(2-286)显然协方差矩阵的Cholekey分解因子方阵可以通过复合矩阵的三角化分解获得(2-287)

2.二阶CDKF

二阶中心差分滤波器可以通过前面二阶近似中的均值和协方差估计得到。首先定义四个包含有中心差分因子的矩阵(2-288)(2-289)(2-290)(2-291)如同一阶中心差分滤波处理过程一样，我们可以通过式(2-236)来获取状态估计方程(2-292)其中，nx表示状态向量的维数，nv表示过程噪声向量的维数。

先验协方差的Cholekey因子同样可以通过以下复合矩阵获取(2-293)使用和一阶中心差分滤波相同的方法，量测估计及其协方差分别为(2-294)(2-295)其中，nw表示量测噪声向量的维数。互协方差矩阵为(2-296)卡尔曼增益为(2-297)状态向量的量测更新为(2-298)依据式(2-286)协方差矩阵，可得状态向量后验更新的协方差矩阵为(2-299)显然其含有Cholekey分解(2-300)

2.9小结

本章在卡尔曼滤波的基础上，讨论处理非线性系统滤波算法，主要涉及一些常用算法和国际上近年来提出的与卡尔曼滤波相关的非线性滤波算法。讨论的算法有：扩展卡尔曼滤波、不敏卡尔曼滤波、积分卡尔曼滤波、容积卡尔曼滤波、傅立叶厄米特卡尔曼滤波、中心差分滤波。粒子滤波算法也是一类重要的非线性系统滤波方法，放在下一章讨论。第3章粒子滤波方法3.1引言3.2贝叶斯滤波3.3贝叶斯重要性采样3.4序贯重要性重采样粒子滤波算法3.5马尔可夫链蒙特卡罗粒子滤波算法3.6辅助粒子滤波算法3.7正则化粒子滤波算法3.8边缘粒子滤波算法3.9扩展卡尔曼粒子滤波算法3.10高斯和粒子滤波算法3.11小结

3.1引言

粒子滤波是指通过寻找一组在状态空间中传播的随机样本，对概率密度函数p(xk|yk)进行近似，以样本均值代替积分运算，从而获得状态的最小方差估计的一种算法。粒子滤波算法依据系统状态向量的先验分布在状态空间中产生一组随机样本，然后根据观测量不断地调整粒子的权值和位置，通过调整后的粒子信息修正最初的后验概率函数。用数学语言可描述为：针对平稳的时变系统，假定k－1时刻系统的后验概率密度为p(xk－1|yk－1)，依据一定规则选取N个随机样本点，在k时刻获得量测信息后,经过状态更新和时间更新过程,N个粒子的后验概率密度近似为p(xk|yk),随着粒子数的增加，粒子的概率密度函数就能逼近状态真实的概率密度函数，对状态向量的估计结果与最优贝叶斯估计结果接近。粒子滤波适用于非线性非高斯系统的状态估计，突破了传统卡尔曼滤波理论框架，精度可以逼近最优估计，是一种有效的非线性滤波技术，广泛应用于数字通信、图像视频处理、计算机视觉、语音信号处理、机器学习等领域。

3.2贝叶斯滤波

对于跟踪问题，目标状态序列的状态转移可以用下面的目标状态序列{xk,k∈N}的演变方程来描述(3-1)其中，

是关于状态xk－1的非线性函数，是独立同分布的过程噪声序列，nx,nv分别是状态和过程噪声向量的维数。系统量测方程为(3-2)从贝叶斯估计观点来看，跟踪问题就是计算k时刻状态xk的某种置信程度。从1到k时刻，由于给定量测数据z1:k的值不同，得出的xk值也不同，因此需要构造概率密度函数p(xk|z1:k)。假定初始概率密度函数p(x0|z0)≡p(x0)，x0表示初始状态向量，z0表示尚且没有获得量测值，它也是先验概率密度函数。因此从形式上看，通过预测和更新两个步骤就可以递推地得到概率密度函数p(xk|z1:k)的值。

假定k－1时刻的概率密度函数已知，那么通过ChapmanKolmogorov等式及使用模型(3-1)就可以预测出k时刻状态的先验概率密度函数(3-3)在等式(3-3)中，p(xk|xk－1)=p(xk|xk－1，z1:k－1)，且满足等式(3-1)所描述的一阶马尔可夫过程。状态估计p(xk|xk－1)的概率模型由系统等式(3-1)和统计值vk－1来确定。在k时刻，可以得到量测值z=，然后通过贝叶斯规则更新先验概率密度函数(3-4)式中的常量(3-5)是由模型(3-2)定义的似然函数p(zk|xk)和一步预测的统计值p(xk|z1:k－1)共同确定的。在更新式子(3-4)中，量测值zk被用来修正先验概率，以获得当前状态的后验概率。式(3-3)和式(3-4)是最优贝