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

第8章RTS平滑及分段融合方法

8.1引言8.2RTS平滑算法8.3基于分段RTS平滑的凸组合航迹融合算法8.4小结

8.1引言

多传感器信息融合系统的结构可以分为集中式和分布式。集中式融合需要将所有局部传感器量测数据都传送到中心节点，网络传输负载大，对中心节点的处理能力要求高。在分布式融合系统中，每个传感器均有自己的处理器，在各自节点进行预处理，然后将结果送到中心节点，进行融合处理。由于融合中心的主要任务是对各局部航迹进行融合，所以这种融合结构也称为航迹融合[1]。航迹融合网络负载小，对中心节点的处理能力要求不高，所以航迹融合算法历来是人们研究的热点。最易于实现的航迹融合算法是凸组合融合算法[2]，该算法不考虑局部航迹之间的相关性，直接进行线性加权平均，涉及到的参数少，算法简单，运算量小。其它的航迹融合算法还有Bar-Shalom-Campo融合算法、最优分布式估计融合、协方差交叉法以及联邦滤波器融合算法等[1-3]。也有学者采用模糊逻辑的方法进行航迹融合[4-5]，并且取得了较好的整体融合性能。但是，这些算法涉及到的参数多，运算量大。文献[6]在不考虑融合实时性的前提下，应用RTS(Rauch-Tung-Striebel)平滑算法，取得了较好的滤波效果。Willsky等人提出了多尺度自回归估计融合算法[7]，该算法综合了卡尔曼滤波和RTS平滑算法，提高了多尺度估计融合性能。本章考虑到RTS平滑算法[8-10]对噪声的抑制效果要比单纯的卡尔曼滤波结果好，所以将RTS平滑算法应用到融合过程中，以提高航迹融合性能。

RTS平滑算法的逆向平滑过程从最后一个滤波结果反向递推，逐步向前平滑，这就意味着获得了整个目标航迹跟踪过程之后，才使用RTS平滑算法对系统航迹进行平滑，平滑步骤严重滞后，融合结果的实时性难以得到保证。为了避免输出结果大幅度延迟，本章提出了分段RTS平滑算法。该算法思想是对需要平滑的时间段内的滤波结果分段，然后逐段逆向平滑，由于分段长度可大可小，从而在不同程度上改善融合过程中应用RTS平滑造成的实时性欠佳问题。仿真结果表明，分段平滑算法的分段长度对整个航迹融合结果的误差影响不大，并且比传统航迹融合算法效果好。将分段RTS平滑过程应用到凸组合航迹融合过程中，能够较好地克服由于过程噪声使得凸组合航迹融合算法性能降低的问题。在融合过程中，针对局部传感器有无额外计算能力，结合实施分段平滑过程的时机，本章提出了基于分段RTS平滑的先平滑再融合和先融合再平滑两种改进的凸组合航迹融合算法。仿真结果表明，这两种算法的航迹融合性能均高于凸组合航迹融合算法和最优航迹融合算法。

对于处理非线性系统的扩展卡尔曼滤波、不敏卡尔曼滤波[11-12]、高斯厄米特卡尔曼滤波[13-14]、容积卡尔曼滤波[15]和粒子滤波[16-17]，对应的RTS平滑算法有扩展卡尔曼RTS平滑算法、不敏卡尔曼RTS平滑算法[18]、高斯RTS平滑的通用形式[19-20](包括高斯厄米特RTS平滑算法、容积卡尔曼RTS平滑算法)和粒子RTS平滑算法[21-22]等。本章对这些处理非线性系统的平滑算法也做了论述。这些平滑算法都可以结合航迹融合算法进行分段融合，并能够获得较好的融合效果。

8.2RTS平滑算法

8.2.1卡尔曼滤波RTS平滑

