基于观测距离的EKF和UKF目标跟踪方法对比*

赵思琦，胡晓桐，郭 琴，曾萃霞，邓小飞

(吉首大学信息科学与工程学院，湖南 吉首 416000)

根据运动观测器获得测量距离并对目标进行跟踪，是目标运动分析领域中的一个经典问题，在很多应用场合都可以获得较精确的距离信息.因此，研究仅利用距离信息进行目标跟踪具有十分重要的意义[1].对于非线性系统，Bucy等[2]在1971年提出了扩展卡尔曼滤波(Extended Kalman Filter, EKF).其基本思想是采用 Taylor 变化对非线性方程进行展开，截断高阶项对非线性系统进行一阶线性化，以一阶线性化方程作为系统状态方程和量测方程，然后在标准卡尔曼滤波框架下进行滤波处理.EKF算法具备计算量小、易实现的优点，但在处理强非线性系统和非高斯白噪声环境时效果较差，且需要计算状态方程和量测方程的雅可比(Jacobian)矩阵.此外， EKF 线性化过程中只截取了展开式的一阶项，故算法线性化过程中存在较大的近似误差[3].基于此，Julier等[4]在2000年提出了无迹卡尔曼滤波(Unscented Kalman Filter,UKF).UKF 采用无迹变换(Unscented Transform, UT)对非线性系统状态空间变量进行高斯密度(概率密度)近似，通过确定性采样获取一定数量的采样点来处理非线性系统状态方程和量测方程的均值，以及协方差在卡尔曼滤波框架下的非线性传递.UKF不需要计算雅可比矩阵或海森(Hessians)矩阵，拓宽了算法的使用范围，并通过保留高阶项，提高了算法的计算精度[5].相比于EKF，UKF没有忽略高阶项，因此对于非线性分布的统计量有较高的计算精度，有效地克服了EKF估计精度低和稳定性差的缺点[6].笔者拟基于观测距离，分别采用EKF和UKF进行目标跟踪，并通过Matlab仿真工具对2种方法进行比较.

1 扩展卡尔曼滤波原理

1.1 局部线性化

离散非线性系统动态方程可表示为

X(k+1)=f(k,X(k))+G(k)W(k)，

(1)

Z(k)=h(k,X(k))+V(k).

(2)

当过程噪声W(k)和观测噪声V(k)恒为0时，非线性系统(1)和(2)的真实解称为“真轨迹”或“真状态”.

为了使数学计算更加方便，假设输入的控制量为0，过程噪声是高斯白噪声且均值为0，噪声驱动矩阵G(k)是已知的，观测噪声V(k)是加性均值为0的高斯白噪声，再假设过程噪声和观测噪声序列二者之间相互独立.

(3)

令

则由(3)式可得状态方程

X(k+1)=Φ(k+1|k)X(k)+G(k)W(k)+Φ(k)，

(4)

初始值X(0)=E(X(0)).

(5)

令

则由(5)式可得观测方程

Z(k)=H(k)X(k)+y(k)+V(k).

(6)

1.2 线性卡尔曼滤波

在得到线性化的状态方程(4)和观测方程(6)之后，可得扩展Kalman滤波递推关系：

P(k|k+1)=Φ(k|k+1)P(k|k)ΦT(k|k+1)+Q(k+1)，

(7)

P(k+1)=(I-K(k+1)H(k+1))P(k+1|k).

(8)

其中滤波的初值和滤波的误差方差矩阵的初值分别为

X(0)=E(X(0)),P(0)=var(X(0)).

(9)

(10)

1.3 EKF设计步骤

现选用一个非线性系统来详细说明EKF设计步骤.设系统状态方程为

(11)

观测方程为

(12)

状态方程和观测方程都为非线性系统.EKF算法实现步骤如下：

(ⅰ)初始化初始状态X(0)，Z(0)和协方差矩阵P0；

(ⅵ)协方差矩阵P(k|k-1)=Φ(k)P(k-1|k-1)ΦT(k)+Q，

(ⅶ)由(7)式可得EKF滤波增益K(k)=P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R)；

(ⅷ)目标状态更新为X(k)=X(k|k-1)+K(Z(k)-Z(k|k-1))；

(ⅸ)由(8)式可得协方差更新为P(k)=(In-K(k)H(k))P(k|k-1).

以上步骤为EKF的一个计算周期，如此循环，就是各个时刻对非线性系统的处理过程.

2 无迹卡尔曼滤波原理

2.1 UT算法

UT算法是UKF中的核心步骤，是一种对随机变量在经过非线性变换之后进行概率统计的方法.通过UT算法得到2n+1个Sigma采样点X和权值ω，再来计算y的统计特征：

(1)计算2n+1个Sigma采样点：

(13)

(2)计算Sigma采样点相应的权值：

(14)

其中：m为均值；c为协方差；i为采样点的位数；λ为缩放比例参数，可用其降低预测误差，λ=α2(n+k)-n；α为控制采样点分布状态的值.

