《目标跟踪系统中的滤波方法》课件第6章

第６章无序量测条件下的滤波方法

6.1引言6.2问题描述6.3单步滞后无序量测算法6.4基于UT变换的单步滞后无序量测算法6.5仿真实验及结果分析6.6小结

6.1引言

在时间序列信息的实时处理中，传感器对信号进行量测，然后将量测数据传输到处理中心。而在传输过程中，由于传输延迟等原因使得来自同一目标的较早时刻的量测在较晚的量测之后到达处理中心的情况时有发生，这就是所谓的滞后无序量测(Out-Of-SequenceMeasurements，OOSM)问题。如果各个传感器的滞后间隔最多只有一步，则称之为单步滞后无序量测(Single-step-lagOOSM)。处理滞后量测问题的最优方法为，从滞后时刻起，对所有量测数据进行估计。此时相当于没有滞后情况发生[1]。但是最优方法存在的缺点是需要存储从滞后发生时刻起的所有量测值，存储量较大，而且在滞后数据到达后，需要重新进行状态估计，计算量也较大。为了解决上述问题，人们提出了一系列次优算法，目的是利用滞后的量测数据对当前时刻的目标状态进行再更新，以获得更精确的状态估计及其误差协方差矩阵，典型的如Bar-Shalom提出的A1、B1和C1算法等[1-3]。文献[4，5]从理论上分析了Bar-Shalom提出的A1算法的最优性，指出其最优性与过程噪声的离散化模型有关，提出了一种DDM(DirectDiscrete-timeModel)条件下的改进算法和一种与过程噪声离散化模型无关的最优无序量测滤波算法，提高了滤波的精度。文献[6]采用等效量测的方法将文献[2]、[3]中的单步滞后OOSM更新算法扩展到了多步滞后OOSM更新算法。文献[7]提出了基于最佳线性无偏估计(BestLinearUnbiasedEstimation，BLUE)准则的OOSM更新算法。文献[8，9]在单步OOSM算法的基础上，提出了用于解决多个传感器单步滞后OOSM融合问题的方法。尽管上述算法在应用中取得了良好的效果，然而这些算法都是将卡尔曼滤波作为基础滤波算法，因此只能用于解决线性高斯系统的状态估计问题。对于非线性系统，则需要在上述算法的基础上应用扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)算法思想对其进行扩展，从而获得相应的基于EKF的A1、B1和C1等更新算法(以下简称EKFA1、EKFB1、EKFC1)。然而有些非线性问题并不能够用EKF算法求解，例如当非线性方程的雅可比矩阵(或Hessian矩阵)不存在时，基于EKF的OOSM更新算法失效。再者，如果量测函数是高度非线性的，在利用泰勒级数展开式做近似的过程中，误差过大，从而造成整个更新结果误差较大。针对这些情况，文献[10，11]提出了基于粒子滤波的OOSM算法，可以处理任意非线性非高斯系统的估计问题。文献[12-14]将不同的粒子滤波改进算法应用于OOSM更新过程，在一定程度上改善了滤波的性能。然而，基于粒子滤波的OOSM算法一方面需要存储滤波过程中所有粒子及其权值，另一方面大量粒子参与运算需要极大的运算量，再加上它本身就是处理滞后量测问题的次优算法，这与研究量测滞后算法的动机互相矛盾。本章在经典的A1算法的基础上，推导了非线性系统的单步滞后OOSM更新算法，提出了基于UT变换(UnscentedTransformation)[15，16]的单步滞后OOSM算法。该算法适用于动态方程是线性而量测方程是非线性的非线性高斯系统，在处理过程中不需要求解非线性量测方程的雅可比矩阵(或Hessian矩阵)。由于UT变换属于确定点采样方法，采样点数目很少，一般为状态向量维数的2倍加1，因此本章算法计算量增加幅度不大，具有较好的实时性。

6.2问题描述

假定给定系统的状态向量为x，量测向量为z，其线性状态方程和非线性量测方程如下

xk=Fk，k－1xk－1+vk，k－1

(6-1)

zk=h(xk，k)+wk

(6-2)

其中，Fk，k－1是状态转移矩阵，vk，k－1是服从高斯分布的零均值过程噪声，h是非线性量测函数，wk是服从高斯分布的零均值量测噪声。过程噪声和量测噪声互不相关，其方差分别为E[vk，jvTk，j]=Qk，j，E[wkwTk]=Rk。

由式(6-1)可得从时刻d到时刻k(d<k)的状态转移方程为

xk=Fk，dxd+vk，d

