INS／GPS紧耦合系统中的模糊自应SRUKF算法

袁建国，袁艳涛，刘飞龙，庞 宇，林金朝

（重庆邮电大学光纤通信技术重点实验室，400065重庆）

INS／GPS紧耦合系统中的模糊自应SRUKF算法

袁建国，袁艳涛，刘飞龙，庞 宇，林金朝

（重庆邮电大学光纤通信技术重点实验室，400065重庆）

为解决惯性导航系统（INS）与全球定位系统（GPS）紧耦合中标准无迹卡尔曼滤波（UKF）由于计算舍入误差使协方差矩阵负定和实际应用中由于量测噪声时变而严重影响滤波精度的问题，提出了基于模糊控制理论的自适应平方根无迹卡尔曼滤波（NASRUKF）算法.该算法在滤波过程中不是直接计算协方差矩阵，而是计算协方差矩阵的平方根，从而可以保证协方差矩阵的非负定性；然后根据实时得到的量测信息的实际方差与理论方差的比值，通过设计的模糊控制系统（FCS）实时调整量测噪声矩阵.实验表明：该算法对时变的噪声具有很好的自适应性，相比于UKF算法具有更高的精度并使得系统具有更高的稳定性和鲁棒性.

INS／GPS；卡尔曼滤波；SRUKF；模糊控制；自适应

惯性导航系统（INS）是一种完全自主的导航系统，不需要从外部接收信号，具有更新频率高、抗干扰能力强等优点，但其误差会随时间不断积累，不适合长时间工作；全球定位系统（GPS）能够实现高精度长时间的定位，但需要依赖外界信息，因此易受外界环境干扰，且由于更新频率低不适合高动态性能的环境.INS／GPS组合导航可以弥补各自的缺点，提供更优的导航［1］.

INS／GPS紧耦合相比松耦合具有精度更高、抗干扰能力更强等优点.在紧耦合组合导航中，扩展卡尔曼滤波器EKF（extented kalman filter）、无迹卡尔曼滤波器UKF（unscented kalman filter）等得到广泛的应用［2］.UKF引入了UT变换，无需计算雅克比矩阵，且不存在高阶截断误差，但UKF要求量测噪声先验已知，而实际应用中量测噪声是时变且未知的，这就会导致系统不能很好地工作，甚至会使UKF发散［3-4］.一般自适应UKF算法能对量测噪声自适应，但在数值计算中会存在舍入误差，使协方差矩阵逐渐失去非负定，尤其当测量向量中某个或某几个分量很准确，而数值计算的有效位数却相对较少时这种现象较易发生.协方差矩阵非负定性的逐渐丧失将会使增益矩阵计算失真，从残差中提取对一步预测值的补偿信息就会越来越错误，最后导致滤波器发散不能正常工作［5-7］.针对上述问题，本文将模糊控制理论引入平方根无迹卡尔曼滤波器SRUKF（square-root unscented kalman filter），提出一种模糊自适应SRUKF算法，不仅可以保证协方差矩阵的非负定性而且还可以对时变的测量噪声进行跟踪和调整.实验表明本文算法相比于传统的UKF算法精度更高，可靠性更强.

1 INS／GPS紧耦合状态方程和测量方程

INS与GPS状态矢量如下：

式中：δφe，δφn，δφu，δve，δvn，δvu，δL，δλ，δh分别为INS三维姿态角误差、速度误差和位置误差；εrx，εry，εrz为陀螺仪常值漂移；∇αx，∇αy，∇αz为加速度计零偏；δtu为时钟偏差等效距离误差；δtru为时钟频漂等效距离变化误差．INS／GPS组合导航状态方程为

式中：WI（t）为3个方向上陀螺的白噪声和加速度计的白噪声，WG（t）为GPS时钟钟差噪声和时钟频率噪声．系统状态方程离散数学模型为

式中：Wk为系统噪声，Qk为协方差阵．

选取伪距差Δρ，伪距率差Δ˙ρ和航偏角差Δφ作为观测向量，则系统观测方程离散数学模型为［8］

式中：Vk为测量噪声，Rk为协方差阵．

2 基于模糊控制理论的自适应SRUKF算法

2.1 SRUKF滤波算法

SRUKF滤波算法非线性系统模型如下：

式中：f为非线性函数；h为关于状态的一阶连续偏导数；wk和vk为过程噪声和测量噪声，其协方差阵分别为Qk和Rk．则基于SRUKF的滤波步骤为：

