基于反馈判决的鲁棒自适应机动目标跟踪算法

汪家宝，陈树新，吴 昊，何仁珂，郝思冲

(1.空军工程大学信息与导航学院，西安，710077； 2.93184部队，北京，100076)

机动目标跟踪系统在导航制导、军事打击等领域有着广泛的应用，然而针对干扰源目标，采用主动式的探测跟踪易于暴露己方所在位置，被动的纯方位无源跟踪(bearings-only tracking，BOT)不主动发射信号，因而具有良好的隐蔽性[1-4]。

纯方位跟踪需解决的问题本质上是一种基于信息非完整获取条件下的非线性估计问题。为此，学者们相继提出扩展卡尔曼滤波(extended Kalman filter，EKF)[5]、无迹卡尔曼滤波(unscented Kalman filter，UKF)[6-7]、容积卡尔曼滤波(cubature Kalman filter，CKF)[8-9]等非线性滤波算法。EKF将非线性方程线性化进行一阶近似，但在系统非线性程度较高时会引起较大的截断误差。UKF和CKF都是通过确定性采样策略来近似状态的后验概率密度，相比而言，CKF和UKF均能达到三阶估计精度，但CKF所用的采样点比UKF少,且其数值稳定性优于UKF[10]。文献[11]提出了平方根容积卡尔曼滤波算法(square root CKF，SRCKF)，通过传递状态协方差的平方根，提高了滤波数值稳定性。上述算法只有构建的系统模型与实际情况较为匹配，且噪声统计特性服从高斯分布时，才会取得较好的滤波特性，而在实际情况中不能完全满足要求。

针对系统建模失准问题，周东华等人[12]将单渐消因子引入卡尔曼滤波算法，形成强跟踪滤波器(strong tracking filter，STF)，充分利用了残差序列的有效信息，对模型建构失准、过程参数变动具有较强的自适应性。文献[13]利用多重渐消因子对估计均方误差各通道分别加权修正，相比单渐消因子更能提高渐消滤波的自适应调节能力。在实际系统中，传感器获得的观测信息由于存在外部干扰或者仪器本身故障可能会出现野值，从而导致滤波发散。Huber函数通过引入权函数，降低了受野值干扰的观测值的权重，有效地抑制野值对滤波造成的影响[14]。文献[15]提出了基于广义M估计的鲁棒CKF算法MR-CKF，引入马氏距离作为异常误差的判别量，利用三段等价权函数合理地处理大小异常误差，提高了CKF的鲁棒性。上述的研究仅针对一种异常情形，为同时处理非线性系统的模型失准和观测异常问题，需兼顾抗差能力和自适应性，但这两种需求往往存在矛盾，不能同时满足[16]。文献[17]提出了一种鲁棒多渐消因子CKF(robust multiple fading factors CKF，RMCKF)，利用Huber函数构造修正的测量噪声协方差，并以此为前提计算多渐消因子抑制滤波异常，RMCKF可以减小动力学模型误差和测量模型误差的影响。

为了更好地解决系统出现的模型失准和观测异常问题，本文提出了一种基于反馈判决的鲁棒自适应SRCKF算法(feedback decision based robust adaptive SRCKF，FRA-SRCKF)，在每一次迭代过程中，以基于测量残差的马氏距离作为异常误差的判别因子，一旦有滤波异常存在，滤波器拆分为鲁棒滤波器和自适应滤波器，分别作用至下一时间步生成判别因子后进行二次判断，从而确定上一滤波周期所需的滤波器类型，达到反馈判决的效果。

1 机动目标跟踪系统模型和SRCKF算法

1.1 系统模型

所构建的目标相对运动系统方程可表示成：

xk=Fxk-1-uk-1+wk-1

(1)

式中：xk∈nx，nx为状态向量维数；过程噪声wk-1是满足均值为零、协方差为Qk-1的高斯白噪声。转移矩阵F、确定性输入uk-1以及Qk-1的表达式分别为:

(2)

(3)

Qk-1=ΓΓT·q

(4)

式中：q为过程噪声强度；Γ是过程噪声转移矩阵，可表示为:

(5)

双观测站纯方位跟踪的量测方程为：

