未知噪声协方差的自适应容积卡尔曼滤波

杨恒占， 王盛博， 何红丽

(1.西安工业大学陕西省自主系统与智能控制国际联合研究中心，西安，710021； 2.中国飞行试验研究院，西安，710089)

非线性滤波问题应用于目标跟踪，由于其广泛的军用及民用特点，是近年来研究的热点问题[1]。目标跟踪的主要任务是通过从传感观测中提取有价值的信息来估计动态目标参数并显示目标的轨迹，其跟踪精度在很大程度上取决于滤波算法的性能。虽然测量噪声的统计特性可以从传感器中得到，但其中包含了外部干扰导致得到的特性不可靠。由于外部随机干扰的影响，很难得到具有精确统计协方差的系统噪声[2]。同时在机动目标跟踪系统中涉及到时变噪声协方差，传统的滤波算法无法满足要求[3]。因此，为了处理噪声协方差的不确定性，需要高性能的自适应滤波算法。

容积卡尔曼滤波(Cubature Kalman Filter，简称CKF)是一种新型高斯滤波算法，为非线性系统的状态估计提供新的实现方式，利用球面—径面积分准则计算非线性变换后随机变量的均值和协方差，采用一组等权值的容积点对最优状态的后验分布进行逼近。由于CKF将采样点都设置在超球面上，不存在权重为负的情况，相比其它非线性滤波器具有更高的非线性逼近性能和估计精度，并且运算速度也得到了提高[4]。CKF中目标跟踪的精度主要依赖于对噪声统计特性的研究。然而噪声统计特性在实际情况下大多为非零均值、非白噪声，非线性系统又往往受到外界不确定因素的干扰，这些都会影响滤波结果。为了处理这种未知噪声，许多研究者对自适应滤波算法进行改进。丁家琳等[5]研究者采用基于Sage-Husa噪声估计器的自适应滤波器，在未知噪声特性的情况下，很大程度上提高了滤波精度，但巨大的运算量导致其在实际工程中很难应用。在常用估计方法中可以看到，状态估计时刻影响着新息的测量噪声统计特性，因此，估计误差使得测量噪声协方差在计算过程中产生误差，进而导致滤波器性能下降。Zhang等[6]研究者针对这一问题研究了一种基于测量的自适应卡尔曼滤波算法(MAKF)，克服了针对导航系统改进的Sage-Husa自适应滤波器的不稳定问题，但是，MAKF只有当一个测量噪声协方差小于另一个时才有效。

本文针对未知时变噪声协方差的系统，提出了一种新的非线性跟踪系统自适应CKF方法，该方法避免了两种噪声之间的相互作用。首先根据新息序列与残差序列的相关性，推导出一种新的过程噪声协方差Q估计方法；然后采用残差序列对测量噪声协方差进行估计，其次采用加权因子，得到当前时刻更新后的噪声协方差，利用更新后的噪声协方差矩阵来校正状态估计，有效避免了不准确状态估计的缺陷。仿真结果表明，未知噪声条件下跟踪精度有了明显的提高。

1 非线性估计的CKF算法

考虑如下非线性离散系统：

(1)

式中：xk∈Rn为系统的状态向量；zk∈Rm为系统的量测输出向量；f(·)和h(·)分别为非线性系统的转移函数和量测函数；wk-1∈Rn为系统噪声，vk∈Rn为量测噪声，wk-1和vk均为高斯白噪声且相互独立；协方差矩阵为Qk和Rk。

容积卡尔曼滤波算法将非线性滤波转换为求解非线性函数与高斯概率密度乘积的积分问题，然后利用2n个容积点进行加权求和对状态后验概率密度进行逼近。对于任意分布函数ρ(x)利用容积积分准则求解积分问题，使用容积准则获得的基本容积点和所对应的权值如下：

(2)

1.1 时间更新

1)利用Cholesky方法对协方差进行分解并进行容积点的计算。

Sk-1/k-1=Chol(Pk-1/k-1)

i=1,2,…,2n

(3)

其中，Sk-1/k-1为协方差矩阵平方根。

计算非线性状态方程传播容积点。

(4)

计算状态估计值并估计误差协方差矩阵。

(5)

1.2 量测更新

对容积点进行更新，通过量测方程传播容积点，预测观测向量及协方差。

利用Cholesky方法对误差协方差进行分解并对容积点进行更新。

Sk/k-1=Chol(Pk/k-1)

(6)

计算非线性量测方程传播的容积点。

Zi,k/k-1=h(Xi,k/k-1)

(7)

