一种新型模糊自适应Kalman滤波器在组合导航中的应用

万振塬杨功流涂勇强(1.北京航空航天大学仪器科学与光电工程学院北京1001912.惯性技术国防重点实验室北京100191)

一种新型模糊自适应Kalman滤波器在组合导航中的应用

万振塬1，2，杨功流1，2，涂勇强1，2
(1.北京航空航天大学仪器科学与光电工程学院，北京100191；2.惯性技术国防重点实验室，北京100191)

针对光纤陀螺捷联惯导(FOG SINS)／GPS组合导航系统实际工作环境中，由于系统噪声与量测噪声模型发生变化而带来的滤波器发散的问题，提出一种新型模糊自适应Kalman滤波器(FSHAKF)。通过引入IMU精度因子与GPS水平精度因子，构造模糊推理系统(FIS)，实时更新自适应参数，有效地解决了传统Sage⁃Husa自适应滤波器(SHAKF)估计模型不准确、系统噪声与量测噪声无法同时估计以及滤波器长时间易发散的问题。仿真实验表明，本文提出的FSHAKF算法相较于SHAKF算法，估计精度得到明显提高，且避免了滤波器的发散。

捷联惯导系统；组合导航；模糊推理系统；自适应Kalman滤波

0 引言

Kalman滤波器已经广泛应用于光纤陀螺捷联惯导(FOG SINS)／GPS组合导航系统中，但是在实际的工程应用中，大多数系统的量测噪声和系统噪声预先都是未知的或者不确定的，即使建立了正确的系统模型，在工作过程中也会存在一些不确切因素可能导致数学模型摄动，例如环境温度变化繁复、GPS信号不稳定导致光纤陀螺器件噪声与GPS量测噪声发生变化。在此情况下进行滤波很有可能导致滤波发散［1］。

自适应滤波是一种具有抑制滤波器发散的滤波方法。往往通过对系统噪声方差阵Q及量测噪声方差阵R进行实时估计，然后反馈给滤波器增益，进而适应复杂的噪声环境。目前最常用的滤波方法是SHAKF算法，相关研究发现该算法不能同时对系统噪声与量测噪声进行估计，否则容易造成滤波器发散［2］。针对这一问题，一些学者提出了简化的SHAKF算法，但是并未从根本上解决噪声方差阵Q、R在新息中的复杂耦合关系，且易造成滤波模型不准确，最终影响组合导航精度［3⁃5］。另一方面，一些学者通过监测新息理论值与实测值，应用模糊推理系统不断调整量测噪声的加权来修正滤波估计结果［6⁃7］，但并未深入讨论模糊推理系统在系统噪声与量测噪声模型同时变化的复杂情况下的使用方法。

在以上理论研究的基础上，本文提出了一种新型的模糊自适应Kalman滤波器(FSHAKF)。首先对SHAKF进行简化处理，保证滤波器不发散。然后，通过构造IMU精度因子与GPS水平精度因子，建立模糊推理系统，实时调整自适应参数。FSHAKF准确地发挥了新息的作用，解决了简化处理带来的模型不准确问题，提高了组合导航系统状态估计精度。

1 Sage-Husa自适应Kalman滤波

1.1 SHAKF基本原理

利用时变噪声统计估计器对量测噪声和系统噪声的统计特性进行实时的在线修正和估计，与此同时通过量测信息进行迭代滤波，这种方法被称为Sage⁃Husa自适应滤波算法。

假设系统状态方程和量测方程可离散化成如下形式：

其中，Xk、Zk分别为状态量和量测量；Φk，k－1为k－1时刻到k时刻系统状态一步转移矩阵；Hk为k时刻量测矩阵；Wk和Vk为k时刻离散型Gauss白噪声：Wk～WN(qk，Qk)，Vk～WN(rk，Rk)，且假设这两种噪声互不相关。此处不加证明地给出时变噪声统计估计方程：

式(2)中给出的是其递推形式，b为遗忘因子。Sage⁃Husa自适应滤波理论上能在线同时估计系统噪声和量测噪声，但从式(2)可知，它们都依赖新息向量ek，即ek的异常变化同时影响和的计算，不能保证滤波估计的准确性，只能在已知时估计出，或者在已知时估计出。另外，由于式(2)中、中的减号项存在，不能保证和的半正定性和正定性，而从算法分析中可看出，由于减法的存在会使估计结果失去这种性质，从而造成滤波发散。

1.2 简化的SHAKF

通过上文分析可知，为了抑制滤波发散，必须保证和的半正定性和正定性。因此，将式(2)中和的迭代部分改写为：

即对迭代算式进行简化，去除了影响和的半正定性和正定性的减号项－Φk／k－1Px，k－1ΦTk／k－1、－HkPx，k／k－1HTk。

由式(2)可知，和的值主要取决于上一时刻的系统噪声和量测噪声以及当前时刻新息向量ek，而减号项对和的取值影响不大。因此，去除减号项只会对滤波精度产生一定影响。反而，如果和不能保证半正定性，则会使滤波发散。实际应用中，滤波算法的稳定性与可靠性更有价值。鉴于此，相关学者选择牺牲一定滤波精度以保证滤波器的稳定性。

