EKF与UKF的目标跟踪算法应用与对比

唐 哲，王庭军，陈志豪

（中国航天科技集团公司九院16所，陕西 西安 710100）

目标跟踪是计算机视觉领域的一个研究热点和重要研究方向[1]。目标跟踪的基本问题是在某个特定的区域内找到目标的位置，以确定目标的运动轨迹、具体形态，在精确制导、智能视频监控、军事侦察、人机交互、机器人导航等领域具有广泛的应用，有着重要的实用价值[2]。针对目标外观变化建立外观模型，进行在线学习并且利用辅助目标来改善目标跟踪的效果成为近年来的研究热点[3]。目标跟踪算法是目标跟踪技术的核心，运动目标跟踪算法具有广泛的研究价值和挑战性，当前主流的运动目标跟踪算法有Kalman滤波算法、Mean Shift算法、粒子滤波算法等。其中Kalman滤波目标跟踪算法成本较低，实时性强、计算量小，效率较高，预测结果稳定，广泛应用于导弹追踪、军事雷达系统和机器人导航等领域。

自20世纪60年代卡尔曼滤波理论一经提出，就不断发展并得到越来越广泛的应用[4]。卡尔曼滤波算法在满足白噪声、随机变量服从高斯分布的线性系统中广泛应用。随着对卡尔曼滤波原理研究的不断深入，该滤波理论在惯性器件姿态测量[5-6]、车载组合导航、雷达目标跟踪和天气预测等重要领域应用越来越广泛。以雷达的目标跟踪系统为例，目标的状态方程和观测方程在同一坐标系下不可能都是线性的[7]。扩展卡尔曼滤波（Extended Kalman Filter，EKF）与无迹卡尔曼滤波（Unscented Kalman Filter，UKF）就是两种典型的处理非线性滤波问题的方法。

其中EKF的核心思想是，围绕滤波值X(k)将非线性状态函数f(·)和非线性观测函数h(·)展开成Taylor级数并略去二次项及以上项，得到一个近似化的线性化模型，然后应用经典卡尔曼滤波完成对目标的滤波估计。所以EKF的滤波精度和系统的非线性程度有关，随着系统的非线性程度的增强，滤波精度会变差，甚至出现发散现象。与此相类似的，如果在非线性函数的Taylor级数展开中不仅保留一次项，还保留二次项，则相应的滤波算法即为二阶广义卡尔曼滤波，简称二阶EKF。从理论和实践上来讲，二阶EKF总是优于一阶EKF的，代价是需要计算更复杂的二阶雅克比矩阵。而UKF摒弃了对非线性函数进行线性化处理的传统做法，它是利用UT变换在估计点附近确定采样点，用这些样本点表示的高斯密度近似状态的概率密度函数。UKF算法是对非线性函数的概率密度分布进行近似，并不是对非线性函数进行近似，并没有把高阶项忽略掉，因此对于非线性分布的统计量有较好的计算精度[8]。

文章首先从理论角度给出了EKF与UKF滤波的原理，建立了单目标跟踪模型，并进行了仿真，仿真数据结果表明，二阶EKF滤波精度较一阶EKF有所提高，UKF明显优于一阶EKF滤波，进而可以在目标跟踪或者其他一般的非线性系统中广泛应用，具有重要的理论和工程实用价值。

1 非线性滤波技术

1.1 EKF算法

离散非线性系统的动态方程可表示如下：

式中X(k)是n维系统状态向量；Z(k)为m维观测向量；f(·)和h(·)为非线性函数；G(k)为系统噪声驱动阵；W(k)零均值系统白噪声；V(k)为零均值观测白噪声；{W(k)}和{V(k)}互不相关，方差分别为Q和R。

将系统方程（1）围绕滤波值做一阶泰勒展开，得：

其中，

将观测方程（2）围绕滤波值Xˆk做一阶泰勒展开，得：

其中，

因此，非线性动态方程可线性化为：