(6)

1.2 平方根容积卡尔曼滤波

1.2.1 初始化

初始化状态向量x0|0，误差协方差P0|0并对其进行Cholesky分解，获得误差协方差的平方根更新初值S0|0,即：

S0∣0=[chol(P0|0)]T

(7)

1.2.2 时间更新

1)利用上一时刻Sk-1|k-1计算容积点并进行容积点的传播：

(8)

(9)

式中：m=2nx。

2)计算状态一步预测值及预测误差协方差的平方根：

(10)

(11)

其中：

(12)

(13)

1.2.3 量测更新

1)基于Sk|k-1再次计算容积点并进行容积点的传播：

(14)

(15)

2)估计预测量测及新息协方差阵的平方根：

(16)

(17)

其中：

(18)

(19)

3)估计互协方差矩阵：

Pxz,k|k-1=γ′k|k-1(Z′k|k-1)T

(20)

(21)

4)计算滤波增益：

(22)

5)更新状态估计值：

(23)

6)估计状态协方差的平方根：

(24)

2 基于反馈判决的鲁棒自适应SRCKF算法

2.1 鲁棒SRCKF和自适应滤波算法

首先讨论测量噪声存在异常的情况。观测值受异常分布噪声影响或出现野值时，以上算法性能下降，即鲁棒性不强。为此，可引入M估计方法，利用Huber代价函数，针对大残差提供l1范数来约束残差信息的权重。Huber等价权函数可表示为[15]：

(25)

(26)

下面讨论系统模型与实际模型不匹配的情况。SRCKF算法需要较为精确的动态模型，一旦动态模型建模失准，滤波器容易发散。针对这一现象，利用STF的正交性原理，可以得到基于SRCKF的强跟踪滤波算法，其通过设计渐消因子λk实时调整预测误差协方差Pk|k-1，使残差序列保持正交以快速跟踪系统的真实状态。强跟踪滤波算法应使得以下条件成立：

(27)

(28)

将单渐消因子替换为多重渐消因子，以对预测误差协方差的多个通道分别进行调节，这样调整后预测误差协方差平方根的计算式(11)可改写为：

(29)

多重渐消因子矩阵Λk的计算应满足：

(30)

式中：β为弱化因子，其目的是为了防止造成过调节，一般取β≥1。Vk为残差序列协方差的估计值，其计算方法为：

(31)

式中：ρ为ek的遗忘因子，0<ρ≤1，一般取ρ=0.95。

(32)

(33)

假设Λk=diag[λk,1,λk,2,…,λk,nx]，同时考虑到Hk仅仅与目标跟踪位置信息相关，多重渐消因子计算方式如下：

(34)

式中：tr(·)表示矩阵(·)的求迹运算。

以上基于SRCKF的强跟踪滤波算法称为自适应SRCKF算法(adaptive SRCKF，ASRCKF)。

2.2 滤波异常判别与反馈判决

上述无论是鲁棒方法还是自适应方法，都是基于残差进行处理，只能有效解决量测异常和模型失准中的一种问题。为了区分正常数据和异常数据，本文构建基于量测残差的Mahalanobis距离作为滤波异常的判别因子

(35)

τk反映了真实测量值与预估测量值之间的偏离程度，若εk服从标准正态分布，则有：

τk～χ2(nz)

(36)

式中：nz为量测向量的维数，nz=2。

选择显著水平α=0.05，定义χ2(2)的上分位点为δα，于是有：

p(τk>δα)=α

(37)

表示在正常滤波情况下，τk大于δα的概率只有5%。此时，δα就可以作为阈值对每次计算的判别因子进行滤波是否正常的检验。

研究表明，单独使用马氏距离作为判别因子不足以反映滤波异常的来源，模型失准和量测异常都可能使τk超出阈值，后续仿真实验会对此进一步验证。由于2种误差的解决方案具有矛盾性(前者强化观测信息，后者弱化观测信息)，因此不能同时使用。本文的解决思路是当滤波出现异常时，滤波器分解为鲁棒滤波器和自适应滤波器，分别作用至下一时刻生成判别因子，然后比较两个判别因子的大小，从而确定上一滤波周期所需的滤波器类型，此方法能够选择合适的算法对滤波异常进行处理。FRA-SRCKF的流程见图1，其中N为总采样次数。