(6-3)上述公式可写为负时间的更新形式(从时刻k到时刻d的状态转移)，即

xd=Fd，k(xk－vk，d)

(6-4)

其中，状态转移矩阵Fk，d=F－1d，k。

假定在时刻k，处理中心已经通过计算获得了k时刻的状态向量的充分统计量(6-5)(6-6)其中，k时刻的累计量测值Zk={zi}ki=1。并假定此时先前时刻d(d<k)的量测zd到达处理中心，我们要解决的问题是如何利用量测zd直接更新k时刻的状态估计值及其协方差，即要求解其中，Zd={Zk，zd}。如果k－l≤d<k－l+1，则是l步滞后OOSM。特别地，当l=1时，称之为单步滞后无序量测(Single-step-lagOOSM或1-step-lagOOSM)。单步滞后OOSM的情形如图6.1所示，本章只讨论单步滞后OOSM问题。图6.1单步滞后OOSM示意图

6.3单步滞后无序量测算法

6.3.1回溯状态

Bar-Shalom提出了用于解决单步滞后OOSM的A1算法[2-3]。该算法适用于线性高斯系统。对于弱非线性高斯系统，通过求解非线性函数的雅可比矩阵或Hessian矩阵，即可以得到k时刻的一阶(或二阶)近似的线性状态转移矩阵Fk,k－1或者线性量测矩阵Hk。此时，就可以直接利用A1算法求解单步滞后OOSM问题，从而获得EKFA1算法。

假定线性高斯系统的状态转移方程和量测方程如式(6-1)和式(6-2)所示，则k时刻非线性量测函数的雅可比矩阵为(6-7)根据式(6-4)，可得(6-8)此外，状态回溯的协方差是在Zk的条件下最后两项xk和vk，d的交叉协方差。首先定义(6-9)为了获得yk|k，使用线性估计的基本方程，即有^(6-10)(6-11)和当所有随机变量是联合高斯分布时，这些方程产生条件均值。除此之外，这些方程可以产生线性最小均值方差误差(LinearMinimumMeanSquareError，LMMSE)估计。式(6-10)中的第一个协方差为(6-12)为了计算式(6-12)，下式可写为(6-13)接下来，利用上式，有(6-14)由于上式中相互正交，根据假设E[vk，jvTk，j]=Qk，j，有(6-15)利用上述结果和式(6-11)表示协方差，根据矩阵的分块，可以明确地写出(6-16)式(6-10)其它相关项是(6-17)该式是新息的标准协方差，其中新息为(6-18)

因此，式(6-10)中过程噪声的条件均值为(6-19)所以，从时刻d到时刻k的状态回溯值为(6-20)式(6-19)对应的协方差为(6-21)式(6-8)中状态项和噪声项的交叉协方差为(6-22)此外，由标准卡尔曼滤波，易得(6-23)结合上述结果，可以获得式(6-20)所表达的回溯状态向量的协方差为(6-24)值得注意的是，给定Zk，上述公式的条件协方差都是独立的。6.3.2具有无序量测状态估计的最优更新过程

根据式(6-24)，d时刻的回溯量测值的协方差为(6-25)由量测方程知

，结合式(6-4)可得k时刻的状态和量测之间的协方差矩阵为(6-26)因此，k时刻状态估计xk|k的具有一步无序量测值zd的更新方程为^(6-27)其中，xd|k根据式(6-20)计算得到，增益的计算公式为^(6-28)更新后的状态估计的协方差为(6-29)6.3.3A1算法

现将具有一步滞后量测的A1算法过程叙述如下：

从时刻k到时刻d的回溯状态为(6-30)与回溯状态向量相关的协方差为(6-31)(6-32)(6-33)回溯状态的协方差为(6-34)回溯量测值的协方差为(6-35)

k时刻的状态和量测值之间的协方差为(6-36)更新中所用的增益计算为(6-37)具有一步无序量测zd的最近状态估计xk|k的更新方程为^(6-38)更新后的状态估计协方差为(6-39)6.3.4次优算法B1和C1

在下列的B1算法和C1算法中，将回溯噪声假定为0，因此为次优算法。其中B1算法过程如下。

从时刻d到时刻k的回溯状态为(6-40)与回溯状态向量相关的协方差计算如下(6-41)(6-42)(6-43)回溯状态的协方差为(6-44)回溯量测值的协方差为(6-45)k时刻的状态向量和量测向量之间的协方差计算如下(6-46)增益计算公式为(6-47)

具有一步无序量测zd的最近状态估计xk|k的更新方程为^(6-48)(6-49)更新后的状态估计协方差为算法B1和算法A1之间的差别是式(6-40)和式(6-41)，它们分别是式(6-30)和式(6-31)的近似和简化。

