基于修正量测的UKF算法

崔 灿, 刘 攀, 张 敏

(1 湖北工业大学机械工程学院， 湖北 武汉 430068； 2 湖北工业大学工程技术学院， 湖北 武汉 430068)

在许多实际问题中，大量的滤波问题都是非线性的，解决非线性滤波问题的两种主要方法都是基于卡尔曼滤波，卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态，然而简单的卡尔曼滤波必须应用在符合高斯分布的系统中。第一种就是扩展Kalman滤波(EKF)，它只取非线性函数泰勒展开式的一阶线性段，其高阶项舍去，来到达线性化的目的。通过将非线性的阶段线性化，然后舍弃高阶项或逼近的方法来处理非线性问题[1]。然而，当非线性函数的泰勒展开不能舍弃时，这种线性化会造成函数的较大误差，甚至对最终的滤波结果造成很大影响。另一种是通过采样的方法来逼近非线性分布的方法，将固定数量的参数支近似一个高斯分布的UT变换作为基础，使用Kalman线性滤波框架，并且采样不是随机采样，而是采取确定的sigma点，这种方法是无迹Kalman滤波[2]，并且它可以达到泰勒三阶的近似效果。UKF无迹卡尔曼滤波是在卡尔曼滤波和变换的基础上发展而来的，它是利用无损变换使线性假设下的卡尔曼滤波应用于非线性系统，但是在UKF的计算中，当存在随机向量相关或奇异矩阵的时候[3]，则不能采用Cholesky分解，该分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。这将影响方差矩阵平方根的计算，进而严重影响sigma点的求解[4]。本文就上述问题引入修正量测，将一个正定矩阵替换之前的奇异矩阵，使得协方差阵转化为正定矩阵，进而可以使滤波正常进行[5]。

1 修正量测UKF

上文提到传统的做法是将非线性函数转变为线性化来解决问题[6]，而无迹卡尔曼滤波运用的是滤波框架和无迹变化来处理预测方程中的均值问题和协方差非线性传递问题[7]。无迹卡尔曼滤波算法逼近非线性函数的概率密度分布，并用一系列确定的样本来逼近状态的后验概率密度[8]。无迹卡尔曼滤波流程为：

Wk中包含协方差矩阵Q,Vk中包含协方差阵R

可以看出一旦产生奇异矩阵，UT变换时，算法精度会受到很大的影响，严重影响着算法的收敛。换言之，协方差矩阵R必须局部修正。因此，通过数次试验后本文选择了一个正定矩阵

用来替换之前的协方差矩阵，整个协方差为正定矩阵，继而在滤波器中可以使用构造的C-UKF(Correction Unscented Kalman Filter)算法。 然而，这种调整需要付出相应的代价，调整之后的R’不等于先前的R，导致滤波器变为次优[9]。下文将对这种调整进行具体研究。

2 仿真实验设计与验证

在雷达跟踪系统的东-南-高(ESU)的坐标系中(图1)，方位角ψ的取值范围一般为-π<ψ≤π若此时存在一架飞行器穿过YOZ平面自西向东进行飞行，那么此时雷达跟踪系统的观测值会从π瞬间突变到-π，如图2所示。

图 1 飞行器坐标系

图2 飞行角度与雷达观测值变化

如果方位角和量测信号突变，将导致滤波器发散。选用方位角的余弦和正弦值代替方位角信息，观测向量等效为

[ρθcos(ψ) sin(ψ)]

Vψ=[cos(ψ) sin(ψ)]T

令ψ的微小变化量为δψ，可得微小变化量δVψ

δVψ=[-sin(ψ)δψcos(ψ)δψ]T=

[-sin(ψ) cos(ψ)]Tδψ

令微小变化量δψ代表方位角ψ的测量误差，可以得到Vψ的协方差矩阵

将雷达系统中的协方差矩阵视为非奇异矩阵。将方位角信息转化为Vψ，造成量测的局部相关性，且Rψ是奇异矩阵，表现在协方差矩阵上为

在这种情况下，则

R在这里必为奇异矩阵。

在三维空间中，设定飞行器以恒定的速度在飞行，在 ESU 坐标系下，地面有一雷达对其进行观测。其状态方程在n时刻表示为

状态转移方程为

目标到达雷达距离ρ，俯仰角θ，方位角ψ，这三个参数构成了雷达的观测向量Z=[ρθΨ]T，观测方程为

式中：wn是由高斯随机变量组成的向量，协方差矩阵是

修改之后的观测方程为

仿真试验分两个方面进行:

1)设定飞行轨迹使之不通过角度突变区域，图3所示。

2) 设定飞行轨迹使之通过角度突变区域，如图4所示。

图 3 飞行轨迹(经过角度突变)

图 4 飞行轨迹(不经过角度突变)

在雷达系统中，两种方法对位移的估计性能如图3所示，以速度为测量的雷达系统中，两种方法对速度的估计性能如图4所示。对象图表中的误差估计是由AVE(Absolute value error)与RMSE(Root Mean Square Error)得到，可表示为:

(a)

(b)

(c)

(d)图 5 仿真结果图

仿真结果:上述仿真分别是飞行器通过角突变区时，轨迹在X，Y,Z轴上位置的均方根误差。当我们使用一个较小的值替换奇异矩阵的原值时[10]，虽然会损失一定的滤波精度，对奇异矩阵进行了修改，但是解决原先滤波中奇异协方差矩阵的问题，使得C-UKF精度比传统UKF精度更高，对于滤波中的奇异矩阵误差变大的问题具有更好的解决性能。

3 结束语

通过雷达观测飞行器时角度突变的问题[11]，将导致滤波器发散的情况来仿真对比 UKF和C-UKF算法[12]。实验结果表明，C-UKF通过Cholesky分解，进而选取sigma点，使正定矩阵替换之前的奇异矩阵，通过修正量测的UT，很好的处理了奇异矩阵的问题，让其自身变得精度更高。