基于拉依达准则与线性拟合的改进型无迹卡尔曼滤波粗大误差补偿算法

(温州大学 电气数字化设计技术国家地方联合工程实验室，浙江 温州 325035)

0 引言

无迹卡尔曼滤波在目标跟踪方面得到了广泛的应用。不论是在军事领域还是民用领域，目标跟踪技术都是十分重要的。但目前目标跟踪技术仍存在一些问题，传感器等仪器观测错误、仪器故障等情况，降低了目标跟踪的准确性，需要通过滤波技术滤波、检测并补偿，从而降低目标跟踪的误差。

目前，卡尔曼滤波技术主要有线性卡尔曼滤波(Kalman Filter, KF)、扩展卡尔曼滤波 (Extended Kalman Filter, EKF)、无迹卡尔曼滤波(Unscented Kalman Filter, UKF)和粒子滤波(Particle Filter, PF)等。KF只适用于随机线性动态系统。 EKF使KF能适用于非线性系统，但其线性化的过程存在近似误差，精度不高。随后， UKF克服了EKF估计精度低、稳定相差的问题。本文采用UKF对目标跟踪进行状态预测估计。

目前，亦有大量学者对卡尔曼滤波在目标跟踪中出现的误差进行了深入研究。熊艳艳等人归纳了4种粗大误差的判别准则，并对其特点进行了对比分析[1-5]。Bretas提出了状态估计的两步方法，用估计值将粗大误差代替，实现了粗大误差的补偿[6]。Zhu等人提出了一种鲁棒粒子滤波的粗大误差识别算法，有效地消除或补偿了粗大误差[7]。Ting等人介绍了一种改进的卡尔曼滤波器，使用加权最小二乘的方法，对观测数据进行鲁棒的实时离群检测[8]。燕必希等人做了单目相机对目标跟踪定位的实验，当运动目标发生遮挡时,能够完成对目标的定位和跟踪[9]。刘柯在卡尔曼滤波对目标跟踪的基础上，统合了传统的均值漂移算法[10]。

本文提出的改进型UKF采用拉依达法则对卡尔曼滤波中的粗大误差进行检测，并将不同种类的粗大误差进行分类标记，采用估计值替代法和残差拟合法补偿粗大误差。基于匀加速直线运动双观测目标跟踪仿真实验，将该改进型UKF应用于目标跟踪上。结果表明，改进型UKF能有效地减小了粗大误差观测值对目标跟踪的影响，明显地提高了无迹卡尔曼滤波器的准确性与可靠性。

1 无迹卡尔曼滤波UKF

1.1 UKF原理

无迹卡尔曼滤波采用卡尔曼线性滤波框架，对于一步预测方程，使用UT变换来处理均值和协方差的非线性传递问题。无迹卡尔曼滤波对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度。无迹卡尔曼滤波没有把高阶项忽略，因此对非线性问题有较高的准确度，克服了扩展卡尔曼滤波估计精度低、稳定性差的问题。

1.2 无迹卡尔曼滤波算法实现过程

具有白噪声的状态变量X和有观测噪声的观测变量Z构成的非线性系统描述为：

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

(1)

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

(2)

式中，f是非线性状态方程函数，h是非线性观测方程，W(k)是白噪声，V(k)是观测噪声，k是当前时刻。无迹卡尔曼滤波的核心是UT变换，UT变换需要先产生2n+1个sigma点，并计算其权值，得到：

(3)

λ=α2(n+κ)-n

(4)

(5)

(6)

从而可得到无迹卡尔曼滤波算法实现过程，如下：

(7)

X(i)(k+1|k)=f[k,X(i)(k|k)]

(8)

(9)

P(k+1|k)=

-X(i)(k+1|k)]T+Q

(10)

(11)

Z(i)(k+1|k)=h[X(i)(k+1|k)]

(12)

(13)

(14)

(15)

K(k+ 1) =Pxk zkPzk zk-1

(16)

(17)

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

PZKZKKT(k+1)

(18)

式中，P(k+1|k)表示利用上一步sigma点集来估计协方差值，Q和R分别为白噪声和观测噪声中的协方差矩阵，Pzkzk和Pxkzk为系统观测的协方差，K(k+1)为卡尔曼增益矩阵.

使用状态函数f对sigma点集进行状态预测，利用预测结果，进行加权求均值，得到系统状态量的一步预测以及协方差矩阵。根据得到的预测值，再求取一组新的sigma点集。将新的sigma点集代入观测方程函数h，得到观测预测值，从而通过加权求和得到系统观测预测的均值和协方差，计算卡尔曼增益矩阵。最后计算系统的预测状态和协方差，得到的均值和协方差至少具有二阶精度。

2 观测中的粗大误差

2.1 粗大误差产生的原因

