基于无偏伪线性卡尔曼滤波的3D 到达角目标跟踪

赵 唯 ,黄子豪 ,郝程鹏

(1.中国科学院 声学研究所,北京,100190;2.中国科学院大学 电子电气与通信工程学院,北京,100049)

0 引言

水下到达角目标跟踪是仅通过观测站采集到的到达角信息来估计目标位置和速度等状态的技术,由于其无需观测站辐射信号,具有抗干扰性强、高隐蔽性的优点[1-2],是水下目标跟踪重要的方法之一。随着到达角测量的实现成本不断降低,到达角目标跟踪技术的适用范围不断扩大,现已广泛应用至导航定位、侦察监控、水下搜救及资源开发等民用和军用领域[3-6]。

到达角目标跟踪是一类典型的非线性滤波问题,其最大的挑战在于角度观测值和目标状态之间的非线性关系[7]。针对2D 空间中的到达角目标跟踪问题,Aidala[8]提出了伪线性卡尔曼滤波(pseudo linear Kalman filter,PLKF)算法,PLKF 利用观测站与目标之间的几何关系将观测方程转换为伪线性形式,可直接应用线性卡尔曼滤波,具有结构简单、对初始误差不敏感的优点[9],但PLKF 对目标状态估计存在较大的偏差。随后,Ngugen 等[10]对PLKF 的偏差进行了分析,认为偏差主要来源于伪线性方程中观测噪声和观测矩阵的相关性,并提出了基于偏差补偿的伪线性卡尔曼滤波(bias compensated PLKF,BCKF)算法和基于工具变量的伪线性卡尔曼滤波(instrumental variable PLKF,IVKF)算法。其中BCKF 对PLKF 的估计结果进行了偏差补偿,IVKF 则利用BCKF 的估计结果构造与观测噪声无关的工具变量矩阵,以此减少噪声和观测矩阵的相关性,进一步减小了估计偏差。但随着观测噪声的增加,偏差补偿项的准确性降低,使得BCKF 的估计偏差明显增大,由于工具变量矩阵的构造依赖于BCKF 的估计结果,IVKF 算法也易受到BCKF 偏差增大的影响,工具变量矩阵构造的准确性降低,跟踪性能也随之下降。对此,Huang等[11]提出了噪声分离的方法,通过代数运算直接构造一个不含噪声的观测矩阵,提出一种无偏伪线性卡尔曼滤波(unbiased PLKF,UBKF)算法,有效提高了2D 空间中目标跟踪的精度。由于目标多在3D 空间运动,实际应用意义更高的3D 无源到达角目标跟踪也得到了发展。Ngugen 等[12]提出了基于工具变量的3D 伪线性卡尔曼滤波(3Dinstrumental variable PLKF,3D-IVKF)算法,将2DIVKF 拓展到3D 空间。与2D-IVKF 类似,3D-IVKF的跟踪精度会随观测噪声增大迅速下降,且由于维度增加,算法的复杂度较高。

在实际应用中,到达角目标跟踪面临的另一个挑战是观测站定位误差导致的估计性能下降,这是因为在目标匀速直线或匀加速运动假设下,为保证目标运动参数可观测,需要观测站进行机动运动,而机动观测站的位置信息依赖于定位系统且往往存在定位误差[13]。针对这一问题,文献[14-16]利用位置已知的校准源来降低观测站定位误差对到达时间差(time difference-of-arrival,TDOA)目标定位的影响,虽然定位精度有所提高,但校准源的使用增加了定位系统的复杂度。Farina[17]通过对观测方程线性化,得到与观测站定位误差和观测噪声相关的近似噪声协方差矩阵,提出了一种最大似然估计器(maximum likelihood estimator,MLE)。Pang 等[18]利用观测站定位误差的统计特性建立了新的2D 到达角偏差补偿算法。以上2 种算法均采用批处理方式,不仅计算复杂度高且需要目标的状态模型为确定系统,难以应对存在过程噪声或目标机动的跟踪问题。