与线性卡尔曼滤波基本方程相比，在线性化后的方程中，状态转移矩阵F(k+1/k)和观测矩阵H(k)由f和h的雅克比矩阵代替，再利用线性卡尔曼滤波递推方程对系统进行滤波即可。

1.2 UKF算法

1.2.1 UT变换

下面以对称分布采样的UT变换为例，简单介绍UT变换的基本原理。假设一个非线性变换Y=f(X)，可以把n维随机向量X变换为m维随机向量Y，并且已知X的均值和方差阵PX，则Y的均值和方差PY可以通过UT变换求取，步骤如下：

根据和PX计算2n+1个σ样本点：

计算非线性变换产生的样本点：

确定权值：

式中，α是很小的正数；κ=3－n；β为非负的权系数，与X的分布形式有关。

确定映射的均值和方差阵：

1.2.2 UKF滤波算法实现

在选定滤波初值：

的前提下，UKF滤波递推步骤为：

计算k－1时刻2n+1个σ样本点；

计算k时刻的一步预测模型值：

计算一步预测值的σ样本点；

将预测的σ样本点带入观测方程：

计算增益矩阵：

计算滤波值：

2 单目标跟踪模型

2.1 系统建模

考虑一个在二维平面x－y内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量

表示。假设M在水平方向（x轴方向）作近似匀加速运动，垂直方向上（y轴方向）亦作匀加速直线运动。两方向上运动都具有加性系统噪声W(k），则在笛卡尔坐标系下该质点运动的状态方程为式中F如下所示，其中，T为采样时间。

假设位于坐标原点(0,0)的雷达对质点M进行跟踪，可以得到雷达与M之间的距离和质点M相对于雷达的。

角度，实际测量中雷达具有测量噪声v(k)，观测方程为：

系统噪声W(k)具有协方差阵Qk、V(k)具有协方差阵Rk，两者均已知。初始滤波值取X0，初始滤波均方误差阵P0已知。

2.2 EKF跟踪算法

对于上述的单目标跟踪模型，可见其状态方程式线性的，观测方程是非线性的。由（4）式方法可得其线性化观测方程为：

其中，

下面列出关于状态方程（21）和观测方程（22）的EKF滤波递推方程。

状态一步预测：

一步预测均方误差阵：

滤波增益：

滤波均方误差阵：

状态滤波值：

2.3 二阶EKF跟踪算法

在二阶EKF跟踪算法中，对非线性函数h(·)进行二阶Taylor级数展开，应用二阶EKF递推方程并结合该模型的特殊性，可以得到二阶EKF与一阶EKF的不同之处仅在于状态滤波值增加了一个修正项πk，使得估计为无偏估计[9-10]，即：

式中，

其中Tr为对矩阵求迹，二阶雅克比矩阵如下，

3 仿真与分析

指定滤波初值X0，W(k)和V(k)的协方差矩阵Qk和Rk及初始滤波均方误差阵P0，如下：

选择采样时间为0.5 s，采样次数为100次，在Matlab平台中对模型仿真。

首先，比较相同条件下的一阶EKF滤波与UKF滤波结果，一阶EKF滤波与UKF滤波的位置误差、速度误差和加速度误差对比如图1—3所示。

由图1可以看出，在相同仿真条件下，大多数仿真时间内，EKF滤波位置误差明显大于UKF滤波位置误差，而且在第2 s、第14 s、第19 s、第44 s附近均出现较大的误差值。仅在第8～10 s内，UKF滤波误差值略大于EKF滤波误差值。图1所示100个时间步长UKF位置误差均值约为6.53 m，EKF位置误差均值约为10.93 m。

图1 EKF与UKF滤波位置误差对比

从图2和图3所示的滤波速度误差和滤波加速度误差，也可以看出在大多数滤波时间内，UKF滤波是优于EKF滤波的。这表明从整体上来说，UKF的状态估计优于EKF的状态估计。

图2 EKF与UKF滤波速度误差对比

图3 EKF与UKF滤波加速度误差对比

为了衡量误差的整体水平，对100个时间序列的仿真位置误差求平均值，并做多次仿真实验，每次仿真的滤波位置误差平均值对比情况如表1所示，可以看出，UKF滤波误差均值大多数情况下是小于EKF滤波误差的，也就是说，UKF从概率统计意义上优于EKF。

表1 试验位置误差对比（单位：m）

在相同条件下，一阶EKF与二阶EKF仿真位置误差对比情况如表2所示，由表2可知，二阶EFK滤波精度比一阶EFK滤波精度有所提高，但提高并不明显，这主要是因为式（29）中的修正项πk相比Xk的量级较低，即二阶雅克比矩阵Dk,1较小。

参考文献[9]中，二阶EKF滤波比一阶EKF滤波表现出较大的改进，原因之一是其非线性项包含求导不变性的指数项。而文中单目标跟踪模型非线性项，二阶雅克比矩阵较小，所以二阶EKF和一阶EKF滤波精度相近。对于非线性系统，非线性滤波方法都优于严格的线性滤波方法，但没有一种确定的非线性滤波方法在任何系统、任何模型中总能优于其他非线性滤波。不同算法之间的比较是主观的，和系统非线性的强弱、系统动力学或随机噪声的产生有很大的关系[11-12]。

4 结语

EKF是通过将非线性方程局部线性化的策略，并结合线性卡尔曼滤波递推方程来实现非线性系统滤的。在线性化过程中需要计算非线性函数的雅克比矩阵，具有一定的计算量；并且需要忽略高阶项，引入系统误差，甚至可能导致滤波发散。UKF是通过一种非线性变换来解决均值和协方差的传递问题的，无需忽略高阶项，提高了滤波精度，原理上适合任何非线性系统的滤波问题。本文简单介绍了一阶EKF、二阶EKF和UKF的滤波原理，建立了单目标跟踪系统模型并在相同条件下进行了仿真。对仿真数据分析表明，UKF滤波精度明显高于一阶EFK滤波精度。在非线性不强的条件下，二阶EKF相对于一阶EKF的优越性体现不明显。说明UKF可以实际应用于目标跟踪系统，进而可以在一般的非线性系统中广泛应用，具有很高的理论和工程实用价值。

[参考文献]

[1]YILMAZ A，JAVED O，SHAH M.Object tracking[J].ACM Computer Survey，2006（4）：13.

[2]罗海波，许凌云，惠斌，等.基于深度学习的目标跟踪方法研究现状与展望[J].红外与激光工程，2017（5）：406-413.

[3]LIU Q，ZHAO X，HOU Z.Survey of single-target visual tracking methods based on online learning[J].Computer Vision Letters，2014（5）：419-428.

[4]KALMAN R E.A new approach to linear fi ltering and prediction problems[J].Journal of Basic Engineering，1960（1）：35-45.

[5]周树道，金永奇，卫克晶，等.采用卡尔曼滤波算法的MEMS器件姿态测量[J].实验室研究与探索，2015（2）：38-42.

[6]XU H，ZENG C，HUANG Q H.Drift compensati-on of mems gyro with kalman fi ltering algorithm[J].Chinese Journal of Sensors and Actuators，2016（7）：962-965.

[7]万莉，刘焰春，皮亦鸣.EKF、UKF、PF目标跟踪性能的比较[J].雷达科学与技术，2007（1）：13-16.

[8]秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].3版.西安：西北工业大学出版社，2015.

[9]西蒙.最优状态估计：卡尔曼，H∞及非线性滤波[M].张勇刚，李宁，奔粤阳，译，北京：国防工业出版社，2013.

[10]HENRIKSEN R.The truncated second-order nonlinear fi lter revi-sited[J].IEEE Transactions on Automatic Control，1982（1）：247-251.

[11]SCHWARTZ L，STEAR E.A computational comparison of several nonlinear fi lters[J].IEEE Transactions on Automatic Control，1968（1）：83-86.

[12]WISHER R，TABACZYNSKI J，ATHANS M.A comparison of three non-linear fi lters[J].Automatica，1969（4）：487-496.