最优平滑的目的是在接收到T时刻的量测值后，计算k时刻状态向量xk的后验边缘分布，即p(xk|y1:T)，此处T>k。滤波和平滑之间的差别在于，最优滤波器仅仅使用直到时刻k之前的量测值进行估计，而最优平滑器不仅使用k时刻之前的量测，而且使用k时刻之后的量测值。在获得滤波后验状态分布之后，用贝叶斯最优平滑方程计算从k时刻到T时刻中各时刻的边缘后验分布。根据马尔可夫特性，在状态xk+1给定的条件下，状态向量xk与yk+1:T相互独立，由此可得p(xk|xk+1，y1:T)=p(xk|xk+1，y1:k)。使用贝叶斯公式，在xk+1和y1:T给定的条件下，状态向量xk的概率分布可以表示为(8-1)在y1:T给定的条件下，xk和xk+1的联合分布可以计算为(8-2)其中，在k+1时刻平滑后，状态向量的概率为p(xk+1|y1:T)。通过对xk+1求积分，获得在y1:T条件下xk的边缘分布，即(8-3)因此，计算平滑分布p(xk|y1:T)的后向递推方程通过下列贝叶斯平滑方程给出(8-4)(8-5)其中，p(xk|y1:k)表示在时刻k滤波后的概率分布，p(xk+1|y1:k)是k+1时刻的预测概率分布，若将状态向量看做为离散分布，则在上式的积分运算可以被替换为累加和。

RTS平滑器可以用来求解平滑过程，该方法获得近似解为(8-6)为了获得RTS平滑器的方程，首先给出多元高斯分布的定义及其中的两个引理。

若随机变量x∈Rn是具有均值为m∈Rn、协方差为P∈Rn×n的高斯分布，则其概率密度函数为(8-7)其中|P|为矩阵P的行列式。

引理8.1

多元高斯变量的联合密度。如果随机变量x∈Rn和y∈Rm具有如下概率密度(8-8)则x，y的联合概率密度和y的边缘分布概率密度为(8-9)(8-10)

引理8.2

多元高斯变量的条件密度。如果随机变量x,y具有如下联合高斯概率密度(8-11)则x，y的边缘和条件概率密度如下(8-12)(8-13)(8-14)定理8.1RTS平滑器中，离散固定区间RTS平滑器的后向递推方程如下证明：根据多元高斯分布中的引理8.1，给定y1:k时，xk和xk+1的联合高斯分布为(8-20)其中(8-21)根据状态向量的马尔可夫特性，可得(8-22)于是借助于引理8.2，可得条件概率分布(8-23)其中给定所有量测数据条件下xk和xk+1的联合概率分布为

(8-27)

其中(8-28)由此通过引理8.2，xk的边缘分布为(8-29)(8-30)(8-31)其中证毕。8.2.2高斯RTS平滑算法的通用形式

首先假设滤波结果的高斯近似分布为(8-32)此外，假定在时刻k+1进行平滑后的近似高斯分布为(8-33)根据状态向量的马尔可夫特性，在给定xk+1的条件下，xk和yk+1|T相互独立，因此有(8-34)借助于贝叶斯公式，在xk+1和y1:T条件下，xk的条件概率密度函数可计算为(8-35)也就是说，p(xk|xk+1，y1:T)的高斯近似形式由以下两项构成：

(1)构造联合分布p(xk，xk+1|y1:k)的高斯近似形式；

(2)以xk+1为条件，计算上述联合分布，可得p(xk|xk+1,y1:T)的高斯近似形式。

这里需要采用假设密度滤波的方法，通过近似计算分布p(xk，xk+1|y1:k)的前二阶矩。注意到因为该分布独立于未来量测值yk+1:T，因此近似也仅仅依赖于滤波结果和系统动态模型。