下面给出次优算法C1的过程，该算法同样在假定回溯噪声为0的基础上，进一步做了简化。该算法仅仅使用了式(6-20)、式(6-24)和式(6-26)的第一项。从时刻d到时刻k的回溯状态为(6-50)回溯状态的协方差为(6-51)

回溯量测值的协方差计算为(6-52)

k时刻的状态向量和量测向量之间的协方差计算如下(6-53)

增益矩阵计算公式为(6-54)具有一步无序量测zd的最近状态估计xk|k的更新方程为^(6-55)

更新后的状态估计协方差为(6-56)算法C和算法B的差别仅在于式(6-44)被替换为式(6-51)。6.3.5B1和C1算法的均方误差

在两种次优算法计算各自协方差的过程中，由于都忽略了式(6-19)的计算，因而其最终的状态估计是非精确的。两种算法的估计都是有偏的，相应的矩阵均方误差同样受到该有偏估计的影响，下面计算这种有偏性对矩阵均方误差的影响大小。

注意到两种算法都使用了下列更新形式(6-57)其中*表示B1算法或者C1算法。上述估计的误差可用下式表达(6-58)其中(6-59)相应地，矩阵均方误差为(6-60)过程噪声的矩阵均方误差为(6-61)其中最后一项由式(6-19)给出，易知其依赖于k时刻的量测值。为了获得数据相互独立结果，式(6-61)替换为

(6-62)(6-63)得到最终结果

6.4基于UT变换的单步滞后无序量测算法

6.4.1用UT变换解决单步滞后OOSM

假定系统方程如式(6-1)和式(6-2)所示，依据线性最小均值方差(LinearMinimumMeanSquareError，LMMSE)准则估计方程[17]，可得(6-64)式(6-65)(6-66)下面首先来推导E[zd|k|Zk]，即

(6-67)由于E[vk，d]=0，式(6-67)在最后一步推导做了近似。此时(6-67)化简为(6-68)式(6-66)中有cov[xk，zd|Zk，zd]和cov[zd|k|Zk]需要求解。前者是k时刻的状态向量和d时刻的量测值之间的协方差，后者是d时刻追溯量测值之间的协方差。在量测方程是非线性的情况下，这两个量可以通过基于EKF的单步滞后OOSM算法予以解决，但是这需要计算非线性方程的雅可比矩阵(或Hessian矩阵)。此时，如果雅可比矩阵(或Hessian矩阵)不存在，则不能使用基于EKF的方法。再者，如果量测函数是高度非线性的，在利用泰勒展开式做近似的过程中，误差过大，就会造成整个更新结果误差较大。为此，本章提出采用UT变换的方法来求解式(6-66)中的协方差，从而解决了基于EKF的方法所带来的问题。通常认为近似一个概率分布比近似一个非线性函数或者非线性变换更容易。UT变换的思想就是通过一些选定的点并且给定这些点相应的权值来近似一个概率密度函数的分布[15，16]。UT变换过程中利用随机变量估计值及其方差产生相应采样点集，通常称为西格玛点集(Sigmapoints)。权值的确定与状态向量的维数及其它一些参数的选择有关，与西格玛点本身及其方差无关。对每个西格玛点实施非线性变换从而获得相应的变换采样点，非线性变换的均值和方差就可以通过这些变换采样点得到。UT变换的好处是不需要计算非线性函数的雅可比矩阵(或Hessian矩阵)，对于强非线性变换的近似性能仍然较好。

UT变换过程可以描述为：对于均值为xk|k、方差为Pk|k的n维随机变量x，可以通过2n+1个西格玛点χ(i)及其权值ω(i)进行近似，即^其中(P)i表示矩阵P的第i列。为了求解cov[xk，zd|Zk，zd]和cov[zd|Zk]，我们使用UT变换方法进行计算。具体实现步骤如下：

输入：xk|k，Pk|k，xd|k，Pd|k；

输出：cov[xk，zd|Zk，zd]，cov[zd|Zk];

Step1：将xk|k，Pk|k代入式(6-69)、式(6-71)和式(6-73)，并利用式(6-70)、式(6-72)和式(6-74)计算k时刻状态估计值的西格玛点集及其权值{χ(i)k|k，ω(i)k|k}2ni=0；

Step2：将xd|k，Pd|k代入式(6-69)、式(6-71)和式(6-73)，并利用式(6-70)、式(6-72)和式(6-74)计算d时刻状态估计值的西格玛点集及其权值{χ(i)d|k，ω(i)d|k}2ni=0；