2 模糊自适应Kalman滤波

2.1 FSHAKF基本原理

为了保证和的正定性，上一小节中将其迭代算式做了简化处理。但是这样会导致估计不准确，最终影响滤波精度。另外可以推出信息均方误差理论值满足：

即新息向量ek中耦合着系统噪声及量测噪声的影响，若采用此新息向量同时对和进行估计，其结果是不准确的。目前这方面的研究集中在如何抑制滤波器发散上，对于滤波精度方面，往往默认其受系统噪声影响较小，不再做进一步处理或者直接省去对的更新。

为了实现在抑制滤波器发散的同时保证估计精度，首先引入卫星导航系统水平精度因子(PDOP)及IMU精度因子(FT)。

卫星导航系统水平精度因子(PDOP)可由GPS输出直接获得，PDOP的大小直接反映出GPS此时量测噪声的水平。参考文献[8]，GPS的测距误差会引起用户的位置误差∂P，用户的位置误差协方差为：

其中，σ为GPS距离测量的方差，x、y、z代表卫星在地球坐标系下的几何位置，t为系统时间。

在FOG SINS／GPS组合导航过程中，光纤陀螺、石英加速度计噪声受温度影响较大。尽管可以事前对其进行温度补偿，但并不能完全保证系统噪声模型不发生变化。通过分析惯性器件的温度补偿结果，建立温补残差与温度及温度相关量之间的关系，对温补残差的均方差Δtemp二次建模如下：

采用最小二乘拟合方法求解出温度T，温度变化率及乘积项的权值系数，对温补残差均方差做归一化处理得到IMU精度因子FT∈(0，1)。

2.2 模糊推理系统构建

本文采用Mamdani型模糊逻辑系统，其规则只有4条，便于实现。常用的设计过程可分为3部分：模糊化过程、模糊控制规则生成过程、反模糊化的过程。

本文设计的模糊推理系统(FIS)的输入为FT、PDOP，输出为，为多输入单输出系统。输入输出模糊集分别为：(GPS精度高，GPS精度低)；(IMU精度高，IMU精度低)；(Increase，Maintain，De⁃crease)。

依据IMU温度特性及GPS精度分布规律设计隶属度函数；解模糊化方法选用重心法，即输出取隶属函数曲线围成区域面积的重心。此FIS系统输入输出特性曲线如图2所示。将FIS每次运算得到的带入式(3)，对、进行自适应调整。

2.3 FSHAKF在组合导航系统中的应用

本文以FOG SINS／GPS组合导航系统为研究对象，采用FOG SINS为主导航系统，利用GPS提供的速度和位置信息作为外观测量。

系统状态方程可以写成以下形式：

式中，X(t)＝ ［ϕEϕNϕUδVEδVNδLδλ εxεyεzxyz］T为13维状态向量，E、N、U分别代表东、北、天三个轴向，ϕE、ϕN、ϕU为姿态误差，δVE、δVN为速度误差，δL、δλ为纬度误差和经度误差，εx、εy、εz为陀螺随机常值漂移，x、y、z为加速度计随机常值零偏。F(t)为系数矩阵，W(t)为系统噪声矩阵。

量测方程为：

式中，H(t)为系数矩阵，υ(t)为量测噪声［8］。图3为模糊自适应滤波器在组合导航系统中的应用原理图。有量测更新时，IMU精度因子与GPS水平精度因子被送入模糊控制器，经过模糊处理得到自适应调节因子，并以此参数对SHAKF进行自适应修正。经过修正的SHAKF对新息进行处理估计出状态量，以此校正组合导航系统的姿态、速度、位置输出。

3 仿真验证与结果分析

假设载体的初始位置为北纬40.120662°，东经116.344311°，高度为50m，导航时间为29min，SINS采样频率为100Hz，GPS输出频率(滤波频率) 为1Hz。在Matlab仿真环境下设定载体运动状态，包括转弯、加减速、匀速直行，具体的载体运动轨迹如图4所示。

设置组合导航系统初始参数为：SINS惯性器件陀螺仪的常值漂移为0.05(°)／h，随机漂移为0.01(°)／h ，加速度计的常值零偏为150μg，随机漂移为30μg·s；GPS速度测量误差0.1m／s，位置测量误差水平2m。分别采用传统SHAKF算法、简化的SHAKF算法和本文提出的FSHAKF算法，对以下两种噪声条件进行仿真实验。

(1)量测噪声改变

在仿真进行到5000s时，GPS量测误差逐渐变为初值的6倍；11000s时，GPS量测误差逐渐变为初值的4倍。SINS系统噪声保持不变。仿真结果如图5所示。

(2)系统噪声、量测噪声同时改变

仿真进行到5000s时，设置的SINS系统噪声缓慢变为初值的7倍，GPS量测误差逐渐变为初值的6倍；11000s时，GPS量测误差逐渐变为初值的4倍；14000s时，SINS系统噪声缓慢变为初值的5倍。仿真结果如图6所示。