粗大误差是指在观测过程中，产生的不符合正态分布的数据的误差。目标跟踪过程中观测是必不可少的内容，目标跟踪的准确性取决于观测数据的准确性，粗大误差会直接影响观测的结果，从而导致目标跟踪轨迹紊乱。

粗大误差主要由观测过程中某些不确定或者不可控的意外因素引起，有多种的可能性，比如仪器故障、观测设备不准确等问题。粗大误差是随机的，它的产生是难以避免的。因此在无迹卡尔曼滤波中需要将其检测、标记并补偿，以此来减小甚至是完全剔除粗大误差的影响。

2.2 粗大误差的种类

粗大误差有3种典型的类型，分别为离群值(Outlier)、静差(Bias)和漂移(Drift)。离群值如图1(a)所示，离群值一般为观测数据随机产生的误差超过正常范围的个别分散的点。静差如图1(b)所示，静差是一段连续的具有粗大误差的连续的点，其粗大误差符合正态分布，静差也可以看作一段连续的离群值。仪器故障易产生静差。漂移可以是线性的、非线性的，也可以是周期性的。漂移如图1(c)所示，漂移影响比其他两种粗大误差的影响大，漂移的影响随时间增大。当观测有漂移时，观测装置的校准就会更加困难。静差可以看成是一种特殊情况下的漂移。

图1 含有不同粗大误差的观测数据

带有以上粗大误差的观测都会导致目标跟踪不准确，导致无迹卡尔曼滤波性能下降。因此，需要时刻检测观测过程中的粗大误差，考虑到以上3种粗大误差，本文提出了一种无迹卡尔曼滤波粗大误差检测方法，可以将粗大误差分类标记。

3 基于拉依达准则的粗大误差检测方法

3.1 拉依达准则

拉依达准则是以三倍观测值的标准差σ作为极限取舍的标准[1-5]，因此拉依达准则也被称作3σ准则。标准差σ为已经经过大量重复观测后计算出的参数[3]，其计算式为：

(19)

拉依达准则判别依据如下：

(20)

相别于其他准则，拉依达法则适用于大量观测次数的情况，σ经过大量数据计算更加准确。根据概率论统计，误差服从正态分布时，误差大于3σ的概率极小[4]。因此采用拉依达法则判别粗大误差时，虽然可能会有不合理的离群值不能被检测出来，但是概率极小，因此本文方法采用拉依达法则。

3.2 粗大误差检测标记算法实现

本节针对离群值、静差和漂移3种粗大误差进行检测标记，判定法则采用拉依达法则。

根据图1(a)可知，离群值为孤立的一个点，前一个采样点和后一个采样点均为正常值。因此利用式(20)计算每个采样点的残差Rn，若前一个采样点和后一个采样点的Rn≤3σ，则该点标记为离群值。

Z(k)=mark_outlier

(21)

根据图1(b)可知，静差为一段连续的离群点，且静差段的残差不随时间改变。而根据图1(c)可知，漂移也为一段连续的离群点，但其漂移段的残差大小不断增大，不符合正态分布，期望值μ大于upp。upp为设定的阀值。因此先判定粗大误差为静差还是漂移。若前一个采样点和当前采样点均满足拉依达法则，则设置一个时间步长为T移动窗口，该移动窗口包含T个采样点，随着时刻移动。计算窗口内的正态分布期望值μ，即残差平均值，表达式为：

(22)

若μ>upp，则判定窗口内的误差为静差或者漂移。若μ< upp，则表示窗口内的误差符合正态分布，是正常误差。

为区分静差和漂移，计算窗口内的标准差[6]，表达式为：

(23)

(24)

式中，ε是设定的阈值。若移动窗口内标准差小于或等于设定的阈值，表示窗口内的粗大误差的是一个恒值误差，则将窗口内的T个采样点标记为静差。若标准差大于设定的阈值，表示粗大误差不符合正态分布，则标记为漂移。

4 无迹卡尔曼滤波粗大误差补偿方法

由于目标跟踪是一个即时更新的过程，在过程中每个检测到的粗大误差都需要即时补偿，以免影响之后的目标跟踪轨迹。因此可以认为在当前时刻之前的所有采样点中将不存在含有粗大误差，无迹卡尔曼滤波不再受到之前粗大误差的影响，对当前时刻的状态的预测是准确的。

离群值一般为观测数据随机产生的不符合正态分布的点，对其他观测数据的影响较小。当检测到一个离群值时，直接用该时刻采样点的无迹卡尔曼滤波估计值代替离群值。这样可在离群值对之后的状态预测产生影响前，用一个接近于真实值的估计值将其代替，使无迹卡尔曼滤波能继续准确地预测之后的状态。离群值是单个离散的点，其残差无法估计，只能采用替代的补偿方法。

Zrec(k)=Zukf(k)

(25)

式中，Zrec是补偿之后的观测值，Zukf是无迹卡尔曼滤波观测估计值。