Step3：对Step2所得西格玛点集进行非线性变换，即^^^^(6-75)其中，由西格玛点集的求解过程易知，ω(i)d|k=ω(i)

k|k，i=0，…，2n；

Step4：计算追溯量测值(6-76)

Step5：计算k时刻的状态向量和d时刻追溯量测值之间的协方差，即(6-77)

Step6：计算d时刻追溯量测值之间的协方差，即(6-78)将式(6-77)和式(6-78)代入式(6-66)，求得OOSM状态更新增益Wk，d。再将式(6-65)、式(6-66)、式(6-68)代入式(6-64)，可求得OOSM状态更新向量xk|d。此时，状态估计对应的协方差矩阵为^(6-79)因此，由式(6-77)、式(6-78)和k时刻状态向量协方差Pk|k即可求得Pk|d。假设过程噪声的向后预测为零，则式(6-30)可以简化为(6-80)另外，对式(6-34)进一步近似得(6-81)

6.4.2单步滞后OOSM多传感器量测融合方法

在多传感器集中式量测融合过程中，每个传感器都可能出现单步滞后OOSM(为简单起见，只考虑单步滞后OOSM情况)。当出现单步滞后OOSM量测时，采用上述基于UT变换的单步滞后OOSM算法对当前时刻的状态估计及其协方差序贯更新，可以获得更为精确的状态估计及其协方差。另外，在多个传感器量测融合过程中，还可能出现某些时刻没有接收到量测值，不能使用当前时刻的量测值对一步状态预测及其协方差进行更新，此时可将一步状态预测及其协方差作为当前时刻的状态估计及其协方差。假设k时刻输入参数包括k－1时刻的状态估计及其协方差xk－1|k－1和Pk－1|k－1、k时刻的量测值{zik}mi=0，以及稍后到达的单步滞后量测{zid}ni=0，其中m+n≤M，M表示传感器的个数，则具体融合过程如下：

输入：

输出：^

Step1：若m>0，则使用集中式序贯融合算法进行融合，融合结果为xk|k，Pk|k；若m=0，则利用状态转移方程，计算状态一步预测值及其协方差，并将其作为k时刻的状态估计及其协方差，即^(6-83)

Step2：对于{zid}ni=0，若n=0，则不做处理；若n>0，则循环应用基于UT变换的单步滞后OOSM算法对k时刻的状态估计及其协方差进行更新，并把更新后的结果作为第k步的状态估计及其协方差，即(6-84)

6.5仿真实验及结果分析

6.5.1实验模型

假设目标做如下运动[18]：①1～49步，目标沿着x方向以初始速度1向前运动；②50～100步，以角速度pi/2/51/T完成90°的顺时针转弯动作；③101～199步，继续向前运动；④200～250步，以角速度pi/2/51/T完成90°的顺时针转弯动作；⑤251～350步，继续向前运动；⑥351～400步，以角速度pi/2/51/T完成90°的顺时针转弯动作；⑦401～500步，继续向前运动。其中，T=0.01。传感器1的位置为(－1km，

－2km)，传感器2的位置为(1km，1km)。目标运动轨迹以及传感器位置如图6.2所示。图6.2目标运动轨迹状态向量为xk=[ξk;x

ξk;y

ξk;x

ξk;y]T，其分量分别表示x和y方向上的位置和速度分量。状态方程为··(6-85)其中，过程噪声vk服从高斯分布，其协方差矩阵为(6-86)其中功率谱密度q=0.12。量测方程为(6-87)6.5.2仿真结果及分析

分别采用EKFA1、EKFB1、EKFC1算法和本章算法，利用滞后量测对当前滤波结果进行更新。MonteCarlo仿真1000次，计算每一步的均方根误差(RMS)。图6.3给出了x和y方向的位置误差，图6.4给出了x和y方向的速度误差。由图可见，有序量测(In-SequenceMeasurements)融合效果最好；丢弃滞后量测(Discarded)的策略效果最差；本章算法比EKFA1算法滤波精度高一些。其中，In-Sequence表示在两个传感器融合过程中，没有OOSM情况发生；Discarded表示在两个传感器量测融合过程中，对于出现的滞后量测，直接丢弃。在状态估计的最初一段时间，本章算法性能比EKFA1算法性能改善很多，经过多步滤波之后，本章算法虽然仍然优于EKFA1算法，但是二者的误差非常接近。图6.3位置分量的估计误差采用MonteCarlo方法仿真1000次，对不同方法的平均均方根误差(AverageRootMe