分析两组仿真结果发现：在系统噪声和量测噪声发生较大变化时，采用本文提出的FSHAKF算法可较快地收敛到相应量测噪声对应的状态估计精度，且多次实验未出现滤波器发散情况；而采用简化的SHAKF算法的估计精度较低，稳定性较差，甚至有发散趋势。

另外，采用传统SHAKF滤波器完成上述仿真条件实验时，发现P阵经常会快速增大，滤波器发散严重。因篇幅所限，本文未给出该算法滤波曲线。

4 结论

[1] Crisan D，Obanubi O.Particle filters with random resampling times[J].Stochastic Processes and their Applications，2011，122(4):1332⁃1368.

[2] 李旦，秦永元，梅春波.组合导航自适应卡尔曼滤波改进算法研究[J].测控技术，2011，30(3):114⁃116. LI Dan，QIN Yong⁃yuan，MEI Chun⁃bo.An improved a⁃daptive Kalman filter algorithm for SINS／GPS integrated navigation system[J].Measurement＆Control Technology，2011，30(3):114⁃116.

[3] 赵龙，吴康.新型自适应Kalman滤波算法及其应用[J].压电与声光，2009，31(6):908⁃911. ZHAO Long，WU Kang.A new type of adaptive Kalman filtering algorithm and its application[J].Piezoelectrics＆Acoustooptics，2009，31(6):908⁃911.

[4] 宋杰，晋帆.组合导航中抑制自适应滤波发散的研究[J].弹箭与制导学报，2012，32(2):46⁃48. SONG Jie，JIN Fan.The study on restrain of adaptive fil⁃tering divergence for integrated navigation[J].Journal of Projectiles Rockets Missiles and Guidance，2012，32(2): 46⁃48.

[5] Venkataraman V，Fan G，Havlicek J P，et al. Adaptive Kalman filtering for histogram⁃based appearance learning in infrared imagery[J].IEEE Transactions on Image Pro⁃[6] cessing，2012，21(11):4622⁃4635.

[6] Gao X D，You D Y，Katayama S. Seam tracking moni⁃toring based on adaptive Kalman filter embedded Elman neural network during high⁃power fiber laser welding[J]. IEEE Transactions on Industrial Electronics，2012，59 (11):4315⁃4325.

[7] 聂浩翔，关劲，卞鸿巍.模糊自适应Kalman滤波在INS／GPS组合导航系统中的应用[J].舰船电子工程，2011，31(1):67⁃70. NIE Hao⁃xiang，GUAN Jin，BIAN Hong⁃wei.Fuzzy adap⁃tive Kalman filtering in INS／GPS integrated navigation system[J].Ship Electronic Engineering，2011，31(1): 67⁃70.

[8] 袁信，余济祥，陈哲.导航系统[M].北京:航空工业出版社，1993. YUAN Xin，YU Ji⁃xiang，CHEN Zhe.Navigation system [M].Beijing:Aviation Industry Press，1993.

Application of a New Fuzzy Adaptive Kalman Filter to Integrated Navigation

WAN Zhen⁃yuan1，2，YANG Gong⁃liu1，2，TU Yong⁃qiang1，2
(1.School of Instrumentation Science and Opto⁃electronics Engineering，Beihang University，Beijing 100191; 2.Science and Technology on Inertial Laboratory，Beijing 100191)

In order to solve the filter divergence phenomenon brought by changes of system noise model and measure⁃ment noise model，while FOG SINS／GPS integrated navigation system is working in actually environment，this paper presents a new fuzzy adaptive Kalman filter.By constructing fuzzy inference system(FIS)to update adaptive parameter in real time with the IMU accuracy factor and GPS horizontal accuracy factor，FSHAKF solve these problems of the traditional SHAKF，which contains inaccurate estimation model，system noise and measurement noise immeasurable at the same time and easy di⁃vergent filter while working for a long time.Simulation results show that，FSHAKF algorithm proposed by this paper compared to SHAKF algorithm，obtains higher estimation accuracy significantly and avoids the divergence of the filter effectively.

strapdown inertial navigation system(SINS);integrated navigation;fuzzy inference system(FIS); adaptive Kalman filter

U<666.1 文献标志码：A class="emphasis_bold">666.1 文献标志码：A 文章编号：1674⁃5558(2017)03⁃01294666.1 文献标志码：A

1674⁃5558(2017)03⁃01294

A 文章编号：1674⁃5558(2017)03⁃01294

10.3969／j.issn.1674⁃5558.2017.02.015

万振塬，男，硕士，研究方向为导航、制导与控制。

2016⁃07⁃21

国家自然科学基金(编号：61340044，11202010)

本文提出的FSHAKF算法从一个全新的角度解决了噪声方差阵Q、R在新息ek中的复杂耦合关系，实现了二者同时在线估计，在抑制常规SHAKF算法及其简化算法易发散的前提下，提高了滤波估计精度，进而提高组合导航精度。

需要特别指出的是，本文设计模糊自适应Kalman滤波器是针对系统噪声受温度影响较大的FOG SINS／GPS组合导航系统。而仿真条件A情况下的实验结果表明，FSHAKF同样适用于系统噪声较为稳定的组合导航系统，具有一定的普适性。