基于自适应嵌入式CKF的目标跟踪算法

蔡宗平， 于泽祥， 杨 剑

(火箭军工程大学，西安 710025)

0 引言

在许多实际问题中，目标跟踪是一种非线性[1]滤波问题，由于非线性系统通常无法得到解析递推表达式，所以处理该类问题必须采用近似方法。

1971年，文献[2]通过泰勒展开的方式将非线性系统近似为线性系统，提出扩展卡尔曼滤波(EKF)算法，在工程上容易实现。但针对强非线性系统，该算法的一阶线性化近似精度较低[3]。文献[4]通过无迹变换的采样点集来逼近非线性函数概率分布，提出无迹卡尔曼滤波(UKF)算法，较EKF具有更好的非线性估计性能，但其性能易受参数取值的影响，且在状态维数较高时，会出现滤波精度下降甚至发散的现象[5-6]。为此，文献[7]提出容积卡尔曼滤波(Cubature Kalman Filter,CKF)算法，利用三阶球面-径向容积准则计算非线性近似中的概率积分问题。相比于UKF算法，CKF算法在高维状态下拥有更高的数值稳定性和滤波精度[8-9]，然而球面容积公式的固有缺陷[10]限制了其精度和高阶扩展性。2013年，张鑫春等学者利用嵌入式容积准则计算非线性近似中的概率积分问题，提出嵌入式容积卡尔曼滤波(ECKF)[11]算法，实现了更高精度的滤波估计，并且有较好的高阶扩展性[12]。

事实上，上述算法在解决非线性滤波问题时，需要已知系统的噪声统计特性[13]，然而在实际情况中，系统噪声的统计特性往往是时变未知的，这是因为系统噪声易受外界干扰、滤波对象的物理特性等因素的影响。

本文以传统的Sage-Husa算法[14]为基础，结合嵌入式容积规则推导出适用于非线性条件下的噪声估计器，提出自适应嵌入式容积卡尔曼滤波(AECKF)算法，以实现实时估计和修正噪声统计特性的目的，并且采用一种基于协方差匹配判据[15]的方法对滤波发散的情况进行判断，从而抑制滤波中的发散现象，增强滤波器的鲁棒性。理论分析和实验结果都表明，本文提出的自适应ECKF滤波算法的估计性能和稳定性要明显优于标准ECKF滤波算法。

1 系统描述

考虑一般非线性系统

(1)

式中：xk∈Rn为系统的状态向量；zk∈Rp为系统的观测向量；fk-1(·)为k-1时刻到k时刻的状态转移函数；hk(·)为量测函数；wk-1为统计特性未知的独立高斯系统噪声序列；vk为统计特性已知的独立高斯观测噪声序列。wk-1，vk具有的统计特性为

(2)

式中，δkj为克罗内克函数。

2 自适应嵌入式容积卡尔曼滤波(AECKF)算法

2.1 嵌入式容积卡尔曼滤波(ECKF)算法

传统的CKF算法采用三阶球面-径向容积准则，当状态空间的维数较大时，会出现容积点超出定义区间，或容积点为复数等情况，导致滤波发散。同时，若需构造高阶CKF滤波器，则需同时提高球面容积准则和高斯-拉盖尔积分公式的阶数，计算过程十分复杂。针对传统三阶CKF算法的不足，张鑫春等学者利用嵌入式容积准则计算非线性近似中的概率积分问题，有效地解决了传统CKF算法误差随状态空间维数呈线性增长的缺点。

对于n维状态，采用N=2n+1个容积点实现积分的数值逼近，即

IN(f)

(3)

式中，相应的容积点和权值为

(4)

(5)

2.2 非线性噪声统计估计器

文献[14]基于卡尔曼滤波框架推导出了常规Sage-Husa噪声估计算法，并用来解决线性条件下噪声统计特性未知的滤波问题，当系统为非线性时该算法不再适用。本文在此基础上对传统的Sage-Husa算法进行改进，采用嵌入式容积准则得到可以应用于ECKF的噪声统计估计器。

在常值噪声条件下，对于式(1)描述的非线性系统，基于Sage-Husa算法的系统噪声均值估计算式为

(6)

结合标准ECKF滤波算法得到嵌入式容积规则下系统噪声均值的估计算式为

(7)

而在常规Sage-Husa算法中系统噪声协方差的递推公式为

(8)

可得嵌入式容积规则下系统噪声协方差矩阵的递推公式为

(9)

当系统噪声的统计特性为时变未知时，利用渐消记忆指数加权法得到时变噪声统计估计器为

(10)

(11)

式中,dk=(1-b)/(1-bk)，b为遗忘因子，通常取值范围为0.95

2.3 防止算法发散的改进

解决目标跟踪问题时常常会因为滤波环境发生变化导致滤波发散，同时，雷达环境下的目标检测通常会产生一定比例的野值，在滤波时如果不进行甄别并剔除，也会造成滤波结果不稳定甚至发散等问题。