如果我们假定滤波结果是高斯分布p(xk|y1:k)=N(xk|mk|k，Pk|k)，则近似推导过程如下。首先计算高斯积分为(8-36)(8-37)(8-38)下列联合分布的高斯近似可以构造为(8-39)式(8-35)的除法结果仍然是高斯分布，因此根据高斯分布的计算规则可得(8-40)(8-41)(8-42)其中如果定义平滑增益为(8-43)则上述方程可写为(8-44)(8-45)给定所有观测数据条件下，xk和xk+1的联合分布现在变为(8-46)其中(8-47)(8-48)xk的边缘分布为(8-49)其中(8-50)(8-51)因此，该通用的固定区间平滑器以滤波结果mT|T，PT|T为起点，按照k=T－1，T－2，…，0的次序做如下步骤的计算：

(1)预测。根据式(8-36)~式(8-38)计算预测均值mk+1|k，预测协方差Pk+1|k和互协方差Ck，k+1。

(2)平滑。计算平滑增益Gk、平滑均值mk|T和协方差Pk|T如下(8-52)(8-53)(8-54)上述方法可以更一般化到非加性噪声的情形，此时，状态转移方程和量测方程如下(8-55)(8-56)其中，qk－1和rk是服从高斯分布的高斯噪声。用更为一般的形式替换式(8-36)~(8-38)中的积分(8-57)(8-58)(8-59)需要指出的是，在不敏变换情况下该方法等价于在UKF中使用的扩维方法。实际上，随机变量qk也可以被替换为非高斯分布，只需要在式(8-57)~式(8-59)中将高斯分布N(qk|0，Qk)用非高斯噪声分布函数进行替换即可。8.2.3不敏卡尔曼滤波RTS平滑

结合不敏变换和高斯RTS平滑的通用形式，具有加性噪声形式的不敏RTS平滑算法步骤如下。

Step1：构造西格玛点集:(8-60)(8-61)(8-62)其中，λ=α2(n+κ)－n，n为状态向量维数，运算符[]i表示矩阵的第i列。

Step2：借助状态转移方程计算传播后的西格玛点集:(8-63)

Step3：计算预测均值mk+1|k、预测协方差矩阵Pk+1|k和互协方差矩阵Dk+1:(8-64)(8-65)(8-66)其中权值W(m)i表示第i个西格玛点对应的权值，W(c)i表示第i个西格玛点对应的协方差矩阵的权值，为方便起见,常令W(m)i=W(c)i，其定义见UKF滤波一节。

Step4：计算平滑增益Gk、平滑均值msk|k和协方差Psk|k:(8-67)(8-68)(8-69)上述步骤的计算是从最后一个时刻的滤波结果msT|T=mT|T，PsT|T=PT|T开始的，递推时刻序列为k=T－1，…，0。

假定滤波结果的近似均值和协方差为

p(xk|y1:k)≈N(xk|mk|k，Pk|k)

(8-70)

时刻k+1的平滑分布已知且近似高斯分布:

p(xk+1|y1:T)≈N(xk+1|msk+1|k+1，Psk+1|k+1)

(8-71)

对最优平滑估计的基于不敏变换的近似方法推导如下。

Step1：给出xk和xk+1的联合分布的高斯近似如下(8-72)上式中变量可以通过式(8-64)~式(8-66)求解，也就是利用不敏变换求解具有加性噪声的状态估计过程。

Step2：由于式(8-72)是高斯分布的，借助于高斯分布的计算规则，xk的条件分布给定如下其中

Step3：给定所有观测数据，xk，xk+1的联合分布为(8-77)其中(8-78)(8-79)于是xk的边缘分布为(8-80)其中(8-81)(8-82)采用类似的方法，结合扩维不敏卡尔曼滤波的思路，易于获得扩维的不敏RTS平滑算法。8.2.4高斯厄米特RTS平滑算法

在加性高斯RTS平滑算法的通用形式中使用高斯厄米特近似方法，可以获得高斯厄米特RTS平滑算法，算法过程如下：

Step1：用一维单位西格玛点的笛卡尔乘积构造多维单位西格玛点，即(8-83)Step2：构造西格玛点如下(8-84)

Step3：将西格玛点带入状态转移方程，计算西格玛点的状态转移值(8-85)

Step4：用一维权值的乘积构造多维权值(8-86)其中每一个ik取值为1，…，p。

