UKF在组合导航系统中的应用分析

UKF在INS/GPS组合导航系统中的应用分析*王利循，郭杭（南昌大学，江西 南昌330031）摘 要：Unscented卡尔曼滤波器（UKF）是一种新的非线性滤波算法，它避免了对非线性系统状态方程进行线性化。

建立了直接法解算INS/GPS组合导航系统的非线性UKF模型，获得了较好的计算结果。

通过与EKF方法比较，UKF方法更能有效地解决组合系统卡尔曼滤波中系统状态方程的非线性问题，并使INS/GPS组合导航系统具有较高的导航定位精度。

关键词：Unscented卡尔曼滤波器（UKF）；INS/GPS组合导航；扩展卡尔曼滤波器（EKF）一、引言在INS/GPS组合导航系统中，扩展卡尔曼滤波器 (EKF)得到了广泛应用。

EKF通过对非线性系统状态方程和测量方程，使用线性化的方法将线性卡尔曼滤波器应用到非线性系统中。

这种线性化通常有两大缺点：一是当非线性系统方程的非线性较强时，对非线性系统状态方程和观测方程雅格比矩阵的求导非常困难；二是当采样间隔较大时，这些线性化有可能导致卡尔曼滤波器不稳定。

为了解决这个问题， Juler和Uhlmann提出一种非线性滤波方法UKF。

这种滤波器能有效地减小线性化误差对系统造成的影响。

本文采用直接法解算INS/GPS组合导航系统卡尔曼滤波，推导了UKF在车辆导航中的应用。

实例取自作者在加拿大新Brunswick大学（UNB）测量系工作期间，2007年9月与研究同行一起在Fredericton，NB进行的车载GPS/INS测量的结果。