1）选定滤波初值x0和p0，即

对k=1，2，3，…，执行步骤．

2）计算k-1时刻的2n+1个σ样本点，即

3）计算时刻的k一步预测模型，即

在SRUKF算法中状态方程是非线性的，由于INS／GPS组合导航系统经过精对准后平台误差角很小，滤波周期也比较小，在小角度误差的情况下，建立起来的误差方程是线性的，这样用卡尔曼滤波的方法直接得到状态方程的一步预测，然后进行以下的计算，这样不仅可以减少算法复杂度还可以减少计算量.

4）误差协方差平方根一步预测，即

5）计算k时刻的一步预测增广样本点，即

6）残差协方差平方根阵和互协方差阵测量更新，即

7）计算滤波值，即

2.2 基于模糊理论的自适应控制

SRUKF滤波算法可以保证协方差矩阵的非负定性，但系统测量噪声特性需先验已知.系统运行环境的不确定性使测量噪声会随时变化，所以测量噪声的协方差阵Rk也会不断变化，这样可能会导致卡尔曼滤波不能正常工作，甚至会发散.本文引入控制模糊理论对系统观测噪声进行自适应，对滤波模型进行实时修正，提高系统的性能.

基于模糊控制理论的自适应SRUKF算法将实时得到的量测新信息的实际方差与理论方差的比值作为模糊控制系统（FCS）输入，有FCS的输出权值在线实时调整量测噪声矩阵.INS／GPS紧耦合中基于模糊自适应的SRUKF系统结构如图1所示.

图1 模糊自适应SRUKF系统结构

图1 中cr／pr表示新信息实测残差方差与理论方差的比值，FCS模块根据模糊规则得到修正权值Wk，用来修正伪距、伪距率和航向角的观测噪声矩阵，从而当系统的观测噪声未知时，可以对观测噪声进行实时、准确估计，提高系统的性能．

残差的理论值定义为［9-10］

残差的实测方差定义为

式中：m=k-M+1，M根据具体情况而定．残差实测方差与理论方差的比值定义为

式中Tr（·）表示对矩阵求迹，当滤波模型准确时qk应该接近1，如果测量噪声增大，则cr增大，进而qk将大于1，这时增大Rk可以使qk回到1附近；如果测量噪声减小，则cr减小，进而qk将小于1，这时减小Rk使qk回到1附近．用FCS模块的输入值来调节Wk大小，进而调节Rk的大小．FCS输入、输出函数的隶属模糊度如图2与图3所示．

图2 输入函数

图3 输出函数

其模糊控制规则如下：

if qk∈［0.2，0.7］，then Wk=1-0.5μ（qk）．

if qk∈［0.7，1.3］，then Wk=1+0.2（1-μ（qk））．

if qk∈［1.3，2.0］，then Wk=1+0.5μ（qk）．

模糊控制系统最终输出权值Wk，然后对测量噪声协方差矩阵自适应调整：Rk=WkRk／k-1．

3 实验与结果分析

基于实际采集数据用MATLAB软件在计算机上进行仿真.试验中INS系统的数据更新频率为100 Hz，GPS系统为1 Hz，初始位置经纬度误差为1′，高度误差为5m，初始姿态角误差为0.1°，初始速度误差为0，蛇螺仪漂移为20（°）／h，加速度计的噪声为5×10-4g，伪距初始测量噪声方差为10 m2，伪距率初始测量噪声方差为0.5（m／s）2.卡尔曼滤波的系统噪声可以通过对系统的大量反复实验得到，其测量噪声在时刻发生变化.分别用UKF算法与模糊自适应SRUKF算法作对比分析，真结果如图4～6所示.

图4 位置误差

图5 速度误差

图6 姿态角误差

从图4～6中可以看出，由于测量噪声不断变化，模型初始的R不能准确描述测量噪声的统计特性，所以由于模型误差使普通的UKF滤波误差明显大于NASRUKF滤波的误差.而对于NASRUKF，由于此算法能够自适应调整R阵逼近真实的量测噪声水平，保证模型的准确性，故滤波误差明显小于普通的UKF滤波误差.

逐渐增大测量噪声的方差再次用普通的UKF和NASRUKF进行仿真，各自误差曲线如图7～10所示.

图7 NASRUKF位置误差

图8 UKF位置误差

图9 NASRUKF速度误差

图10 UKF速度误差