为解决上述问题,文中提出了一种3D 修正无偏伪线性卡尔曼滤波(3D-modified UBKF,3DMUBKF)算法。该算法考虑了观测站定位误差对方位角及俯仰角观测方程伪线性化的影响,通过修正伪线性噪声的协方差矩阵降低观测站定位误差的影响,接着对观测方程进行整体噪声分离,以此降低观测矩阵与观测噪声之间的相关性引起的偏差。仿真分析表明,3D-MUBKF 算法相较于已有算法在机动和非机动2 种场景下表现更优,在观测噪声和观测站定位误差较大时能够显著降低跟踪误差,且计算复杂度低。

1 系统模型

1.1 目标状态模型

考虑使用单运动观测站对方位角和俯仰角进行观测的3D 到达角目标跟踪问题。在观测时刻k处,目标位置速度vk=[vx,k,vy,k,vz,k]T,均为待估计未知参数,目标的状态向量,状态方程可表示为

式中:F为状态转换矩阵;wk为均值为0、协方差为Qk的独立高斯过程噪声,代表目标在运动过程中的干扰,会使目标发生轻微的未知机动。假设使用常速度模型来对目标运动进行建模,则F和Qk可表示为

式中: 0m×n表示m×n的零矩阵;In×n表示n×n的单位矩阵;T为采样间隔;Q0=diag(qx,qy,qz),qx,qy及qz分别代表过程噪声在x,y及z轴上的功率谱密度分量。

1.2 观测模型

k时刻测量的含噪方位角和含噪俯仰角可表示为

k时刻不确定的观测站的位置可表示为

则k时刻的观测方程为

2 PLKF 分析

针对3D 到达角目标跟踪问题,利用等价代数运算将观测方程转换为伪线性形式是一种简单高效的滤波方式,但伪线性化会使目标的状态估计出现偏差。文中推导存在观测站定位误差情况下的3D-PLKF,并分析观测站定位误差对3D-PLKF的主要影响以及估计偏差的主要来源。

2.1 存在观测站定位误差的3D-PLKF

首先对存在观测站定位误差的到达角观测方程进行伪线性化,方位角的观测方程通过等价代数运算可以转换为

考虑实际情况中的方位角观测噪声和观测站定位误差后,式(8)可写为

将式(6),式(11)代入式(9)得

根据几何关系cosθk(px,k-rx,k)+sinθk(py,k-ry,k)=dkcosϕk和 sinθk(px,k-rx,k)=cosθk(py,k-ry,k),并省略2 阶以上的噪声项,式(12)可以简化为

式中:dk=∥pk-rk∥为目标到观测站的距离;uθ,k=[-sinθk,cosθk,0]T。

采用同样的方法对俯仰角观测方程进行伪线性化,式(5)可以写为

考虑俯仰角观测噪声以及观测站定位误差,式(14)可写为

将式(6),式(11)及式(17)代入式(16),并根据式(8)、式(14)及几何关系d=cosϕkcosθk(px,k-rx,k)+cosϕksinθk(py,k-ry,k)+sinϕk(pz,k-rz,k),可将伪线性噪声表示为

式中,uϕ,k=[-sinϕkcosθk,-sinϕksinθk,cosϕk]T。

此时,k时刻观测方程的伪线性形式可写为

且在观测噪声较小的情况下,伪线性噪声 ηk的协方差其中,Rθ,k≈式(19)已为线性形式,因此可直接应用线性卡尔曼滤波,得到存在观测站定位误差时的3D-PLKF,具体步骤为

2.2 偏差分析

参考文献[10]中的推导方法,存在观测站定位误差时的3D-PLKF 的估计偏差可表示为

式中:E{Ak}代表前一采样时刻偏差的传递,不是偏差产生的直接原因,若其余2 项偏差等于0,则E{Ak}可忽略;E{Bk}代表由Pk|k和wk-1的相关性引起的偏差,在过程噪声较小时可忽略;由于观测矩阵Hk和伪线性噪声 ηk均与到达角观测噪声有关,Hk和 ηk的相关性不可忽略,E{Ck}≠0。因此,观测矩阵Hk和伪线性噪声 ηk之间的相关性是PLKF 算法存在偏差的主要原因。

3 3D-MUBKF 算法