二、INS/ GPS 组合导航系统直接法滤波在直接法滤波中，直接以导航系统输出的导航参数作为状态，卡尔曼滤波器经过计算，得到导航参数的最优估计值，并作为组合导航的输出值，其结构如图1所示：图1 直接法INS/GPS组合导航系统滤波结构三、组合导航系统状态方程和量测方程取导航坐标系为东、北、天地理坐标系，则惯导系统的力学编排方程和姿态误差方程分别为：个人1简介:王利循，男，1985.1，汉，江西，学士，GNSS/导航数据处理，南昌大学建工学院07级硕士研究生个人2介: 杭，男，1960.10，汉，浙江，博士，教授，GNSS/导航数据处理等2(2sin tan )(2cos )(2sin tan )(2cos )()cos e e eie n ie u e n u u N N e n nie e u n e u u e N M e n u ie e u n e e n N M e N n M uv v v w v w v f f f R h R hv v v w v v f f f R h R h v v vw v g f f f R h R h v R h v R h h v n ϕϕϕφϕϕφφϕφφλϕϕ⎧=+−+++−⎪++⎪⎪=−+−++−⎪++⎪⎪=++−++−⎪++⎨⎪⎪=+⎪⎪⎪=+=⎩ ⎪⎪φ（1） 222(sin tan )(cos )()(sin tan )sin ()tan (cos )tan (()e e n n eie n ie u e N N M M e n ee n ie e u ie N M N N e n e e u iee n i N M N N v v v v w w R h R h R h R h v v v v w h R h R h R h R h v v v v w h R h R h R h R h δφϕϕφϕφδδφϕϕφφδϕδϕδϕφϕφφϕδ=+−++−+++++=−+−−+++++++=++−+−++++n h d w d w 2cos sec )e e u N v dR hϕϕδϕ⎧⎪⎪⎪⎪⎪⎨⎪+⎪⎪⎪+⎪+⎩（2）其中，fe、fn、fu为东、北、天向的比力量测值，其来自加速度计的输出；e v δ、n v δ、δϕ、h δ在实际计算中可以由惯导系统的速度、位置输出减GPS的对应输出来近似获得；w ie 为地球自转角速度；g为重力角速度；而陀螺常值漂移de、dn、du取为一阶马尔可夫模型：dd d β=−+ w ) （3） 其离散形式为(1)()(t d d k e d k w k β−=+＋ （4）其中，w d 为高斯白噪声。

于是，选取系统状态变量为：INS输出的载体速度信息ve 、vn 、vu ，位置信息λ、ϕ 、h，e φ、n φ、u φ平台姿态误差角，陀螺常值漂移de、dn、du。

因此，INS/ GPS 组合导航系统的状态向量x 为[]T e n u e n u e n u x v v v h d d d λϕφφφ=结合方程（1）、（2）和（3）可以列出INS/GPS 组合导航系统的状态方程：()[(),(),]xt f x t w t t = （5）式中，f[·]为非线性的连续函数，w(t)为系统噪声。

选取GPS 输出的载体速度信息eg ng ug v v v ，位置信息g g g h λϕ 作为量测Z, 即[]Teg ng ug g g g Z v v v h λϕ=则可获得组合导航系统的量测方程为(,)k k k Z H x v = （6）四、UKF 滤波方法UKF是由Julier等人[2]提出的一种解决非线性滤波问题的新方法，其主要是针对扩展卡尔曼滤波的模型线性化近似的缺点而提出来的。

UKF方法利用一系列近似高斯分布的采样点，通过Unscented变换来进行状态与误差协方差的递推和更新，在每个更新过程中，采样点随着非线性状态方程传播并随着量测方程变换，可以完全捕获高斯随机变量的真实均值和协方差，并对任何非线性系统都可精确到泰勒展开的二阶精度[3]，由此不仅保证了状态估计的精度，而且避免了对非线性方程的线性化过程, 还具有较好的鲁棒性。

由于UKF使用的是离散时间非线性模型，因此需要对系统模型进行离散化处理，本文采用四阶Runge - Kutta法以数值积分的形式实现。

处理后的系统模型和观测模型为1()()T k k k k k k k x f x w E w w Q +T k =+ ≈ΓΓ （7） 11111()()T k k k k k z h x v E v v R +++++=+ =1k + （8）因为量测噪声是加性噪声，所以只需把系统噪声添加到状态变量中，则状态变量和其协方差矩阵为0[]0k a T T Ta k k k k k P x x w P Q ⎡⎤= =⎢⎥⎣⎦此时系统模型和观测模型变为111()()a a k aa k k k x f x z h x +++== （9）系统状态维数变为：N=N x +N w 。

按规则产生Sigma 点集 S＝{ i=0,1,…,2N+1:,},ai kX iW ˆˆˆ[,a a a a k k k k X x x x =+−(10),,ˆ,(1,...,);ˆ,(1, (2)a a i k k i a a i kk iX x i N Xx i N =+ = =− =+N (11)其中，i 为方根矩阵的第i 列，一般取N＋r=3。

时间更新：,|1,,(,a a a i k k i k i k )w x f x x += （12）2|1,|10Na k k i i k k i x W x +==∑+T Q +)a （13）2|1,|1|1,|1|10{}{}N a a k k i i k k k k i k k k k k i P W x x x x +++++==−−∑ （14）,|1,(a a i k k i k z h z += （15）2|1,|10Na k k i i k k i z W z +==∑+++} （16）测量更新：2|1,|1|1,|1|110{}{}Nzz a a T k k i i k k k k i k k k k k i PW z z z z R +++++==−−∑ （17）2|1,|1|1,|1|10{}{Nxz a a k k i i k k k k i k k k k i PW x x z z ++++==−−∑T +) （18）1|1|1xz zz k k k k k K P P−++= （19） 1|1|1(k k k k k k k x x K z z +++=+− （20）11|1|1zz k k k k k k k P P K P K −+++=− （21）其中，X k+1和P k+1就是系统状态的滤波值和滤波误差方差阵；其中加权系数：W 0＝r/(N+r)，W i =0.5/(N ＋r)。

五、 计算结果通过实际测量的数据可得，车辆轨迹的初始参数见表1。

陀螺常值漂移误差0.7(。

)/s。

设INS的输出采样周期为0.01s，GPS的输出采样周期和UKF滤波周期均为1s，计算时间为153s。

由MATLAB编程计算，EKF方法计算的结果是发散的。

UKF计算出来的车辆运动轨迹（水平）如图2所示。

根据图3～4可以看出，对组合导航系统进行UKF滤波计算，可得水平位置精度优于4秒，高度精度优于5cm；速度精度优于3～4cm/s。

表1, INS/GPS 组合导航系统初始参数设置初始位置（经、纬、高度）-66.66722876, 45.94458737。

91.525m 初始位置误差水平1m ，高度5m 初始速度（东北天） （0.0005,-0.003,-0.009）, m/s 初始速度误差0.6m/s 初始姿态 （0，0，28.31。

） 初始姿态误差 （2, 2, 2。

）UKF——水平运动轨迹（经度和纬度）经度（弧度rad ）纬度（弧度r a d ）图2，车辆运动轨迹（水平）图六、结果分析（1）根据UKF 方法计算的结果可知，本文中所采用的方法能够很好地处理INS/GPS 组合系统实际测量数据，并达到了厘米级位置和厘米/秒级速度的精度。

（2）通过UKF 和EKF 计算结果中的姿态误差角的协方差值进行比较：如图5~6，得知EKF 所算数据中的姿态误差角是持续增大的，并且误差比较大。

所以在EKF 中对系统线性化时，结果非线性系统方程的非线性增强，对非线性INS/GPS 系统状态方程协方差矩阵的求逆非常困难，计算得出的结果为：协方差矩阵的条件数为RCOND = 1.510816e-019；这也是EKF 方法的致命缺点。

（3）根据EKF 方法计算结果（图6）是发散的，说明此方法不适合车载INS/GPS 组合系统动态数据的处理。

（4）而且，UKF 对非线性系统可精确到泰勒展开的二阶精度，因此滤波精度也比EKF 要高。

即UKF 对处理非线性系统的适应性比EKF 要强。

（5）造成所测数据中的姿态误差角误差比较大，可能是由于INS/GPS 组合系统中陀螺仪的精度太低的原因。