Step5：计算预测均值mk+1|k、预测协方差Pk+1|k以及互协方差Dk+1，即(8-87)(8-88)(8-89)

Step6：增益Gk，状态向量估计msk|k和协方差矩阵Psk|k计算如下(8-90)(8-91)(8-92)8.2.5容积卡尔曼滤波RTS平滑

在加性高斯RTS平滑算法的通用形式中使用3阶球形容积近似方法，可以获得加性噪声条件下的容积卡尔曼滤波RTS平滑器，该算法过程如下:

Step1：构造西格玛点(8-93)其中单位西格玛点定义为(8-94)Step2：将西格玛点代入状态转移方程，可得(8-95)

Step3：计算预测均值mk+1|k、预测协方差矩阵Pk+1|k和互协方差矩阵Dk+1：(8-96)(8-97)(8-98)

Step4：计算增益Gk、均值msk|k和协方差矩阵Psk|k：(8-99)(8-100)(8-101)在非加性形式的高斯RTS平滑算法的通用形式中使用3阶球形容积近似方法，可以获得非加性噪声条件下的容积卡尔曼滤波RTS平滑器，该算法过程如下：

Step1：由n′=n+nq维扩维随机变量(xTk，qTk)T构造西格玛点集(8-102)其中，。Step2：通过状态转移方程传播西格玛点集(8-103)

Step3：计算预测均值mk+1|k、预测协方差矩阵Pk+1|k和互协方差矩阵Dk+1(8-104)(8-105)(8-106)

Step4：计算增益Gk、均值msk|k和协方差矩阵Psk|k，(8-107)(8-108)(8-109)8.3基于分段RTS平滑的凸组合航迹融合算法

8.3.1分段RTS平滑算法

假设某一动态系统的总时长为k=1，2，…，N。分段RTS平滑算法就是对N个观测值分段进行前向滤波和逆向平滑，假设分段长度为L，且1≤L≤N，则分段RTS平滑算法的步骤如下：

Step1：输出变量清空，即xs=[]，Ps=[]。

Step2：局部滤波结果变量清空，即M=[]，P=[]，分段平滑计数器清零，即count=0。

Step3：如果对目标的跟踪没有结束，则继续向下处理；否则输出结果xs，Ps，整个分段RTS平滑算法结束；

Step4：接收当前滤波结果并保存在变量M，P中，并采用计数器count计数。若count=L，则针对局部滤波结果变量M，P采用常规RTS平滑算法进行逆平滑过程，平滑结果保存在输出变量xs，Ps中；转到Step2。

算法中，x，P既可以是局部节点的滤波状态及方差阵，也可以是中央节点对各个局部节点滤波结果融合之后的状态值及方差阵；xs和Ps表示分段RTS平滑之后的状态值及方差阵。

如果定义分段RTS平滑算法的响应时间为从数据采样开始到获得第一个平滑结果为止的时间，则有

tr=(tsampling+tfiltering+tsmoothing)L

(8-110)^其中，tsampling表示平均采样时间，tfiltering表示平均滤波时间，tsmoothing表示平均逆向平滑时间，L表示RTS平滑算法分段的长度。当L≥N时，分段RTS平滑算法退化为常规RTS平滑算法；当L<N时，分段RTS平滑算法的响应时间比不分段的RTS平滑算法的响应时间短。

融合中心接收到L个滤波数据后，立刻进行逆向平滑过程，不必等待后续数据的到来，这样做可以降低逆向平滑的滞后时间。分段长度L取值越小，融合算法的实时性就越好。8.3.2算法描述及分析

在凸组合航迹融合过程中应用分段RTS平滑算法，针对局部节点有无额外计算能力，结合实施逆向平滑过程的时机，将其分为两种：

(1)先平滑再融合(SmoothingFirstandFusingNext，SFFN)，即先对局部航迹进行分段RTS逆向平滑，然后再利用凸组合算法进行融合。