在UT算法中，最重要的是确定Sigma采样点的方法，也就是如何确定使用Sigma采样点的个数、位置和相应权值.

2.2 UKF设计步骤

对于不同时刻k，X为一个随机变量，Z为一个观测变量，分别添加高斯白噪声W(k)和V(k)，构成一个非线性系统：

(15)

其中f为非线性函数的状态方程，h为非线性函数的观测方程.设W(k)的协方差矩阵为Q，V(k)的协方差矩阵为R，则UKF算法实现步骤如下：

(ⅰ)根据UT预测方程(13)和(14)，获得一组Sigma采样点及其权值，得到如下的X(i)(k|k)：

(ⅱ)对2n+1个Sigma采样点进行预测：

X(i)(k+1|k)=f(k,X(i)(k|k))i=1,2,…,2n+1.

(16)

(ⅲ)将(16)式中预测到的Sigma采样点代入到观测方程中，得到预测观测量

Z(i)(k+1|k)=h(X(i)(k+1|k))i=1,2,…,2n+1.

(17)

(ⅳ)由(17)式得到的预测观测量，通过加权求和得到预测系统结果的均值及协方差：

(18)

(19)

(ⅵ)根据卡尔曼增益值获得准确的状态向量和协方差，对系统结果的状态向量和协方差进行更新：

P(k+1|k+1)=P(k+1|k)-K(k+1)PZkZkKT(k+1).

由上述步骤可以看出，UKF在处理非线性系统问题时，可以直接在预测点附近进行UT，得到Sigma采样点，将采样点的均值和协方差与原统计特性作比较，之后直接进行非线性映射，可得到近似的概率密度函数.

3 基于观测距离的EKF和UKF目标跟踪理论及仿真结果

3.1目标跟踪算法数学建模

假定目标做匀速直线运动，运动速度为v，在k时刻，设目标的位置为s(k)，经过采样时间T之后，目标的位置为s(k+1)=s(k)+vT.显然，在直角坐标系中目标在x和y方向都有相应的分量，运动系统的状态量包括x方向的位置、y方向的位置、x方向的速度分量和y方向的速度分量.在目标运动过程中，可表示为

X(k+1)=ΦX(k)+G(k)W(k)，

(20)

其中

通过观测站或者传感器对目标进行探测(如雷达探测).设雷达站的位置为(x0,y0)，目标k时刻的位置为(x(k),y(k))，又设观测量Z(k)为目标与雷达之间的距离，则观测方程为

(21)

其中V(k)为雷达自身的测量误差.由此得到匀速运动的系统模型，其中状态方程(20)为线性的，观测方程(21)为非线性的.

3.2 EKF目标跟踪

根据局部线性化方法将非线性方程(21)线性化，并由(10)式得到相应的雅可比矩阵

(22)

3.3 UKF目标跟踪

根据3.1节的数学模型，设采样时间间隔T=1 s，运行总时间N=60 s，则过程驱动矩阵Φ和噪声驱动矩阵G(k)为常数矩阵：

W(k)的协方差矩阵Q=σw*diag(1,1)，其中σw为可调参数，σw≪1.V(k)的协方差矩阵R为已知量.设置UT中的相关系数，α=0.01，K=0，β=2，维数n=9.

(23)

3.4 仿真结果

根据(20)—(23)式求解，运行各时刻点，分别得到EKF和UKF的跟踪轨迹和误差曲线，结果如图1和图2所示.

EKF和UKF对运动目标的跟踪轨迹如图1所示.从图1可以看出，EKF对真实轨迹的仿真效果没有UKF好，当横坐标增大到一定程度时，EKF与真实轨迹有较大误差.这是由泰勒展开式保留一阶近似项、省略二阶(及以上)项导致的线性化误差所引起的，因为UKF使用UT，所以在此情况下UKF的仿真结果优于EKF.

EKF和UKF的跟踪误差如图2所示.从图2可以看出，EKF的跟踪误差与UKF的跟踪误差的差距很明显，EKF的变化剧烈，而UKF的较好，且UKF均方差变化小，收敛性能优于EKF.这说明相较于EKF，UKF具有良好的跟踪效果.

图1 EKF和UKF对运动目标的跟踪轨迹Fig.1 Track of Moving Targets by EKF and UKF

图2 EKF和UKF的跟踪误差Fig.2 EKF and UKF Tracking Errors

4 结语

由EKF和UKF对目标跟踪的原理可知，EKF是将非线性函数方程通过求其偏导数高阶导数和雅可比矩阵作线性化处理，而得到一个近似的线性化函数方程，再进行目标跟踪；UKF是直接对非线性函数方程通过高斯分布而得到概率密度的近似值，再利用UT得到Sigma采样点，与之前估计的状态方程进行非线性映射，从而得到准确的目标跟踪方程.所以，UKF的准确度和收敛性比EKF的好.UKF算法虽然能够有效提高非线性系统目标跟踪的精度，但目前只是在高斯噪声的环境下进行处理，若系统是在非高斯的环境下，则可能出现较大误差，这就需要对算法进行不断的改进.