针对这些问题，在算法中引入判断机制，通过实际余项与理论余项的相互关系来判断滤波状态是否发散,即

(12)

式中：ε(k)为残差序列；γ为事先设定的储备系数，γ≥1；tr为矩阵的迹。

由滤波原理可知

(13)

蔡宗平等： 基于自适应嵌入式CKF的目标跟踪算法把式(13)代入式(12)，发散判据变为

(14)

若式(14)成立，说明滤波器工作正常；若该式不成立，则说明滤波器可能发散，此时直接跳过k时刻的状态，依据k-1时刻的状态对k+1时刻进行估计，有

(15)

此时，理论上有

(16)

跳过k时刻状态后，滤波发散判据变为

(17)

若式(17)成立，则说明跳过k时刻的状态后，滤波器由发散状态变为稳定状态，此时可以将k时刻的量测值判定为野值并进行剔除，从而抑制滤波发散的问题；若该式不成立，则说明观测环境已发生变化，需要在k时刻对Qk重新进行计算。

2.4 AECKF算法步骤

将2.2节得到的时变噪声统计估计器应用于标准ECKF滤波算法的框架下，得到AECKF算法如下。

1) 系统初始化。

(18)

2) 时间更新。

Sk-1|k-1=chol(Pk-1|k-1)

(19)

(20)

(21)

(22)

(23)

Sk|k-1=chol(Pk|k-1)

(24)

(25)

Zi,k|k-1=h(Xi,k|k-1,uk)+r

(26)

(27)

(28)

3) 发散判断。

4) 量测更新。

(29)

(30)

(31)

(32)

(33)

5) 噪声估计。

根据式(10)、式(11)递推估计系统噪声的统计特性。

3 仿真对比与性能评估

3.1 实验场景

本文选用二维空间，目标在0～40 s内做匀速运动；在41～70 s内做角速度ω=1.8 rad/s的匀速转弯运动；在71～100 s内做ω=-1.8 rad/s的匀速转弯运动。仿真时间为100 s。

(34)

仿真场景1为a=1。

仿真场景2为

(35)

3.2 仿真结果

分别在两个仿真场景下做100次蒙特卡罗仿真，将AECKF与CKF，ECKF算法进行仿真实验对比。仿真过程中，Sage-Husa估计器的参数b=0.97，均方根误差(RMSE)定义为

(36)

图1给出了场景1下目标的真实运动轨迹以及分别采用CKF，ECKF与AECKF算法跟踪得到的目标运动轨迹。图2为在仿真场景1中，3种算法得到的目标位置和速度的均方根误差曲线比较。由图2可知，在系统噪声为未知常值的情况下，AECKF算法能有效地减小ECKF算法跟踪的位置和速度误差，提高跟踪精度。表1中的统计数据同样验证了以上分析。

图1 场景1目标的真实运动轨迹与滤波轨迹Fig.1 True trajectory and filtering trajectory of target in Scene 1

图2 场景1位置和速度的均方根误差曲线Fig.2 RMSE of location and speed in Scene 1

表1 场景1性能比较

仿真场景2中目标的真实运动轨迹以及分别采用CKF，ECKF与AECKF算法跟踪得到的目标运动轨迹如图3所示。

图3 场景2目标真实运动轨迹与滤波轨迹

图4为3种算法得到的目标位置和速度的均方根误差曲线比较。表2是场景2中目标的位置和速度的均方根误差均值统计数据。

图4 场景2位置和速度的均方根误差曲线Fig.4 RMSE of location and speed in Scene 2

表2 场景2性能比较

由图4和表2可知，通过AECKF算法得到的位置、速度误差均小于采用标准CKF算法和ECKF算法跟踪目标得到的误差，说明在系统噪声未知时变且与先验噪声存在一定差异的情况下，AECKF算法能够有效地抑制目标跟踪的发散，从而减小跟踪误差，提高了跟踪精度。

表3列出了2种仿真场景下不同滤波算法的平均运行时间。3种算法都具有较高的运算效率，但由于AECKF算法采用自适应滤波算法对时变噪声进行估计，所以该算法的速度较CKF算法与ECKF算法慢一些。但与滤波精度的提高相比，AECKF算法的时间增加处于可接受的范围内。

表3 3种算法的平均运行时间

4 结束语

嵌入式容积卡尔曼滤波(ECKF)算法是一种高精度的非线性滤波算法，但在滤波时，系统噪声的统计特性未知，可能会导致其在处理非线性滤波问题时滤波精度降低，严重时甚至引起发散。本文提出了一种AECKF滤波算法，利用嵌入式容积准则对传统的Sage-Husa算法进行改进，得到适用于嵌入式容积卡尔曼滤波器的非线性噪声统计估计器，并引入判断机制来抑制目标跟踪时的发散问题。仿真结果表明，该算法能够有效克服传统非线性算法在系统噪声统计特性未知和时变的情况下滤波精度较低以及容易引起发散的缺点，提高了滤波器的自适应性与稳定性，状态估计的精度要显著优于传统的非线性滤波算法。