图1 FRA-SRCKF流程图

如图1所示，若在k时刻，τk不大于阈值，则继续按照SRCKF进行滤波处理；否则，SRCKF分解为RSRCKF和ASRCKF两种子滤波器。然后各自按照对应的滤波步骤并行更新(分别继续k时刻的量测更新以及k+1时刻的时间更新)，进而能够在k+1时刻，获得子滤波器的滤波异常判别因子τk+1,RSRCKF和τk+1,ASRCKF。若τk+1,RSRCKF≤τk+1,ASRCKF，则在k时刻选取RSRCKF滤波获得的状态估计值与误差协方差矩阵，并令τk+1=τk+1,RSRCKF，进行k+1时刻的异常判别，否则，在k时刻选取ASRCKF的滤波结果，相应地令τk+1=τk+1,ASRCKF。该做法的依据是k时刻选择合适的滤波算法可以使k+1时刻的预测量测更接近真实的量测值，因此获得的异常判别因子更小。以上算法称为基于反馈判决的鲁棒自适应SRCKF算法(feedback decision based robust adaptive SRCKF，FRA-SRCKF)。

3 仿真与分析

系统模型构建及相关参数定义见式(1)～(6)。在二维笛卡尔坐标系中，选取目标的初始位置为(2 000 m,4 000 m)，初始速度为(6 m/s,-10 m/s)，初始加速度为(0.2 m/s2,-0.3 m/s2)，目标初始状态估计x0|0为[2 400,1 500,10,15,1,1]T。初始状态协方差矩阵P0|0为diag([100,100,1,1,0.01,0.01])。假定系统的过程噪声强度为q=10-2m/s2，离散采样间隔Δt=1 s，测量时间为100 s，即N=100。在仿真实验中，双观测站的初始位置设为s1=[0,0]，s2=[2 000 m,0]。方位角的测量噪声标准差均为1°。定义位置均方根误差RMSEpos为：

RMSEpos(k)=

(38)

本文选取文献[7]中的UKF、文献[15]中的MR-CKF、前文所述的SRCKF和RSRCKF、文献[17]中的RMCKF以及本文提出的FRA-SRCKF算法进行对比实验，所有滤波方法的初始条件均相同。FRA-SRCKF算法判别因子阈值δα=5.991，弱化因子β=2。MR-CKF算法权函数的阈值选择χα1=5.991，χα2=10 000。

3.1 场景1：测量存在异常误差

目标和无人机观测站的运动轨迹设定为：目标做匀加速直线运动，无人机观测站航向保持为45°，速度为100 m/s。异常误差参数设置如表1所示。

表1 异常误差参数设置

图2为量测异常时目标运动真实轨迹、双观测站轨迹以及各算法的滤波估计轨迹图。如图3所示，量测异常存在时τk超出阈值，FRA-SRCKF比SRCKF具有更好的调节能力。

图4和图5分别给出了量测异常时各算法的RMSEpos和RMSEvel。从这两幅图可以发现，UKF、SRCKF、RMCKF在异常量测点处受到较大的影响，出现大幅度波动，虽然具有一定的抑制扰动的能力，但UKF、SRCKF的调节能力有限，其恢复到正常状态的时间较长，并且误差随异常值增大而明显增大，在70 s呈发散趋势，而RMCKF能够快速抑制量测异常，由于其对量测噪声协方差的修正，故对异常值的大小不敏感。值得注意的一点是，尽管MR-CKF、RSRCKF和FRA-SRCKF跟踪效果在面对量测异常时得到良好的改善，其均方误差表现并非相同，这是由于MR-CKF、RSRCKF缺乏对状态预测协方差的自适应调整，因而未能快速收敛到稳定值，表现为存在量测异常时误差仍较大。而FRA-SRCKF在滤波跟踪初期通过反馈判决选择自适应滤波器，能够强化观测量的作用，具有强跟踪能力，在离散量测误差产生时选取鲁棒滤波器，能够有效地抑制异常量测对跟踪的影响。

图2 目标、观测站及各算法滤波轨迹