若检测的粗大误差为静差，由于静差的残差满足正态分布且其标准差бT≤ε。窗口内采样点的误差符合正态分布，当前时刻的误差近似于当前窗口的残差平均值，因此利用该窗口的残差平均值来补偿，观测值更新为：

Zrec(k)=Z(k)-μ

(26)

若检测的粗大误差为漂移，对移动窗口内T个采样点的残差进行线性拟合。在当前k时刻，采用一种基于线性回归的快速简便的方法，将移动窗口内的残差拟合成一条直线，预测当前时刻的残差为这条直线上当前时刻的值。[7]因此利用预测的残差补偿粗大误差，观测值更新为：

Zrec(k)=Z(k)-Y(i)

(27)

静差和漂移的影响远大于离群值，一般由仪器故障产生。若是仍用观测估计值替代粗大误差，补偿效果不明显。移动窗口可以通过前几个采样点的对当前的粗大误差进行预测，综合分析连续粗大误差的影响，预测当前的粗大误差，以此来补偿比直接替代更加准确。

式(25)、式(26)、式(27)中，每一次粗大误差补偿，只是将有误的观测值进行修正，状态值和协方差尚未修正。所以每次观测值更新之后需要重新进行UT变换，剔除粗大误差的影响，准确地跟踪目标的位置。

5 匀加速直线运动双观测目标跟踪

5.1 案例分析

本节以匀加速直线运动为例，对双观测目标跟踪进行分析。首先建立匀加速直线运动的模型，目标做匀加速直线运动，位置表达式为：

(28)

目标在一个二维平面内的匀加速直线运动，可以分解为同时在x轴和y轴方向上做匀加速直线运动。根据式(1)和式(2)的得出状态方程和观测方程分别为：

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

(29)

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

(30)

式中，观测变量有两个，一个与匀速直线运动相同是目标相对于观测点的距离，另一个是目标相对于观测点的角度。匀加速直线运动如果加速度方向于初速度方向不同，目标将会做曲线运动，仅观测相对距离无法确定具体位置，增加一个相对角度，可以在笛卡尔坐标系上确定目标的位置。

5.2 仿真与分析

建立匀加速直线运动模型。设置观测次数N=50，采样时间T=0.5 s，初始状态X(0)=[1000,5000,10,50,2,-4]T。设置UT变换参数α=le-2，β=2，κ=0，维数L为状态X的维数L=6。分别在k=5、k=30处设置离群值，在k=10-20时刻设置静差，在k=35-45时刻处设置漂移。

设置粗大误差后，目标跟踪轨迹如图2(a)所示，由于静差和漂移的影响，传统UKF对目标的跟踪完全偏离了实际的目标运动轨迹，粗大误差对目标跟踪的影响很严重。采取补偿之后的目标跟踪轨迹如图2(b)所示，与图2(a)相比，将粗大误差补偿之后，改进型UKF能准确地跟踪到目标的位置。

图2 目标状态跟踪轨迹图

在补偿前先将3种粗大误差分类标记，粗大误差标记结果如图3所示，图中圆形圈出的是离群值,矩形圈出的是静差，三角形圈出的是漂移。在进入静差和漂移的第一个点，因为实际情况中后续的实际位置和观测数据是无法预知的，因此判断为离群值，该离群值为静差和漂移的起始点，当粗大误差为静差时，移动窗口中静差有跳变，会误判为漂移，可以通过增大移动窗口时间步长T，来减小跳变对判断的影响。

粗大误差影响了传统UKF对目标的跟踪，从而导致对传统UKF观测估计值不准确，图3中将粗大误差标记并补偿，改进型UKF观测值能准确地跟踪到目标，准确地反映目标的实时位置信息。

RMSE是均方根误差，能够反映出观测的精密度，传统UKF与改进型UKF的100组仿真实验统计结果如表1和图4所示。

由表1和图4可知，在100次的仿真实验中，改进型UKF的均方根误差远小于传统UKF，x轴均方根误差减小了6.5569，y轴均方根误差减小了18.3280.这说明改进型UKF算法的效果很明显。其中，粗大误差对y轴分量的影响更大，因此对y轴分量的补偿效果也更加明显。相比于传统UKF，改进型UKF明显增强了UKF对粗大误差的抗干扰能力，使UKF更加稳定、准确。

图3 目标观测轨迹图

表1 均方根误差(RMSE)统计结果

图4 目标轨迹均方根误差图

6 结论

本文提出了一种采用基于拉依达法则检测粗大误差并分类补偿3种粗大误差的改进型无迹卡尔曼滤波算法。将其应用于目标跟踪，通过对匀加速直线运动双观测目标跟踪的仿真，结果显示出该改进型算法能有效地增强无迹卡尔曼滤波对粗大误差的抗干扰能力，实现对目标的准确跟踪。