计算量测预测、量测预测协方差矩阵并估计互协方差矩阵。

i=1,2,…,2n

(8)

计算增益矩阵、状态更新值和协方差阵更新值。

Kk=Pxz,k/k-1(Pxz,k/k-1)T

(9)

2 新型自适应CKF滤波

在导弹轨迹的测试试验中，受各种因素的影响，量测数据和真实数据偏差较大，导致目标跟踪的精度下降。针对这一问题，本文通过估计过程噪声协方差Q和测量噪声协方差R，进而改进CKF滤波算法，提高目标跟踪的精度。

2.1 自适应Q估计

在卡尔曼滤波理论中，新息序列和残差序列可以定义为[7]：

(10)

定理1对于式(1)给定的系统，在时间k处，新息与残差之间的互相关函数为：

(11)

证明：将式(1)中的量测方程代入方程(10)中，新息序列和残差序列可以表示成：

(12)

根据式(10)新息序列和残差序列在时间k处的互相关函数可以表示为：

(13)

当过程噪声和量测噪声不相关时，有:

(14)

(15)

将式(14)和式(15)代入式(13)中得到新息与残差的互相关函数：

(16)

证明完毕。

利用新息序列和残差序列提高Q估计的鲁棒性，对新息序列与残差序列的差求期望:

E[(ηk-εk)(ηk-εk)T]=

(17)

根据式(12)和式(15)有新息序列的期望和残差序列的期望:

(18)

(19)

进一步，根据定理1和式(18)～(19)确定新息序列和残差序列差的协方差，可以得到:

(20)

将式(5)代入式(20)中，可以表示为：

E[(ηk-εk)(ηk-εk)T]=

(21)

进一步得到

(22)

在雷达跟踪系统中Hk/k-1的秩一般大于Qk-1的未知数，因此可以得到唯一的过程噪声协方差Qk-1。此外，Qk-1一般为对角矩阵，计算比较简单。

2.2 自适应R估计

基于残差序列对测量噪声协方差Rk进行估计，测量噪声协方差Rk可以估计为[8]：

(23)

可以进一步引入加权因子δ对Rk进行调整，测量噪声协方差可以更新为：

(24)

式中：0

2.3 新型自适应CKF滤波算法流程

根据上述方法，未知时变噪声协方差下的目标跟踪自适应CKF滤波算法流程如下：

Step2：利用式(3)和式(4)对协方差分解并进行传播容积点的计算；

Step3：利用式(22)更新过程噪声协方差Qk-1；

Step4：利用式(5)估计误差噪声协方差Pk/k-1；

Step5：利用式(24)更新测量噪声协方差Rk；

Step7：对于下一时间段，重复Step1～7。

3 仿真案例及结果分析

在某地地导弹试验基地中，采用雷达对目标进行跟踪，系统的模型如下：

xk=f(xk-1)+wk-1

zk=h(xk)+vk

其中，f(xk-1)=Fxk-1，

在上述模型下，采用MATLAB进行验证，导弹飞行的真实轨迹和在滤波算法下的三维轨迹见图1。图2为导弹飞行的二维轨迹，可以看到本文的自适应CKF滤波算法精度高于自适应UKF滤波算法。图3将真实数据和测量数据进行对比。图4～图9分别表示了X方向、Y方向和Z方向的位置误差和速度误差。图10和图11分别采用标准差和均方误差对两种滤波算法进行比较，虽然在有的时间段自适应UKF精度略高于本文的自适应CKF算法，通过表1对两种算法的RMSE均值进行比较，数据显示本文的算法精度更高。

图1 三维弹道轨迹

图2 二维弹道轨迹

图3 目标量测与真值对比

图4 X方向位置误差

图5 Y方向位置误差

图6 Z方向位置误差

图7 X方向速度误差

图8 Y方向速度误差

图9 Z方向速度误差

图10 标准差

图11 位置均方差

表1 两种算法RMSE均值比较

4 结语

本文针对目标跟踪过程中未知噪声协方差问题提出了自适应CKF滤波方法。首先通过新息序列和残差序列导出的线性矩阵方程实时处理过程噪声协方差Q；然后利用残差序列对噪声协方差进行估计并引入加权因子将测量噪声协方差矩阵与估计值组成称为新的测量噪声协方差阵，文中对自适应UKF和自适应CKF两种滤波算法进行位置误差和速度误差对比。仿真结果表明，在时变噪声协方差条件下，所提出的自适应CKF滤波精度更高。本文提出的新型CKF滤波算法可以应用到任意非线性系统，具有较好的普适性及应用价值。