由2.2 节的偏差分析可知,PLKF 算法的估计偏差主要来源于观测矩阵与伪线性噪声的相关性,接下来对伪线性方程(19)进行噪声分离以消除观测矩阵与伪线性噪声的相关性。利用观测噪声较小时的近似关系,可将式(19)中的观测矩阵改写为

将式(29)和式(30)代入式(19)中,并忽略2 阶以上的噪声项,得到改写后的伪线性方程为

令tθ,k=[-cosθk,-sinθk,0]Mxk+dkcosϕk,tϕ,k=可得噪声分离后的伪线性方程

根据式(25),PLKF 算法的偏差可写为

可以看出,经过噪声分离后,式(34)等于0。以式(1)和式(33)为状态空间,得3D-MUBKF,其具体步骤为

由于真实到达角 θk、ϕk以及观测站与目标之间的真实距离未知,无法得到tθ,k、tϕ,k、uθ,k以及uϕ,k的真实值,可利用状态预测值计算,对待求真实值进行近似估计,即

根据以上分析可知,3D-MUBKF 通过修正卡尔曼滤波过程中的噪声协方差矩阵来降低观测站定位误差对算法的影响,当cθ,k=cϕ,k=0时,3DMUBKF 将退化为未修正观测站定位误差的3DUBKF 算法。

4 仿真分析

4.1 评价标准

文中通过蒙特卡洛仿真实验验证3D-MUBKF算法对匀速目标及机动目标的跟踪性能,并与3DUBKF、3D-BCKF、3D-IVKF 以及文献[17]中考虑了观测站定位误差的MLE 进行比较。MLE 的数值解通过高斯牛顿(Gauss-Newton,GN)算法[20]迭代计算,初始迭代值设置为伪线性估计器(pseudo linear estimator,PLE)[20]的估计值,GN 算法迭代10 次后得到MLE 的数值解。仿真实验以均方根误差(root mean squared error,RMSE)、时均RMSE和计算复杂度为性能指标,且RMSE 定义为

时均RMSE 定义为

4.2 仿真结果

4.2.1 匀速目标跟踪

仿真实验首先考虑单运动观测站跟踪匀速运动目标的跟踪问题,目标与观测站的轨迹如图1所示。

图1 匀速目标跟踪的几何模型Fig.1 Geometric model of constant-velocity target tracking

目标的初始位置为[ -30,600,-300]Tm,速度为[1,6,1]Tm/s,观测站采取Leg-by-Leg 机动方式,第1 段从[ -50,-200,-500]Tm 处开始,以[ 4,0.6,0]Tm/s的速度运动70 s,第2 段以 [4,-0.6,0]Tm/s 的速度运动70 s,第3 段以[ 4,0.6,0]Tm/s 的速度运动60 s,连续观测的总时长为200 s,采样间隔T=1 s,过程噪声的功率谱密度qx=qy=qz=0.07 m2/s3。算法的初始状态服从均值为x0=[-30,600,-300,1,6,1]T、协方差为P0|0=ρ2diag(102,102,102,0.52,0.52,0.252)的高斯分布,其中,变量ρ 控制目标状态初始协方差的大小,可用于评估各个算法对初始化误差的敏感程度。假设到达角的观测噪声以及观测站定位误差均为独立同分布的高斯白噪声,其协方差。文中使用σs=σθ=σϕ量化到达角观测噪声,使用观测站定位误差的标准差与两相邻采样时刻间观测站的运动距离之比kr量化观测站的定位误差[19]:kr=(N-1)σx/(|vr,x|T)×100%=(N-1)σy/(|vr,y|T)×100%=(N-1)σz/(|vr,z|T)×1 00%,观测站的速度向量vr=[vr,x,vr,y,vr,z]T。

首先,比较各算法在 σs=4◦,kr=150%,ρ=4时的跟踪情况,此时观测站定位误差的方差Qr,k=diag(36,0.81,0)。图2 给出了MLE、3D-BCKF、3DIVKF、3D-UBKF、3D-LEUBKF 位置和速度估计的RMSE,其中跟踪时刻k=N/T。可以看出,MLE在位置估计上偏差较小,但对于速度的估计存在较大偏差。3D-UBKF 和3D-MUBKF 较3D-BCKF和3D-IVKF 估计偏差有大幅提升,且3D-MUBKF由于考虑了观测站位置误差对算法的影响,跟踪精度更高。

