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

基于无偏伪线性卡尔曼滤波的 3D 到达角目标跟踪赵唯1,2,黄子豪1,2,郝程鹏1(1.中国科学院声学研究所,北京,100190;2.中国科学院大学电子电气与通信工程学院,北京,100049)摘要:在 3D 到达角目标跟踪研究中,伪线性卡尔曼滤波(PLKF)因计算复杂度低且对初始误差不敏感受到较大关注,但观测矩阵与噪声之间的相关性会使 PLKF 的目标状态估计存在一定偏差。针对这一问题并考虑观测站存在定位误差的实际情况,文中提出一种 3D 修正无偏 PLKF 算法。首先对方位角及俯仰角观测方程进行整体伪线性化,通过修正噪声协方差矩阵来降低观测站定位误差对跟踪精度的影响;其次通过分离观测矩阵中的噪声,降低由观测矩阵和观测噪声相关性引起的估计偏差。仿真分析结果表明,所提算法有效提高了 3D 到达角目标跟踪在非机动和机动 2 种场景下的精度,且具有较低的计算复杂度。关键词:目标跟踪;到达角;卡尔曼滤波;伪线性估计;定位误差中图分类号:TJ630.1;U666.7文献标识码:A文章编号:2096-3920(2023)05-0669-10DOI:10.11993/j.issn.2096-3920.2022-00073DAngleofArrivalTargetTrackingwithUnbiasedPseudo-LinearKalmanFilterZHAOWei1,2,HUANGZihao1,2,HAOChengpeng1(1.InstituteofAcoustic,ChineseAcademyofSciences,Beijing100190,China;2.SchoolofElectricalandCommunicationEngineering,UniversityofChineseAcademyofSciences,Beijing100049,China)Abstract:Intheresearchon3Dangleofarrivaltargettracking,pseudo-linearKalmanfilter(PLKF)hasreceivedgreatattentionduetoitslowcomputationalcomplexityandinsensitivitytoinitialerrors.However,thecorrelationbetweentheobservationmatrixandthenoisewillcauseacertaindeviationinthetargetstatesestimationofPLKF.Inviewofthisproblemandtheactualsituationthattheobservationstationhaspositioningerrors,a3D-modifiedunbiasedpseudo-linearKalmanfilter(3D-MUBKF)algorithmwasproposedinthispaper.Firstly,theoverallpseudo-linearizationoftheazimuthandelevationobservationequationswascarriedout,andtheinfluenceoftheobservationstationpositioningerrorsonthetrackingaccuracywasreducedbymodifyingthenoisecovariancematrix.Secondly,byseparatingthenoiseintheobservationmatrix,theestimation bias caused by the correlation between the observation matrix and the observation noise was reduced.Thesimulationanalysisresultsshowthattheproposedalgorithmeffectivelyimprovestheaccuracyof3Dangleofarrivaltargettrackinginbothnon-maneuveringandmaneuveringscenariosandhaslowcomputationalcomplexity.Keywords:targettracking;angleofarrival;Kalmanfilter;pseudo-linearestimation;positioningerror0引言水下到达角目标跟踪是仅通过观测站采集到的到达角信息来估计目标位置和速度等状态的技术,由于其无需观测站辐射信号,具有抗干扰性强、高隐蔽性的优点1-2,是水下目标跟踪重要的收稿日期:2022-07-07;修回日期:2022-08-17.基金项目:国家自然科学基金项目资助(61971412).作者简介:赵唯(1998-),女,在读博士,主要研究方向为目标跟踪及水下无线传感器网络.第31卷第5期水下无人系统学报Vol.31 No.52023年10月JOURNALOFUNMANNEDUNDERSEASYSTEMSOct.2023引用格式 赵唯,黄子豪,郝程鹏.基于无偏伪线性卡尔曼滤波的 3D 到达角目标跟踪 J.水下无人系统学报,2023,31(5):669-678.水下无人系统学报sxwrxtxb.xml-669方法之一。随着到达角测量的实现成本不断降低,到达角目标跟踪技术的适用范围不断扩大,现已广泛应用至导航定位、侦察监控、水下搜救及资源开发等民用和军用领域3-6。到达角目标跟踪是一类典型的非线性滤波问题,其最大的挑战在于角度观测值和目标状态之间的非线性关系7。针对 2D 空间中的到达角目标跟踪问题,Aidala8提出了伪线性卡尔曼滤波(pseudolinearKalmanfilter,PLKF)算法,PLKF 利用观测站与目标之间的几何关系将观测方程转换为伪线性形式,可直接应用线性卡尔曼滤波,具有结构简单、对初始误差不敏感的优点9,但 PLKF 对目标状态估计存在较大的偏差。随后,Ngugen 等10对 PLKF 的偏差进行了分析,认为偏差主要来源于伪线性方程中观测噪声和观测矩阵的相关性,并提出了基于偏差补偿的伪线性卡尔曼滤波(biascom-pensatedPLKF,BCKF)算法和基于工具变量的伪线性卡尔曼滤波(instrumentalvariablePLKF,IVKF)算法。其中 BCKF 对 PLKF 的估计结果进行了偏差补偿,IVKF 则利用 BCKF 的估计结果构造与观测噪声无关的工具变量矩阵,以此减少噪声和观测矩阵的相关性,进一步减小了估计偏差。但随着观测噪声的增加,偏差补偿项的准确性降低,使得BCKF 的估计偏差明显增大,由于工具变量矩阵的构造依赖于 BCKF 的估计结果,IVKF 算法也易受到 BCKF 偏差增大的影响,工具变量矩阵构造的准确性降低,跟踪性能也随之下降。对此,Huang等11提出了噪声分离的方法,通过代数运算直接构造一个不含噪声的观测矩阵,提出一种无偏伪线性卡尔曼滤波(unbiasedPLKF,UBKF)算法,有效提高了 2D 空间中目标跟踪的精度。由于目标多在 3D 空间运动,实际应用意义更高的 3D 无源到达角目标跟踪也得到了发展。Ngugen 等12提出了基于工具变量的 3D 伪线性卡尔曼滤波(3D-instrumentalvariablePLKF,3D-IVKF)算法,将 2D-IVKF 拓展到 3D 空间。与 2D-IVKF 类似,3D-IVKF的跟踪精度会随观测噪声增大迅速下降,且由于维度增加,算法的复杂度较高。在实际应用中,到达角目标跟踪面临的另一个挑战是观测站定位误差导致的估计性能下降,这是因为在目标匀速直线或匀加速运动假设下,为保证目标运动参数可观测,需要观测站进行机动运动,而机动观测站的位置信息依赖于定位系统且往往存在定位误差13。针对这一问题,文献14-16利用位置已知的校准源来降低观测站定位误差对到达时间差(timedifference-of-arrival,TDOA)目标定位的影响,虽然定位精度有所提高,但校准源的使用增加了定位系统的复杂度。Farina17通过对观测方程线性化,得到与观测站定位误差和观测噪声相关的近似噪声协方差矩阵,提出了一种最大似然估计器(maximumlikelihoodestimator,MLE)。Pang等18利用观测站定位误差的统计特性建立了新的 2D 到达角偏差补偿算法。以上 2 种算法均采用批处理方式,不仅计算复杂度高且需要目标的状态模型为确定系统,难以应对存在过程噪声或目标机动的跟踪问题。为解决上述问题,文中提出了一种 3D 修正无偏伪线性卡尔曼滤波(3D-modifiedUBKF,3D-MUBKF)算法。该算法考虑了观测站定位误差对方位角及俯仰角观测方程伪线性化的影响,通过修正伪线性噪声的协方差矩阵降低观测站定位误差的影响,接着对观测方程进行整体噪声分离,以此降低观测矩阵与观测噪声之间的相关性引起的偏差。仿真分析表明,3D-MUBKF 算法相较于已有算法在机动和非机动 2 种场景下表现更优,在观测噪声和观测站定位误差较大时能够显著降低跟踪误差,且计算复杂度低。1系统模型1.1目标状态模型pk=px,k,py,k,pz,kTvk=vx,k,vy,k,vz,kTxk=pkT,vkTT=px,k,py,k,pz,k,vx,k,vy,k,vz,kT考虑使用单运动观测站对方位角和俯仰角进行观测的 3D 到达角目标跟踪问题。在观测时刻k 处,目标位置,速度,均为待估计未知参数,目标的状态向量,状 态方程可表示为xk=Fxk1+wk(1)Fwk0QkFQk式中:为状态转换矩阵;为均值为、协方差为的独立高斯过程噪声,代表目标在运动过程中的干扰,会使目标发生轻微的未知机动。假设使用常速度模型来对目标运动进行建模,则和可表示为2023年10月水下无人系统学报第31卷670JournalofUnmannedUnderseaSystemssxwrxtxb.xml-F=I33TI33033I33(2)Qk=T33Q0T22Q0T22Q0TQ0(3)0mnmnInnnnQ0=diag(qx,qy,qz)qxqyqz式 中:表 示的 零 矩 阵;表 示的单位矩阵;T 为采样间隔;,及分别代表过程噪声在 x,y 及 z 轴上的功率谱密度分量。1.2观测模型kkk 时刻测量的含噪方位角和含噪俯仰角可表示为k=k+n,k,k=h,k(xk)=arctanpy,kry,kpx,krx,k(4)k=k+n,k,k=h,k(xk)=arcsinpz,krz,kpkrk(5)kkrk=rx,k,ry,k,rz,kTn,kn,k2,k2,kh,k()h,k()kk2,k2,k2,k2,k式中:,分别为方位角和俯仰角真实值;为观测站位置真实值;观测噪声,均为零均值的高斯白噪声,且其协方差分别为和;,分别代表,与目标状态的非线性关系,代表欧式范数。此处假设和为已知的先验信息,在实际应用中和可在到达角估计过程中得到18。k 时刻不确定的观测站的位置可表示为 rk=rk+mkmk N(0,Qr,k),Qr,k=2x,k0002y,k0002z,k(6)rk=rx,k,ry,k,rz,kTmk=mx,k,my,k,mz,kT0Qr,kQr,kn,kn,kmkwk1式中:为可得的观测站位置;观测站定位误差为均值为,协方差为的独立高斯噪声,此处假设为观测站定位中可得的先验信息19,且假设在任意时刻,和之间相互独立。则 k 时刻的观测方程为hk=hk(xk)+nk(7)hk=k,kThk(xk)=h,k(xk),h,k(xk)Tnk=n,k,n,kT式中:为观测向量;为目标状态的非线性函数;噪声向量。3D 无源到达角目标跟踪的状态空间模型由式(1)和式(7)构成,跟踪算法即在获得初始估计以及初始协方差的条件下,利用状态方h0,h1,hkxk程(1)以及历史观测值对目标状态进行迭代估计。2PLKF 分析针对 3D 到达角目标跟踪问题,利用等价代数运算将观测方程转换为伪线性形式是一种简单高效的滤波方式,但伪线性化会使目标的状态估计出现偏差。文中推导存在观测站定位误差情况下的 3D-PLKF,并分析观测站定位误差对 3D-PLKF的主要影响以及估计偏差的主要来源。2.1存在观测站定位误差的 3D-PLKF首先对存在观测站定位误差的到达角观测方程进行伪线性化,方位角的观测方程通过等价代数运算可以转换为sink(px,krx,k)=cosk(py,kry,k)(8)考虑实际情况中的方位角观测噪声和观测站定位误差后,式(8)可写为,k=sink(px,k rx,k)cosk(py,k ry,k)(9)u,k=sink,cosk,0T令,则可将式(9)整理为伪线性形式z,k=H,kxk+,k(10)z,k=uT,k rkH,k=uT,kMM=I33033,kcosn,k 1sinn,k n,k式 中:;,;此处伪线性噪声为方位角观测噪声以及观测站定位误差导致的残差。在观测噪声较小时,满足以及的近似关系,则可以得到以下近似表达式sink=sin(k+n,k)sink+n,kcoskcosk=cos(k+n,k)coskn,ksink(11)将式(6),式(11)代入式(9)得,k=sink(px,krx,k)cosk(py,kry,k)sinkmx,k+coskmy,k+n,kcosk(px,krx,k)+sink(py,kry,k)n,k(coskmx,k+sinkmy,k)(12)cosk(px,krx,k)+sink(py,kry,k)=dkcosksink(px,krx,k)=cosk(py,kry,k)根据几何关系和,并省略2 阶以上的噪声项,式(12)可以简化为,k=n,kdkcosk+uT,kmk(13)dk=pkrku,k=sink,cosk,0T式中:为目标到观测站的距离;。2023年10月赵唯,等:基于无偏伪线性卡尔曼滤波的 3D 到达角目标跟踪第5期水下无人系统学报sxwrxtxb.xml-671采用同样的方法对俯仰角观测方程进行伪线性化,式(5)可以写为sinkcosk(px,krx,k)+sinksink(py,kry,k)=cosk(pz,krz,k)(14)考虑俯仰角观测噪声以及观测站定位误差,式(14)可写为,k=sinkcosk(px,k rx,k)+sinksink(py,k ry,k)cosk(pz,k rz,k)(15)u,k=sinkcosk,sinksink,coskT令,俯仰角观测方程的伪线性形式可写为z,k=H,kxk+,k(16)H,k=uT,kM z,k=uT,k rk,kcosn,k 1sinn,k n,k式中:;。接下来使用俯仰角观测噪声和观测站定位误差对伪线性噪声进行表示,使用和的近似关系,可以得到含噪俯仰角满足sink=sin(k+n,k)sink+n,kcoskcosk=cos(k+n,k)coskn,ksink(17)d=coskcosk(px,krx,k)+cosksink(py,kry,k)+sink(pz,krz,k)将式(6),式(11)及式(17)代入式(16),并根据式(8)、式(14)及几何关系,可将伪线性噪声表示为,k=n,kdk+uT,kmk(18)u,k=sinkcosk,sinksink,coskT式中,。此时,k 时刻观测方程的伪线性形式可写为zk=z,kz,k=Hkxk+k=H,kH,kxk+,k,k(19)kRk=EkTk=diag(R,k,R,k)R,kd2k2,kcos2k+uT,kQr,ku,kR,k d2k2,k+uT,kQr,ku,k且在观测噪声较小的情况下,伪线性噪声的协方差,其中,。式(19)已为线性形式,因此可直接应用线性卡尔曼滤波,得到存在观测站定位误差时的 3D-PLKF,具体步骤为 xk|k1=F xk1|k1(20)Pk|k1=FPk1|k1FT+Qk(21)Kk=Pk|k1HTk(Rk+HkPk|k1HTk)1(22)xk|k=xk|k1+Kk(zkHk xk|k1)(23)Pk|k=Pk|k1KkHkPk|k1(24)2.2偏差分析参考文献 10 中的推导方法,存在观测站定位误差时的 3D-PLKF 的估计偏差可表示为ek=E xk|kxk=EAk+EBk+ECk(25)Ak=Pk|kP1k|k1F(xk1|k1xk1)(26)Bk=Pk|kP1k|k1wk1(27)Ck=Pk|kHTkR1kk(28)EAkEAkEBkPk|kwk1HkkHkkECk,0Hkk式中:代表前一采样时刻偏差的传递,不是偏差产生的直接原因,若其余 2 项偏差等于 0,则可忽略;代表由和的相关性引起的偏差,在过程噪声较小时可忽略;由于观测矩阵和伪线性噪声均与到达角观测噪声有关,和的相关性不可忽略,。因此,观测矩阵和伪线性噪声之间的相关性是 PLKF 算法存在偏差的主要原因。33D-MUBKF 算法由 2.2 节的偏差分析可知,PLKF 算法的估计偏差主要来源于观测矩阵与伪线性噪声的相关性,接下来对伪线性方程(19)进行噪声分离以消除观测矩阵与伪线性噪声的相关性。利用观测噪声较小时的近似关系,可将式(19)中的观测矩阵改写为H,k=sink,cosk,0M sink,cosk,0M+n,kcosk,sink,0M(29)H,k=sinkcosk,sinksink,coskM sinkcosksinksinkcoskTM+n,kcoskcoskcosksinksinkTM(30)将式(29)和式(30)代入式(19)中,并忽略 2 阶以上的噪声项,得到改写后的伪线性方程为z,ksink,cosk,0Mxk+cosk,sink,0Mxkn,k+dkcoskn,k+uT,kmk(31)z,ksinkcosk,sinksink,coskMxk+coskcosk,cosksink,sinkMxkn,k+dkn,k+uT,kmk(32)t,k=cosk,sink,0Mxk+dkcoskt,k=令,2023年10月水下无人系统学报第31卷672JournalofUnmannedUnderseaSystemssxwrxtxb.xml-coskcosk,cosksink,sinkMxk+dk,可得噪声分离后的伪线性方程 zk=z,k z,k=H,kH,kxk+n,k n,k=Hkxk+nk(33)z,k=z,k/t,k z,k=z,k/t,kH,k=sink,cosk,0M/t,k,H,k=sinkcosk,sinksink,coskM/t,k nk=n,k+(uT,kmk/t,k)n,k+uT,kmk/t,kTuT,ku,k=0Rk=E nk nTk=diag(2,k+c,k,2,k+c,k)c,k=uT,kQr,ku,k/t2,kc,k=uT,kQr,ku,k/t2,k式 中:伪 线 性 观 测 值,;观 测 矩 阵;此时伪线性 噪 声,由于且方位角、俯仰角观测噪声独立,协方差矩阵,其中,。根据式(25),PLKF 算法的偏差可写为ek ECk=EPk|kHTkR1k nk=EPk|kHT,k n,k2,k+c,k+HT,k n,k2,k+c,k(34)可以看出,经过噪声分离后,式(34)等于 0。以式(1)和式(33)为状态空间,得 3D-MUBKF,其具体步骤为 xk|k1=F xMUBk1|k1(35)Pk|k1=FPMUBk1|k1FT+Qk(36)Kk=Pk|k1HTk(Rk+HkPk|k1HTk)1(37)xMUBk|k=xk|k1+Kk(zkHk xk|k1)(38)PMUBk|k=Pk|k1KkHkPk|k1(39)kkt,kt,ku,ku,k3Dk3Dkd3Dk由于真实到达角、以及观测站与目标之间的真实距离未知,无法得到、以及的真实值,可利用状态预测值计算、和,对待求真实值进行近似估计,即d3Dk=?xk|k1(1:3)rk?(40)3Dk=arctan xk|k1(2)ry,k xk|k1(1)rx,k(41)3Dk=arcsin xk|k1(3)rz,kd3Dk(42)c,k=c,k=0根据以上分析可知,3D-MUBKF 通过修正卡尔曼滤波过程中的噪声协方差矩阵来降低观测站定位误差对算法的影响,当时,3D-MUBKF 将退化为未修正观测站定位误差的 3D-UBKF 算法。4仿真分析4.1评价标准文中通过蒙特卡洛仿真实验验证 3D-MUBKF算法对匀速目标及机动目标的跟踪性能,并与 3D-UBKF、3D-BCKF、3D-IVKF 以及文献 17 中考虑了观测站定位误差的 MLE 进行比较。MLE 的数值解通过高斯牛顿(Gauss-Newton,GN)算法20迭代计算,初始迭代值设置为伪线性估计器(pseudolinearestimator,PLE)20的估计值,GN 算法迭代10 次后得到 MLE 的数值解。仿真实验以均方根误差(rootmeansquarederror,RMSE)、时均 RMSE和计算复杂度为性能指标,且 RMSE 定义为Rposk=1McMi=1?pik|k pik?1/2Rvelk=1McMi=1?vik|kvik?1/2(43)时均 RMSE 定义为Rposavg=1McUMi=1Nk=L?pik|k pik|k?1/2Rvelavg=1McUMi=1Nk=L?vik|kvik|k?1/2(44)pik pik|kvik vik|kU=NL+1Mc式中:和分别为第 i 次蒙特卡洛实验中 k 时刻目标的真实位置矢量和算法的估计位置矢量;和分别为第 i 次蒙特卡洛实验中 k 时刻目标的真实速度矢量和算法的估计速度矢量;,N 为跟踪过程的总时刻数,L 为补偿参数,用于减少初始跟踪误差的影响;是蒙特卡洛仿真实验次数,文中 N 设定为 200,L 设定为 10,M 设定为1000。4.2仿真结果4.2.1匀速目标跟踪仿真实验首先考虑单运动观测站跟踪匀速运动目标的跟踪问题,目标与观测站的轨迹如图 1所示。30,600,300T1,6,1T目标的初始位置为m,速度为m/s,观测站采取 Leg-by-Leg 机动方式,第2023年10月赵唯,等:基于无偏伪线性卡尔曼滤波的 3D 到达角目标跟踪第5期水下无人系统学报sxwrxtxb.xml-67350,200,500T4,0.6,0T4,0.6,0T4,0.6,0TT=1qx=qy=qz=0.07 x0|0 x0=30,600,300,1,6,1TP0|0=2diag(102,102,102,0.52,0.52,0.252)2,k=2,2,k=2Qr,k=diag(2x,2y,2z)s=kr=(N1)x/(|vr,x|T)100%=(N1)y/(|vr,y|T)100%(N1)z/(|vr,z|T)vr=vr,x,vr,y,vr,zT1 段从m 处开始,以m/s的速度运动 70s,第 2 段以m/s 的速度运动 70s,第 3 段以m/s 的速度运动 60s,连续观测的总时长为 200s,采样间隔s,过程噪声的功率谱密度m2/s3。算法的初始状态服从均值为、协方差为的高斯分布,其中,变量 控制目标状态初始协方差的大小,可用于评估各个算法对初始化误差的敏感程度。假设到达角的观测噪声以及观测站定位误差均为独立同分布的高斯白噪声,其协方差且。文中使用量化到达角观测噪声,使用观测站定位误差的标准差与两相邻采样时刻间观测站的运动距离之比 kr量化观测站的定位误差19:=100%,观测站的速度向量。s=4kr=150%=4Qr,k=diag(36,0.81,0)k=N/T首先,比较各算法在,时的跟踪情况,此时观测站定位误差的方差。图 2 给出了 MLE、3D-BCKF、3D-IVKF、3D-UBKF、3D-LEUBKF 位置和速度估计的 RMSE,其中跟踪时刻。可以看出,MLE在位置估计上偏差较小,但对于速度的估计存在较大偏差。3D-UBKF 和 3D-MUBKF 较 3D-BCKF和 3D-IVKF 估计偏差有大幅提升,且 3D-MUBKF由于考虑了观测站位置误差对算法的影响,跟踪精度更高。kr=150%=4其次,比较各算法在,时随到达s1,2,12s 9s2角观测噪声变化的跟踪性能,图 3 给出了在时的目标位置和速度估计的时均RMSE。可以看出,MLE 在观测噪声较小时偏差较小,但随着观测噪声增大,算法的跟踪性能出现大幅下降,且在时,算法开始发散。3D-BCKF由于偏差补偿项的准确性随观测噪声的增大快速下降,算法性能衰减较大,3D-IVKF 算法由于工具变量矩阵的构造依赖于 3D-BCKF 的估计结果,也表现出较大的偏差,且随着到达角噪声的增大,3D-BCKF 的估计偏差增大,构造的工具变量矩阵准确性降低,3D-IVKF 的性能也出现下降。在时,3D-UBKF 算法由于并未考虑观测站定位误差的影响存在较大的偏差,随着观测噪声增大,观测站定位误差的影响减小,3D-UBKF 与 3D-MUBKF的性能趋于一致,这是因为随着观测噪声的增大,5002 0004001 400700300z/m200y/m400 x/m400100100600 200观测站运动轨迹目标运动轨迹图1匀速目标跟踪的几何模型Fig.1Geometricmodelofconstant-velocitytargettracking050100150200k100200300400500600700800Rk /mRk /(m/s)050100150200k(a)位置 RMSE 曲线(b)速度 RMSE 曲线234563D-UBKF3D-MUBKF3D-IVKF3D-BCKFMLEvelpos图2匀速目标跟踪中不同算法 RMSE 曲线Fig.2RMSE curves of different algorithms in constant-velocitytargettracking2023年10月水下无人系统学报第31卷674JournalofUnmannedUnderseaSystemssxwrxtxb.xml-kr=150%时的观测站定位误差对伪线性噪声协方差的影响变小,观测噪声成为影响算法精度的主要因素。而 3D-MUBKF 关于位置和速度估计的时均 RMSE 明显小于 3D-BCKF 和 3D-IVKF,尤其在观测噪声较大时,偏差仍维持在较低水平。(a)位置时均 RMSE 曲线(b)速度时均 RMSE 曲线24681012s/()24681012s/()02004006008003D-UBKF3D-MUBKF3D-IVKF3D-BCKFMLE02468Ravg/mposRavg/(m/s)vel图3匀速目标跟踪中不同算法时均 RMSE 曲线Fig.3Time-averagedRMSEcurvesofdifferentalgorith-msinconstant-velocitytargettracking4.2.2机动目标跟踪30,1 000,300T2,3,1T0.075,0.15,0Tm/s25,9,1T200,300,500T文中考虑单运动观测站跟踪机动目标的问题,目标与观测站的轨迹如图 4 所示。目标的初始位置为m,在开始的 80 次扫描中以m/s 的速度作匀速直线运动,接着在后续的 40 次扫描中,以的加速度作匀加速直线运动,最后以m/s 的速度在 80 次扫描内作匀速直线运动,由此构成了目标的匀速匀加速匀速的目标轨迹。观测站的初始位置为m,在 xy 平面上以T=1qx=qy=qz=0.2 x0|0 x0=30,1 000,300,2,3,1TP0|0=2diag(102,102,102,0.52,0.52,0.252)与非机动场景相同的速度设置做 Leg-by-Leg 的机动。连续观测的总时长为 200s,采样间隔s,过程噪声的功率谱密度m2/s3。算法的初始状态服从均值为、协方差为的高斯分布,关于到达角观测噪声与观测站定位误差的设定与非机动场景一致。5001 000400300z/m200800500100y/m500 x/m2000100500 400观测站运动轨迹目标运动轨迹图4机动目标跟踪几何模型Fig.4Thegeometricmodelformaneuveringtargettracks 1,2,12 kr=200%=4=4 s=2krkr120%图 5 给出了在,时,各算法关于机动目标位置和速度估计的时均 RMSE 曲线。由于 MLE 算法采用批处理,在目标机动时算法性能衰减严重,超出图片展示范围,此处不再展示。从图 5 中可以看出,3D-MUBKF仍然明显优于 3D-BCKF 和 3D-IVKF,算法的估计偏差小且对观测噪声的变化不敏感。图 6 给出了算法在,从 60%递进至 200%时各算法关于位置估计值和速度估计值的时均 RMSE曲线。可以看出,3D-BCKF 和 3D-IVKF 的偏差一直处于较高水平,由于算法偏差受到偏差补偿准确性的影响,跟踪精度较低,对于观测站定位误差变化不敏感。3D-UBKF 在时偏差保持在较低水平,但随着观测站定位误差的增大,算法的性能出现明显下降,因为 3D-UBKF 并未考虑定位误差的影响。而 3D-MUBKF 由于考虑了观测站定位误差对伪线性噪声协方差矩阵的影响,算法的跟踪精确度更高且对于观测站定位误差的变化更稳定。值得注意的是,考虑到实际跟踪过程中,难以对目标的运动模型进行预知,因此在机动目标跟踪的仿真实验中,这 4 种算法的滤波器预测2023年10月赵唯,等:基于无偏伪线性卡尔曼滤波的 3D 到达角目标跟踪第5期水下无人系统学报sxwrxtxb.xml-675模型仍然使用匀速运动模型,在模型失配的情况下,所提出的 3D-MUBKF 相较于其他算法仍然具备明显优势。1,2,15s=2kr=200%图 7 给 出 了 在、时不同算法关于机动目标位置和速度估计的时均RMSE 曲线。可以看出,3D-IVKF 和3D-UBKF的估计偏差随初始化误差水平的增大逐渐增大,而 3D-MUBKF 及 3D-UBKF 关于机动目标位置的估计误差对于初始化误差水平的变化不敏感,且始终保持在较低水平。4.3计算复杂度分析由于到达角目标跟踪问题的非线性,MLE 算法不存在闭式解,需要通过 GN 算法迭代求解数值解,且 MLE 采用批处理,矩阵计算的维度高,这使得 MLE 的计算复杂度明显高于基于 PLKF 的 4 种(a)位置时均 RMSE 曲线(b)速度时均 RMSE 曲线100200300400500600700800234567891024681012s/()24681012s/()Ravg/mposRavg/(m/s)vel3D-UBKF3D-MUBKF3D-IVKF3D-BCKFkr=200%=4图5机动目标跟踪中不同算法时均 RMSE 曲线(,)kr=200%=4Fig.5Time-averagedRMSEcurvesofdifferentalgorith-msinmaneuveringtargettracking(,)(a)位置时均 RMSE 曲线(b)速度时均 RMSE 曲线6080100120140160180200kr/%6080100120140160180200kr/%1002003004005006003D-UBKF3D-MUBKF3D-IVKF3D-BCKF2345678Ravg/mposRavg/(m/s)vel=4s=2图6机动目标跟踪中不同算法时均 RMSE 曲线(,)=4 s=2Fig.6Time-averagedRMSEcurvesofdifferentalgorith-msinmaneuveringtargettracking(,)(a)位置时均 RMSE 曲线(b)速度时均 RMSE 曲线6080100120140160180200kr/%6080100120140160180200kr/%1002003004005006003D-UBKF3D-MUBKF3D-IVKF3D-BCKF2345678Ravg/mposRavg/(m/s)vel(a)位置时均 RMSE 曲线(b)速度时均 RMSE 曲线Ravg/mpos1357911131513579111315020040060080010003D-UBKF3D-MUBKF3D-IVKF3D-BCKF03691215Ravg/(m/s)vel2023年10月水下无人系统学报第31卷676JournalofUnmannedUnderseaSystemssxwrxtxb.xml-AnmBnmCnn算法。文中通过计算浮点操作数的方式统计 3D-PLKF、3D-BCKF、3D-IVKF 和 3D-MUBKF等算法的计算复杂度。2 个浮点数的简单数学运算可用一次 fflops表示,复杂的矩阵运算需要用相同计算复杂度的 flops 进行等效,假设为维矩阵为维矩阵,为维矩阵,不同矩阵运算对应的浮点操作次数如表 1 所示。表1不同矩阵运算对应的浮点操作次数Table1Numberofflopscorrespondingtodifferentmatrixoperations矩阵运算浮点A AnmAB2nml-nlC1n3根据以上条件可得 4 种算法的等效计算复杂度如表 2 所示,表中 nx为目标状态向量的维数,mz为观测向量的维数。从表 2 可以看出,复杂度由 低 至 高 可 排 列 为 3D-PLKF3D-MUBKF3D-BCKF3D-IVKF,总体来说,3D-PLKF 的计算复杂度最低,3D-MUBKF 由于伪线性噪声修正和噪声分离,计算复杂度有少量增加,3D-BCKF 由于偏差补偿计算复杂度增加,3D-IVKF 计算复杂度最高。在 3D 到达角目标跟踪中,状态向量维数 n 取6,观测向量维数取 2,将其代入计算复杂度的分析结 果 可 得 3D-PLKF、3D-BCKF、3D-IVKF、3D-MUBKF 的复杂度比值为 11.091.611.07。可以看出,3D-MUBKF 以仅次于 3D-PLKF 的计算复杂度实现了更高的跟踪精度。表24 种算法计算复杂度Table2Computationalcomplexityoffouralgorithms滤波步骤3D-PLKF3D-BCKF3D-IVKF3D-MUBKF xk|k1预测2nx2nx2nx2nx2nx2nx2nx2nxPk|k1预测4nx3nx24nx3nx24nx3nx24nx3nx2伪线性化5mznx+5mz5mznx+5mz5mznx+5mz12mznx+25mz1Kk6nx2mz+2nxmz23mznx+mz36nx2mz+2nxmz23mznx+mz312nx2mz+4nxmz2+10mznx+2mz36nx2mz+2nxmz23mznx+mz3 xk|k估计4mznx9mznx+2nx2nx13mznx+2nx2nx4mznxPk|k估计2nx2mz+2nx22nx2mz+2nx24nx2mz+4nx22nx2mz+2nx2总复杂度4nx3+3nx2+8nx2mz+2nxmz2+6mznxnx+5mz+mz34nx3+5nx2+8nx2mz+2nxmz2+11mznx2nx+5mz+mz34nx3+7nx2+16nx2mz+4nxmz2+28mznx2nx+5mz+2mz34nx3+3nx2+8nx2mz+2nxmz2+13mznxnx+25mz+mz315结束语由于 PLKF 算法在 3D 到达角目标跟踪中存在估计偏差,且未考虑观测站定位误差的影响,提出一种改进的 UBKF 算法,即 3D-MUBKF。该算法首先采用修正噪声协方差矩阵的措施来降低观测站定位误差的影响,进一步通过分离观测矩阵中的噪声,克服由观测矩阵和伪线性噪声相关性引起的偏差。仿真实验结果表明,相较于已有算法,3D-MUBKF 在非机动和机动场景下均