从图7～10可以明显看出，经过普通UKF滤波后输出的位置和速度误差在不断变大，这是由于随着滤波步数的增加测量噪声不断增大，UKF不能实时统计测量噪声的特性，使模型与获得的测量值不匹配，导致滤波器发散.输出的估计值相对实际的被估计值的偏差越来越大，误差的不断积累又会导致估计的均方误差阵失去非负定性，使增益矩阵的计算值逐渐失去合适的加权作用从而又进一步加快了滤波的发散.NASRUKF能够自适应调整R跟随真实测量噪声的变化，保证了模型的正确性和均方误差阵的非负定性，所以在强噪声的环境中也能有很好的滤波效果，大大提高了系统的抗干扰能力.

4 结 语

基于模糊控制理论的自适应SRUKF算法通过间接地计算均方误差矩阵的平方根来计算其值，保证了非负定性，并通过引入模糊控制理论来实时自适应地观测噪声.实验表明本文算法可以有效提高系统的精度和抗干扰能力.

［1］罗建军，马卫华，袁建平.组合导航原理与应用［M］.西安：西北工业大学出版社，2012.

［2］LIU YU，WANG HONG，HOU Chaohuan.UKF based nonlinear filtering using minimum entropy criterion［J］. IEEE Transactions on Signal Processing，2013，61（20）：4988-4999.

［3］LUO Zhen，FANG Huajing.Modified state prediction algorithm based on UKF［J］.Journal of Systems Engineering and Electronics，2013，24（1）：135-140.

［4］马春来，朱立新，孟龚.混合SRUKF在GPS／INS组合导航中的应用［J］.电子信息对抗技术，2013，28（2）：24-29.

［5］HU Haidong，HUANG Xianlin.SINS／CNS／GPS integrated navigation algorithm based on UKF［J］.Journal of Systems Engineering and Electronics，2010，21（1）：102-109.

［6］GUSTAFSSON F，HENDEBY G.Some relations between extended and unscented Kalman filters［J］.IEEE Transactions on Signal Processing，2012，60（2）：545-555.

［7］DENG Fang，CHEN Jie，CHEN Chen.Adaptive unscented Kalman filter for parameter and state estimation of nonlinear high-speed objects［J］.Journal of Systems Engineering and Electronics，2013，24（4）：655-665.

［8］陈剑，孙金海，李金海，等.GPS／AHRS紧耦合系统中改进的SRUKF算法［J］.哈尔滨工程大学学报，2012，33（10）：1300-1303.

［9］田易，孙金海，李金海，等.航姿参考系统中一种自适应卡尔曼滤波算法［J］.西安电子科技大学学报：自然科学版，2011，38（6）：103-107.

［10］JWO Dahjing，YANG Chifan，CHUANG ChihHsun，et al. Performance enhancement for ultra-tight GPS／INS integration using a fuzzy adaptive strong tracking unscented Kalman filter［J］.Journal of Nonlinear Dynamics，2013，73（1）：377-395.

（编辑王小唯）

A novel fuzzy adaptive SRUKF algorithm in tightly-coupled INS／GPS systems

YUAN Jianguo，YUAN Yantao，LIU Feilong，PANG Yu，LIN Jinzhao
（Key Lab of Optical Fiber Communication Technology，Chongqing University of Posts and Telecommunications，400065 Chongqing，China）

In tightly-coupled Inertial Navigation System（INS）／Global Position System（GPS）systems，the filtering precision of Kalman filter（UKF）will be affected seriously by the calculation rounding error leading to the covariancematrix negative definiteness and the time-varying measurement noise in practical application.To deal with this problem，an adaptive square root unscented Kalman filter（NASRUKF）based on fuzzy control theory algorithm is proposed.This algorithm not only can ensure the covariancematrix non-negative definiteness，but also can track and adjust the time-varyingmeasurement noise.Experimental results show that the algorithm has a good adaptability for the time-varying noise with higher accuracy，which makes the system have higher stability and robustness in comparison with UKF algorithm.

INS／GPS；Kalman filter；SRUKF；fuzzy control；adaptive

TP23

：A

：0367-6234（2015）11-0108-05

10.11918／j.issn.0367-6234.2015.11.019

2014-11-08.

重庆高校创新团队建设计划；国家自然科学基金项目（61472464）；重庆邮电大学（重庆市）研究生科研创新项目（CYS14144）.

袁建国（1968—），男，教授.

袁建国，yyyyjg＠126.com.