图3 量测异常时滤波判别因子τk

图4 各算法的RMSEpos比较(场景1)

图5 各算法的RMSEvel比较(场景1)

3.2 场景2：测量异常和系统模型失准同时存在

目标和无人机观测站的运动轨迹设定为：目标在0～30 s做匀加速直线运动；在30～80 s进行转弯运动，转弯速率Ω=π/10 rad/s，运动方程详细参数可参照文献[19]，此时间段滤波模型仍采用匀加速直线运动模型，同时在第50 s、60 s、70 s出现量测噪声异常值10σ；在80～100 s恢复匀加速直线运动。无人机观测站运动方式同场景1。为了避免算法受到初始状态估计带来的误差的影响，设置目标跟踪的初始状态值与初始真实状态接近。

图6为目标运动轨迹、各算法的滤波估计轨迹图(此处忽略双观测站的运动轨迹)。从图中可以看出，FRA-SRCKF和RMCKF在双重滤波误差存在的前提下相较于其他算法具有较好的滤波特性。

图6 目标运动轨迹及各算法滤波轨迹

图7记录了场景2中SRCKF以及FRA-SRCKF的滤波异常判别因子τk。可以看出无论是量测异常还是模型失准，基本滤波框架SRCKF中的τk均会超出阈值δα，这表明滤波误差产生后，单独依赖马氏距离作为后续处理方式的根据不具有合理性；FRA-SRCKF由于存在反馈判决可以使τk很快调节到阈值范围以内，能够在抑制2种滤波误差的过程中均发挥作用。

图7 测量异常和模型失准存在时τk

图8和图9分别展示了在量测异常和模型失准同时发生情况下各算法的RMSEpos和RMSEvel。从图中可以看出FRA-SRCKF表现出较好的鲁棒以及自适应特性，能够有效抑制量测异常值引起的离散大误差以及模型失准带来的连续误差。尽管RMCKF整体上也具有较好的性能，但在异常量测处会产生大波动，并且由于量测噪声协方差被错误地修正导致其在跟踪起始阶段就存在较大误差，而这些缺点使得该算法在实际运用中可能无法有效定位跟踪。在目标进行转弯运动以及恢复直线运动过程中，UKF、SRCKF、MR-CKF、RSRCKF受模型失准和量测异常的双重影响，估计精度均明显降低，其中UKF、SRCKF的滤波性能相似，而具有鲁棒特性的MR-CKF、RSRCKF比前者性能要差，这是因为其估计过程并不总是选择l2范数进行代价函数的最小化，正常测量的权重可能会以不恰当的方式被削弱。

图8 各算法的RMSEpos比较(场景2)

图9 各算法的RMSEvel比较(场景2)

表2显示了2个场景中各算法的相对运行时间，以UKF在MATLAB中CPU计算时间为基准单位。

表2 各算法的相对运行时间

从计算复杂度来看，每周期UKF采样点个数比SRCKF多1，故而近似状态后验概率密度的运算量大；而基于CKF的鲁棒算法MR-CKF和RSRCKF仅在重构噪声协方差的方式上有所不同，因此用时大致相同；FRA-SRCKF在滤波误差产生时分解为2个滤波器并行更新，运算量有所增加。与FRA-SRCKF仅在滤波异常的前提下进行调节不同，RMCKF无论滤波正常与否均会进行鲁棒处理，又涉及到多重渐消因子的计算，因而时间代价最大。可以发现，FRA-SRCKF在运算量适中的情况下可实现更好的跟踪效果。

4 结语

针对机动目标非线性跟踪系统中，量测异常或系统建模失准的出现使得估计精度降低的问题，本文提出了一种基于反馈判决的鲁棒自适应SRCKF算法。该算法利用卡方检验判断滤波正常与否，在滤波异常时分解为鲁棒滤波器和自适应滤波器，通过下一滤波周期判别因子的大小确定当前滤波器的最优选择，属于反馈式的后验判决，最终实现以恰当的算法进行滤波处理。仿真实验表明，所提算法在目标跟踪系统中可以有效对抗观测异常和建模失准等问题，增强了滤波的鲁棒性和自适应性能，具有一定的工程应用价值。