图2 匀速目标跟踪中不同算法RMSE 曲线Fig.2 RMSE curves of different algorithms in constantvelocity target tracking

其次,比较各算法在kr=150%,ρ=4时随到达角观测噪声变化的跟踪性能,图3 给出了在σs∈{1◦,2◦,···,12◦}时的目标位置和速度估计的时均RMSE。可以看出,MLE 在观测噪声较小时偏差较小,但随着观测噪声增大,算法的跟踪性能出现大幅下降,且在 σs≥9◦时,算法开始发散。3D-BCKF由于偏差补偿项的准确性随观测噪声的增大快速下降,算法性能衰减较大,3D-IVKF 算法由于工具变量矩阵的构造依赖于3D-BCKF 的估计结果,也表现出较大的偏差,且随着到达角噪声的增大,3D-BCKF 的估计偏差增大,构造的工具变量矩阵准确性降低,3D-IVKF 的性能也出现下降。在σs≤2◦时,3D-UBKF 算法由于并未考虑观测站定位误差的影响存在较大的偏差,随着观测噪声增大,观测站定位误差的影响减小,3D-UBKF 与3D-MUBKF的性能趋于一致,这是因为随着观测噪声的增大,kr=150%时的观测站定位误差对伪线性噪声协方差的影响变小,观测噪声成为影响算法精度的主要因素。而3D-MUBKF 关于位置和速度估计的时均RMSE 明显小于3D-BCKF 和3D-IVKF,尤其在观测噪声较大时,偏差仍维持在较低水平。

图3 匀速目标跟踪中不同算法时均RMSE 曲线Fig.3 Time-averaged RMSE curves of different algorithms in constant-velocity target tracking

4.2.2 机动目标跟踪

文中考虑单运动观测站跟踪机动目标的问题,目标与观测站的轨迹如图4 所示。目标的初始位置为[-30,1 000,-300]Tm,在开始的80 次扫描中以[2,-3,1]Tm/s 的速度作匀速直线运动,接着在后续的40 次扫描中,以 [0.075,-0.15,0]Tm/s2的加速度作匀加速直线运动,最后以 [5,-9,1]Tm/s 的速度在80 次扫描内作匀速直线运动,由此构成了目标的匀速—匀加速—匀速的目标轨迹。观测站的初始位置为[-200,-300,-500]Tm,在xy平面上以与非机动场景相同的速度设置做Leg-by-Leg 的机动。连续观测的总时长为200 s,采样间隔T=1 s,过程噪声的功率谱密度qx=qy=qz=0.2 m2/s3。算法的初始状态服从均值为x0=[-30,1 000,-300,2,-3,1]T、协方差为P0|0=ρ2diag(102,102,102,0.52,0.52,0.252)的高斯分布,关于到达角观测噪声与观测站定位误差的设定与非机动场景一致。

图4 机动目标跟踪几何模型Fig.4 The geometric model for maneuvering target track

图5 给出了在 σs∈{1◦,2◦,···,12◦},kr=200%,ρ=4时,各算法关于机动目标位置和速度估计的时均RMSE 曲线。由于MLE 算法采用批处理,在目标机动时算法性能衰减严重,超出图片展示范围,此处不再展示。从图5 中可以看出,3D-MUBKF仍然明显优于3D-BCKF 和3D-IVKF,算法的估计偏差小且对观测噪声的变化不敏感。图6 给出了算法在 ρ=4,σs=2◦,kr从60%递进至200%时各算法关于位置估计值和速度估计值的时均RMSE曲线。可以看出,3D-BCKF 和3D-IVKF 的偏差一直处于较高水平,由于算法偏差受到偏差补偿准确性的影响,跟踪精度较低,对于观测站定位误差变化不敏感。3D-UBKF 在kr≤120%时偏差保持在较低水平,但随着观测站定位误差的增大,算法的性能出现明显下降,因为3D-UBKF 并未考虑定位误差的影响。而3D-MUBKF 由于考虑了观测站定位误差对伪线性噪声协方差矩阵的影响,算法的跟踪精确度更高且对于观测站定位误差的变化更稳定。值得注意的是,考虑到实际跟踪过程中,难以对目标的运动模型进行预知,因此在机动目标跟踪的仿真实验中,这4 种算法的滤波器预测模型仍然使用匀速运动模型,在模型失配的情况下,所提出的3D-MUBKF 相较于其他算法仍然具备明显优势。