(2)先融合再平滑(FusingFirstandSmoothingNext，FFSN)，即先采用凸组合算法融合局部航迹，再对融合后的航迹进行分段RTS逆向平滑。

这两种方法的融合过程分别如图8.1(a)和图8.1(b)所示。图8.1基于RTS平滑的航迹融合过程在工程实践过程中，如果局部节点有额外的计算能力，就将逆向平滑过程放在局部节点处理，即采用SFFN；如果局部节点没有额外的计算能力，就将逆向平滑过程放在中心节点处理，即采用FFSN。由于在中心节点实施逆向平滑过程使用的数据是融合后的，经过的处理比在局部节点多，因此从理论上讲，采用SFFN算法比采用FFSN算法效果好。仿真结果与该结论一致。

假定各个局部节点的数据采样是同步的，并且忽略数据在分布式传感器网络中的传输时间，那么航迹融合系统的响应时间可以采用下式估算(8-111)其中，tr1、tr2、tr3分别表示SFFN、FFSN和CCF(ConvexCombinationtrack-to-trachFusion)三种算法的响应时间，

tsampling表示平均采样时间，tfiltering表示平均滤波时间，tfusion表示平均融合时间，tsmoothing表示平均逆向平滑时间，L表示RTS平滑算法分段的长度。当L≥N时，分段RTS平滑算法退化为常规RTS平滑算法，故此时前两种算法的响应时间并未减少；当1≤L<N，尤其是当L<<N时，分段RTS算法的响应时间能够大大缩短。由以上分析可知，当分段长度选取得比较小时，分段平滑算法可以有效地缩短融合结果响应时间，从而确保航迹融合的实时性。在使用SFFN和FFSN算法过程中，如果分段RTS平滑算法分段长度过长，则融合结果滞后，无法满足融合实时性要求；如果分段长度过短，则融合性能有可能达不到最优。这就需要在实时性和精确性之间做折中。由于最优分段长度难于提前估算，因此使用过程中只能凭借经验选取合适的分段长度。本章实验一和实验三选取的分段长度值为1，试验结果表明，虽然所选取的分段长度并不能保证估计结果的精度达到理想值，但是同样能够大幅度提高航迹融合性能。8.3.3仿真实验及结果分析

假设某系统状态变量为x=[ξ

ξ]T，其离散状态方程和量测方程为·(8-112)其中，i=1，2表示传感器1和传感器2。过程噪声Q和量测噪声R

i是相互独立的零均值高斯白噪声，过程噪声、量测噪声和状态初始值互不相关。实验中取x0=[0

10]T，P0=[101;110]，R1=5，R2=15，T=1，仿真步数N=100步。实验一算法性能分析

采用分段长度L=1和过程噪声Q=10的基于分段RTS平滑的凸组合融合算法进行1000次MonteCarlo仿真，对比SFFN、FFSM、CCF和OF(Optimaltrack-to-trachFusion)四种融合算法的均方根误差。

实验结果如图8.2(a)和图8.2(b)所示。结果表明，在融合过程中应用分段长度为1的RTS平滑过程后，融合效果明显得到改善，状态向量两个分量的误差大幅度降低。从实验结果来看，所提出的两种方法比CCF和OF算法的误差性能好。图8.2融合算法误差性能对比实验二分段长度对算法性能影响分析

令过程噪声Q=10，依次改变RTS平滑算法的分段长度L=1，2，3，…，N，分别采用不同的分段长度进行MonteCarlo仿真100次，计算不同分段长度仿真结果的均方根误差。当使用平滑尺寸参数k进行仿真时，其均方根误差的计算式为(8-113)实验结果如图8.3(a)和图8.3(b)所示，横坐标表示分段的长度。实验结果表明，分段RTS平滑算法的分段长度对融合结果影响不大，随着分段长度的增加，融合误差趋向常数。而且，不论对分段RTS平滑算法的分段长度如何选择，基于分段RTS航迹融合方法的效果总是优于C