图5 机动目标跟踪中不同算法时均RMSE 曲线(kr=200%,ρ=4)Fig.5 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (kr=200%,ρ=4)

图6 机动目标跟踪中不同算法时均RMSE曲线(ρ=4,σs=2◦)Fig.6 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (ρ=4,σs=2◦)

图7 给出了在 ρ ∈{1,2,···,15}、σs=2◦、kr=200%时不同算法关于机动目标位置和速度估计的时均RMSE 曲线。可以看出,3D-IVKF 和3D-UBKF的估计偏差随初始化误差水平的增大逐渐增大,而3D-MUBKF 及3D-UBKF 关于机动目标位置的估计误差对于初始化误差水平的变化不敏感,且始终保持在较低水平。

图7 机动目标跟踪不同算法时均RMSE 曲线(σs=2◦、kr=200%)Fig.7 Time-averaged RMSE curves of different algorithms in maneuvering target tracking (σs=2◦, kr=200%)

4.3 计算复杂度分析

由于到达角目标跟踪问题的非线性,MLE 算法不存在闭式解,需要通过GN 算法迭代求解数值解,且MLE 采用批处理,矩阵计算的维度高,这使得MLE 的计算复杂度明显高于基于PLKF 的4 种算法。文中通过计算浮点操作数的方式统计3DPLKF、3D-BCKF、3D-IVKF 和3D-MUBKF 等算法的计算复杂度。2 个浮点数的简单数学运算可用一次fflops表示,复杂的矩阵运算需要用相同计算复杂度的flops 进行等效,假设A为n×m维矩阵B为n×m维矩阵,C为n×n维矩阵,不同矩阵运算对应的浮点操作次数如表1 所示。

表1 不同矩阵运算对应的浮点操作次数Table 1 Number of flops corresponding to different matrix operations

根据以上条件可得4 种算法的等效计算复杂度如表2 所示,表中nx为目标状态向量的维数,mz为观测向量的维数。从表2 可以看出,复杂度由低至高可排列为3D-PLKF<3D-MUBKF<3DBCKF<3D-IVKF,总体来说,3D-PLKF 的计算复杂度最低,3D-MUBKF 由于伪线性噪声修正和噪声分离,计算复杂度有少量增加,3D-BCKF 由于偏差补偿计算复杂度增加,3D-IVKF 计算复杂度最高。在3D 到达角目标跟踪中,状态向量维数n取6,观测向量维数取2,将其代入计算复杂度的分析结果可得3D-PLKF、3D-BCKF、3D-IVKF、3DMUBKF 的复杂度比值为1∶1.09∶1.61∶1.07。可以看出,3D-MUBKF 以仅次于3D-PLKF 的计算复杂度实现了更高的跟踪精度。

表2 4 种算法计算复杂度Table 2 Computational complexity of four algorithms

5 结束语

由于PLKF 算法在3D 到达角目标跟踪中存在估计偏差,且未考虑观测站定位误差的影响,提出一种改进的UBKF 算法,即3D-MUBKF。该算法首先采用修正噪声协方差矩阵的措施来降低观测站定位误差的影响,进一步通过分离观测矩阵中的噪声,克服由观测矩阵和伪线性噪声相关性引起的偏差。仿真实验结果表明,相较于已有算法,3D-MUBKF 在非机动和机动场景下均具有性能优势,可以较少的计算复杂度实现更高的跟踪精度,且在观测噪声、观测站定位误差以及初始化误差较大时有效降低跟踪误差。

下一阶段为了扩大3D 到达角目标跟踪的应用范围,需要研究观测噪声和观测站定位误差等先验信息难以获得以及机动目标运动模型难以预知时的目标跟踪算法。