大地测量与地球动力学

传感器	误差参数	工作周期
陀螺仪	2°/h(常值漂移) 0.2°/$\sqrt{\rm h}$(随机游走误差)	0.02 s
加速度计	10^－5g(常值漂移) 0.5×10^－6g/$\sqrt{\mathrm{Hz}}$(随机游走误差)	0.02 s
GNSS	8 m(位置误差) 0.2 m/s(速度误差)	1 s

传感器	误差参数	工作周期
陀螺仪	2°/h(常值漂移) 0.2°/$\sqrt{\rm h}$(随机游走误差)	0.02 s
加速度计	10^－5g(常值漂移) 0.5×10^－6g/$\sqrt{\mathrm{Hz}}$(随机游走误差)	0.02 s
GNSS	8 m(位置误差) 0.2 m/s(速度误差)	1 s

基于双调节因子的GNSS/SINS组合导航Sage-Husa自适应滤波算法

林雪原 ¹ , 孙炜玮 ²^,* , 孙晓范 ¹

大地测量与地球动力学 | 现代大地测量 2025,45(7): 699-704

收起

大地测量与地球动力学 |现代大地测量 2025 , 45 (7) : 699 -704

基于双调节因子的GNSS/SINS组合导航Sage-Husa自适应滤波算法

全屏

[Author(id=1322506628830703716, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, orderNo=0, firstName=null, middleName=null, lastName=null, nameCn=null, orcid=null, stid=null, country=null, authorPic=null, dead=0, email=linxy_ytcn@126.com, emailSecond=null, emailThird=null, correspondingAuthor=0, authorType=1, ext={EN=AuthorExt(id=1322506628914589798, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506628830703716, language=EN, stringName=Xueyuan LIN, firstName=Xueyuan, middleName=null, lastName=LIN, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=¹, address=1 School of Artificial Intelligence, Shandong Vocational University of Foreign Affairs, 788 Changjiang Road, Weihai 264504, China, bio={"content":"

LIN Xueyuan, PhD, professor, majors in integrated navigation and its information fusion, E-mail: linxy_ytcn@126.com

"}, bioImg=null, bioContent=

LIN Xueyuan, PhD, professor, majors in integrated navigation and its information fusion, E-mail: linxy_ytcn@126.com

, aboutCorrespAuthor=null), CN=AuthorExt(id=1322506629006864487, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506628830703716, language=CN, stringName=林雪原, firstName=null, middleName=null, lastName=null, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=¹, address=1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504, bio={"content":"

林雪原，博士，教授，主要从事组合导航及其信息融合研究，E-mail: linxy_ytcn@126.com

"}, bioImg=null, bioContent=

林雪原，博士，教授，主要从事组合导航及其信息融合研究，E-mail: linxy_ytcn@126.com

, aboutCorrespAuthor=null)}, companyList=[AuthorCompany(id=1322506628612599901, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, xref=null, ext=[AuthorCompanyExt(id=1322506628625182814, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628612599901, language=EN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=1 School of Artificial Intelligence, Shandong Vocational University of Foreign Affairs, 788 Changjiang Road, Weihai 264504, China), AuthorCompanyExt(id=1322506628633571423, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628612599901, language=CN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504)])]), Author(id=1322506629094944873, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, orderNo=1, firstName=null, middleName=null, lastName=null, nameCn=null, orcid=null, stid=null, country=null, authorPic=null, dead=0, email=s353375092@126.com, emailSecond=null, emailThird=null, correspondingAuthor=1, authorType=1, ext={EN=AuthorExt(id=1322506629212385387, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506629094944873, language=EN, stringName=Weiwei SUN, firstName=Weiwei, middleName=null, lastName=SUN, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=²^,*, address=2 Naval Aeronautical University, Yantai 264001, China, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null), CN=AuthorExt(id=1322506629287882860, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506629094944873, language=CN, stringName=孙炜玮, firstName=null, middleName=null, lastName=null, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=²^,*, address=2 海军航空大学, 山东省烟台市, 264001, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null)}, companyList=[AuthorCompany(id=1322506628730040416, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, xref=null, ext=[AuthorCompanyExt(id=1322506628738429025, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628730040416, language=EN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=2 Naval Aeronautical University, Yantai 264001, China), AuthorCompanyExt(id=1322506628746817634, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628730040416, language=CN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=2 海军航空大学, 山东省烟台市, 264001)])]), Author(id=1322506629363380334, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, orderNo=2, firstName=null, middleName=null, lastName=null, nameCn=null, orcid=null, stid=null, country=null, authorPic=null, dead=0, email=null, emailSecond=null, emailThird=null, correspondingAuthor=0, authorType=1, ext={EN=AuthorExt(id=1322506632911761520, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506629363380334, language=EN, stringName=Xiaofan SUN, firstName=Xiaofan, middleName=null, lastName=SUN, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=¹, address=1 School of Artificial Intelligence, Shandong Vocational University of Foreign Affairs, 788 Changjiang Road, Weihai 264504, China, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null), CN=AuthorExt(id=1322506633025007729, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, authorId=1322506629363380334, language=CN, stringName=孙晓范, firstName=null, middleName=null, lastName=null, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=¹, address=1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null)}, companyList=[AuthorCompany(id=1322506628612599901, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, xref=null, ext=[AuthorCompanyExt(id=1322506628625182814, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628612599901, language=EN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=1 School of Artificial Intelligence, Shandong Vocational University of Foreign Affairs, 788 Changjiang Road, Weihai 264504, China), AuthorCompanyExt(id=1322506628633571423, tenantId=1146029695717560320, journalId=1281212831689347082, articleId=1322506620114939972, companyId=1322506628612599901, language=CN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504)])])]

林雪原¹, 孙炜玮²^,*, 孙晓范¹

作者信息

1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504

2 海军航空大学, 山东省烟台市, 264001

通讯作者:

孙炜玮，副教授，主要从事组合导航技术研究，E-mail: s353375092@126.com

SUN Weiwei, associate professor, majors in integrated navigation technology, E-mail: s353375092@126.com

作者简介:

林雪原，博士，教授，主要从事组合导航及其信息融合研究，E-mail: linxy_ytcn@126.com

Sage-Husa Adaptive Filtering Algorithm for GNSS/SINS Integrated Navigation Based on Double Regulatory Factors

Xueyuan LIN¹, Weiwei SUN²^,*, Xiaofan SUN¹

Affiliations

1 School of Artificial Intelligence, Shandong Vocational University of Foreign Affairs, 788 Changjiang Road, Weihai 264504, China

2 Naval Aeronautical University, Yantai 264001, China

LIN Xueyuan, PhD, professor, majors in integrated navigation and its information fusion, E-mail: linxy_ytcn@126.com

出版时间: 2025-07-15 doi: 10.14075/j.jgg.2024.08.391

文章导航

摘要

收起

在GNSS/SINS组合导航系统中，当测量噪声方差发生变化时，Sage-Husa自适应滤波算法的测量噪声方差估计功能与故障检测功能有可能相互冲突。为解决该问题，首先，根据故障检测函数设计控制因子，对Sage-Husa自适应滤波算法的测量噪声方差估计模型进行在线调整；然后，根据Sage-Husa自适应滤波算法严重依赖遗忘因子的特性，设计动态遗忘因子以对测量噪声方差进行准确跟踪，进而提出一种基于双调节因子(控制因子和动态遗忘因子)的Sage-Husa自适应滤波(DRSHAKF)算法；最后，基于施加了容错功能的Sage-Husa自适应滤波(SHAKF)算法及DRSHAKF算法，进行组合导航系统的仿真实验。结果表明，相对于SHAKF算法，DRSHAKF算法可以将测量噪声方差估计功能与故障检测函数进行有机融合，充分利用有用的测量信息，进而提高系统滤波精度。

关键词

Sage-Husa算法 / 自适应滤波算法 / 组合导航系统 / 遗忘因子 / 控制因子

Abstract

收起

When the measurement noise variance changes in GNSS/SINS integrated navigation system, the estimation function of measurement noise variance and the fault detection function may conflict with each other in Sage-Husa adaptive filtering algorithm. In order to solve this problem, firstly, we design a control factor according to the fault detection function to adjust the estimation model of measurement noise variance of Sage-Husa adaptive filtering algorithm online. Then, according to the characteristics that Sage-Husa adaptive filtering algorithm relies heavily on forgetting factor, a dynamic forgetting factor is designed to accurately track the measurement noise variance, and a Sage-Husa adaptive Kalman filtering algorithm based on double regulatory factors(DRSHAKF) is proposed. Finally, the simulation experiment of integrated navigation system is carried out based on Sage-Husa adaptive Kalman filtering (SHAKF) algorithm with fault tolerance function and DRSHAKF algorithm. The results show that, compared with SHAKF algorithm, DRSHAKF algorithm can organically integrate measurement noise variance estimation function and fault detection function, make full use of useful measurement information, and improve the filtering accuracy.

Key words

Sage-Husa algorithm / adaptive filtering algorithm / integrated navigation system / forgetting factor / control factor

引用本文

林雪原, 孙炜玮, 孙晓范. 基于双调节因子的GNSS/SINS组合导航Sage-Husa自适应滤波算法. 大地测量与地球动力学, 2025 , 45 (7) : 699 -704 . DOI: 10.14075/j.jgg.2024.08.391

Xueyuan LIN, Weiwei SUN, Xiaofan SUN. Sage-Husa Adaptive Filtering Algorithm for GNSS/SINS Integrated Navigation Based on Double Regulatory Factors[J]. Journal of Geodesy and Geodynamics, 2025 , 45 (7) : 699 -704 . DOI: 10.14075/j.jgg.2024.08.391

正文

收起

GNSS/SINS组合导航系统在各种组合导航系统中由于性能优异而被认为是一种理想的组合导航系统，其常用的信息融合方法为卡尔曼滤波器^[1]。卡尔曼滤波器以准确的系统模型和先验噪声统计特性为基础，但在实际情况中，由于各种干扰因素的存在，使得噪声特性经常出现异常，进而导致滤波精度下降甚至发散^[2]。

自适应滤波是解决上述问题的有效方法，目前常用的、估计精度较高的自适应滤波方法有基于变分贝叶斯的自适应滤波方法^[3-4]和基于Sage-Husa的自适应滤波方法^[5]。相对于具有3个控制向量的变分贝叶斯自适应滤波方法，仅有1个控制参数(遗忘因子)的Sage-Husa自适应滤波方法具有算法简单、方便实现的特点。但Sage-Husa自适应滤波对噪声特性的估计性能严重依赖遗忘因子，为此，路小燕等^[6]和孙淑光等^[7]利用卡尔曼滤波过程中诸多矩阵及向量构建可变的遗忘因子，但该因子在滤波过程中有可能大于1而导致滤波发散；Song等^[8]针对测量噪声异常的情况，在目标跟踪的非线性滤波过程中引入带有衰减指数渐消因子的Sage-Husa自适应滤波，以提高纵向滤波精度；Huang等^[9]和曾庆化等^\]10\]将带有衰减指数渐消因子的Sage-Husa自适应滤波应用于组合导航系统中，可提高对测量噪声特性的估计精度，进而提高组合导航系统的滤波精度。

可以看出，上述研究仅针对Sage-Husa自适应滤波方法中的遗忘因子进行分析，然而组合导航系统不仅要准确地估计系统状态参数，还要具有高可靠性，或者具有较好的容错性。毫无疑问，当测量噪声异常时，一方面自适应滤波方法需要估计该异常噪声的统计特性，另一方面故障检测函数可能发出故障预警，即此时自适应滤波方法的信息估计功能与容错功能相互冲突。同时，需特别说明的是，在上述文献中，噪声统计特性多指噪声的方差或均方差，而假设噪声均值为0。为解决该问题，本文提出基于双调节因子的Sage-Husa自适应滤波方法，旨在将自适应滤波方法的信息估计功能与容错功能进行相互协调和有机融合，充分利用有用的测量信息，进而提高组合导航系统的滤波精度。

1 GNSS/SINS组合导航系统模型

收起

本文以基于位置、速度组合模式的GNSS/SINS松耦合组合导航系统为研究对象，其误差状态量采用SINS平台角误差、SINS速度误差、SINS位置误差、加速度计误差及陀螺误差，导航坐标系取东北天(ENU)地理坐标系，状态方程可表示为^[11]：

(1)

$\boldsymbol{X}_k=\boldsymbol{\varPhi}_{k, k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\varGamma}_{k-1} \boldsymbol{W}_{k-1}$

式中，

$\boldsymbol{X}_k=\left[\begin{array}{lllll}\boldsymbol{X}_1^{\mathrm{T}} & \boldsymbol{X}_2^{\mathrm{T}} & \boldsymbol{X}_3^{\mathrm{T}} & \boldsymbol{X}_4^{\mathrm{T}} & \boldsymbol{X}_5^{\mathrm{T}}\end{array}\right]^{\mathrm{T}}$

为k时刻的状态向量，其中

$\boldsymbol{X}_1=\left[\begin{array}{lll}\varphi_{\mathrm{E}} & \varphi_{\mathrm{N}} & \varphi_{\mathrm{U}}\end{array}\right]^{\mathrm{T}}$

为SINS平台角误差，

$\boldsymbol{X}_2=\left[\begin{array}{lll}\delta v_{\mathrm{E}} & \delta v_{\mathrm{N}} & \delta v_{\mathrm{U}}\end{array}\right]^{\mathrm{T}}$

为SINS速度误差，

$\boldsymbol{X}_3=\left[\begin{array}{lll}\delta \lambda & \delta L & \delta h\end{array}\right]^{\mathrm{T}}$

为SINS经度、纬度及高度误差，

$\boldsymbol{X}_4=\left[\begin{array}{lll}\varepsilon_x & \varepsilon_y & \varepsilon_z\end{array}\right]^{\mathrm{T}}$

为陀螺三轴误差的一阶马尔可夫过程，

$\boldsymbol{X}_5=\left[\begin{array}{lll}\nabla_x & \nabla_y & \nabla_z\end{array}\right]^{\mathrm{T}}$

为加速度计三轴误差的一阶马尔可夫过程；

$\boldsymbol{\varPhi}_{k, k-1}$

为状态转移矩阵；

$\boldsymbol{\varGamma}_{k-1}$

为系统噪声矩阵；

$W_{k-1}$

为系统噪声，且满足

$\boldsymbol{W}_k \sim N\left(0, \boldsymbol{Q}_k\right)$

。

测量方程可表示为：

(2)

$\boldsymbol{Z}_k=\boldsymbol{H}_k \boldsymbol{X}_k+\boldsymbol{V}_k$

式中，测量向量Z_k取SINS和GNSS各自三维位置、三维速度之间的差值；H_k为测量矩阵；测量噪声V_k近似为白噪声，且满足

$\boldsymbol{V}_k \sim N\left(0, \boldsymbol{R}_k\right) .$

。

2 改进的Sage-Husa自适应滤波算法

收起

2.1 组合导航系统的Sage-Husa算法

当W_k和V_k为统计特性不变的白噪声时，传统卡尔曼滤波器得到的估计可以达到最优。但在实际情况中，W_k和V_k难以满足上述假设。Sage-Husa自适应滤波算法可以摆脱上述假设，实时估计噪声的统计特性并获得状态参数的最优估计。然而传统Sage-Husa自适应滤波算法在应用时存在2个缺陷：无法同时动态地估计系统噪声方差矩阵Q_k及测量噪声方差矩阵R_k；应用于组合导航系统时，常存在滤波发散现象。

在组合导航系统中，考虑到系统噪声具有稳定性，而测量噪声方差矩阵对滤波的影响较为显著，为此，组合导航系统的Sage-Husa自适应滤波算法在对状态参数进行估计的同时仅对测量噪声进行估计。

目前，为克服上述缺陷，用于组合导航系统的Sage-Husa自适应滤波算法可表示为^[5]：

(3)

$\hat{\boldsymbol{X}}_{k \mid k-1}=\boldsymbol{\varPhi}_{k, k-1} \hat{\boldsymbol{X}}_{k-1 \mid k-1}$

(4)

$\boldsymbol{P}_{k \mid k-1}=\boldsymbol{\varPhi}_{k, k-1} \boldsymbol{P}_{k-1 \mid k-1} \boldsymbol{\varPhi}_{k, k-1}^{\mathrm{T}}+\boldsymbol{\varGamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\varGamma}_{k-1}^{\mathrm{T}}$

(5)

$\boldsymbol{K}_k=\boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\hat{\boldsymbol{R}}_k\right)^{-1}$

(6)

$\boldsymbol{P}_{k \mid k}=\left(\boldsymbol{I}-\boldsymbol{K}_k \boldsymbol{H}_k\right) \boldsymbol{P}_{k \mid k-1}$

(7)

$\hat{\boldsymbol{X}}_{k \mid k}=\hat{\boldsymbol{X}}_{k \mid k-1}+\boldsymbol{K}_k\left(\boldsymbol{Z}_k-\boldsymbol{H}_k \hat{\boldsymbol{X}}_{k \mid k-1}\right)$

(8)

$\mathit{\pmb{ε}}_k=\boldsymbol{Z}_k-\boldsymbol{H}_k \hat{\boldsymbol{X}}_{k \mid k-1}$

(9)

$\hat{\boldsymbol{R}}_k=\left(1-d_k\right) \hat{\boldsymbol{R}}_{k-1}+d_k \cdot \operatorname{diag}\left(\mathit{\pmb{ε}}_k \cdot \mathit{\pmb{ε}}_k\right)$

式中，

$\hat{\boldsymbol{R}}_k$

为

$\boldsymbol{R}_k$

的估计值；diag(·)表示以某个向量为主对角线元素而产生对角矩阵的函数；各项参数的详细定义可参考文献[1]。

(10)

$d_k=\frac{1-b}{1-b^k}, k=1, 2, \cdots$

式中，b为遗忘因子(0 < b < 1)，b取值越小，估计结果跳变越剧烈；b取值越大，估计结果越平缓；b=1仅适用于估计平稳噪声。

根据式(10)可知，实际情况下测量噪声统计特性的变化规律无法预测，因此在滤波过程中需要动态调整遗忘因子的大小。在滤波过程中，可以设计故障检测函数对滤波状态进行判断，如判断为滤波异常则b取较小的值，随着时间的推移b应取较大的值以对测量噪声方差进行准确跟踪。本文选取的动态遗忘因子b可表示为：

(11)

$b(k)=b_0-a \cdot \mathrm{e}^{\frac{-\left(k-k_0\right)}{c}}$

式中，b₀ < 1且

$b_0 \cong 1$

以确保b≠1，本文取b₀=0.98；k₀为故障检测函数检测到测量噪声异常的时刻；c可以控制遗忘因子的变化速度，本文取c=5；a可以决定遗忘因子变化时的初始值，本文取a=0.1。

2.2 Sage-Husa算法的改进

如果关于式(1)和式(2)的系统假设成立，该假设也包括测量噪声统计特性已知且恒定不变，则测量值应满足均值为

$\hat{\boldsymbol{Z}}_{k \mid k-1}$

、方差为

$\boldsymbol{P}_{\hat{\mathbf{z}}_{k \mid k-1}}$

的高斯分布，即概率密度函数为：

(12)

$\begin{gathered}\rho\left(\boldsymbol{Z}_k\right)=N\left(\hat{\boldsymbol{Z}}_{k \mid k-1}, \boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)= \\\frac{\exp \left[-\frac{1}{2}\left(\boldsymbol{Z}_k-\hat{\boldsymbol{Z}}_{k \mid k-1}\right)^{\mathrm{T}}\left(\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)^{-1}\left(\boldsymbol{Z}_k-\hat{\boldsymbol{Z}}_{k \mid k-1}\right)\right]}{\sqrt{(2 \pi)^m\left|\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right|}}= \\\frac{\exp \left[-\frac{1}{2} \mathit{\pmb{ε}}_k^{\mathrm{T}}\left(\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)^{-1} \mathit{\pmb{ε}}_k\right]}{\sqrt{(2 \pi)^m\left|\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right|}}\end{gathered}$

式中，

$\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k\mid k-1}}=\boldsymbol{H}_k \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{R}_k ; \hat{\boldsymbol{Z}}_{k \mid k-1}=\boldsymbol{H}_k \hat{\boldsymbol{X}}_{k \mid k-1} ； \left|\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right|$

为

$\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}$

的行列式；m为测量向量的维数。

毫无疑问，若关于式(1)和式(2)的系统假设不成立，或者测量噪声方差处于动态变化过程中，则式(12)不再成立。为此，可以通过假设检验来检测实际测量与假设模型的兼容性。根据式(12)取检验统计量：

(13)

$\begin{gathered}r_k=\left(\boldsymbol{Z}_k-\hat{\boldsymbol{Z}}_{k \mid k-1}\right)^{\mathrm{T}}\left(\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)^{-1}\left(\boldsymbol{Z}_k-\hat{\boldsymbol{Z}}_{k \mid k-1}\right)= \\\mathit{\pmb{ε}}_k^{\mathrm{T}}\left(\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)^{-1} \mathit{\pmb{ε}}_k\end{gathered}$

式中，r_k满足自由度为m的卡方分布；ε_k为新息向量。

$\sqrt{\mathit{\pmb{ε}}_k^{\mathrm{T}}\left(\boldsymbol{P}_{\hat{\boldsymbol{Z}}_{k \mid k-1}}\right)^{-1} \mathit{\pmb{ε}}_k}$

表示马氏距离^[12]。

对于显著性水平α，选取卡方分布的一个分位数χ_α(也称为检测阈值)，且满足：

(14)

$P\left(\gamma_k>\chi_\alpha\right)=\alpha$

式中，P(·)表示随机事件的概率。当γ_k>χ_α时，认为测量异常。

当测量噪声方差变化速度较快时，根据式(9)得到的测量噪声方差会滞后于实际测量噪声方差。在该滞后期内，根据式(13)得到的检验统计量可能大于检测阈值，进而系统进入故障隔离状态。为消除该滞后期对滤波精度的影响并充分利用有用的测量信息，引入一个控制因子ω：

(15)

$\omega=\left\{\begin{array}{l}1, r_k \leqslant \chi_\alpha \\\frac{r_k}{\chi_\alpha}, r_k>\chi_\alpha\end{array}\right.$

利用其对式(9)中

$\hat{\boldsymbol{R}}_k$

进行如下调整^[13]：

(16)

$\hat{\boldsymbol{R}}_k^{\prime}=(\omega-1) \cdot \boldsymbol{H}_k \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\omega \cdot \hat{\boldsymbol{R}}_k$

则式(5)可表示为：

(17)

$\boldsymbol{K}_k=\boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \boldsymbol{P}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\hat{\boldsymbol{R}}_k^{\prime}\right)^{-1}$

以式(11)和式(15)所示的2种调节因子为基础，构建本文所提出的基于双调节因子的Sage-Husa自适应滤波(DRSHAKF)算法，其算法流程如图 1所示。

3 仿真结果及分析

收起

3.1 仿真条件

基于上述算法，对GNSS/SINS组合导航系统进行仿真验证。飞行轨迹如图 2所示，飞行总时间为3 600 s，飞行初始位置为(30.5°N、119.5°E，350 m)，初始速度为0。在导航传感器正常工作时，其仿真参数设置如表 1所示。

3.2 仿真结果及分析

以上述仿真条件为基础，在600~1 500 s及2 000~3 200 s两个不同时段内对GNSS测量信息分别施加测量噪声均方差突变与缓变2种情况，其他时段内GNSS正常工作，具体如图 3、图 4中黑实线所示。

对于相同的导航原始数据，本文分别进行基于DRSHAKF算法及施加容错功能的SHAKF算法的GNSS/SINS组合导航系统滤波性能分析。从图 3、图 4可以看出，当GNSS测量噪声均方差缓变时，DRSHAKF算法及SHAKF算法性能基本相当；但当GNSS测量噪声均方差突变时，DRSHAKF算法能够准确地跟踪该突变，而SHAKF算法跟踪到的突变时刻相比于突变起始时刻具有明显的滞后性。

上述现象可以通过基于2种算法的故障检测值曲线进行说明，具体如图 5所示。当GNSS测量噪声均方差突变时，基于SHAKF算法的故障检测值将在一段时间内(600~691 s)始终大于检测阈值，此时组合导航系统处于故障隔离状态，且测量噪声均方差的估计值保留前一时刻的结果；而基于DRSHAKF算法的故障检测值仅在GNSS测量噪声均方差突变的起始时刻(600 s)大于检测阈值，此时基于式(15)得到的控制因子ω>1，进而利用式(16)调整GNSS测量噪声方差估计值。当GNSS测量噪声均方差缓变时，除在2 045 s时刻外，基于2种算法的故障检测值均小于检测阈值，即2种算法在处理GNSS测量噪声均方差缓变时的性能相当；在2 045 s时刻，DRSHAKF算法的故障检测值大于检测阈值，同样可以利用式(15)和式(16)对GNSS测量噪声方差进行调整，进而保证DRSHAKF算法性能的稳定性。

基于DRSHAKF算法得到的控制因子变化曲线如图 6所示。

基于经过式(16)调整后的测量噪声方差

$\hat{\boldsymbol{R}}_k^{\prime}$

，根据图 1重新计算组合导航系统滤波过程中的故障检测函数，结果如图 7所示。由图可见，此时基于DRSHAKF算法的故障检测值始终小于检测阈值，无需对GNSS测量信息进行隔离，即DRSHAKF算法可以充分利用有用的测量信息，进而提高组合导航系统的滤波精度。

基于图 5中DRSHAKF算法故障检测值曲线，得到遗忘因子变化曲线，结果如图 8所示。由图可见，在600 s及2 045 s时刻，尤其是600 s时刻，当DRSHAKF算法判定系统故障后，利用式(11)控制遗忘因子取较小的值，使得DRSHAKF算法对测量噪声变化的适应能力增强，具体如图 3、图 4所示(600 s时刻)。

基于上述分析，图 9~11为基于2种算法的组合导航系统的位置误差、速度误差及姿态误差曲线。可以看出，当GNSS测量噪声均方差缓变时，基于2种算法的组合导航滤波精度相同；当GNSS测量噪声均方差突变时，基于DRSHAKF算法的组合导航滤波精度明显高于SHAKF算法；在其他时段内，基于2种算法的滤波精度相当。该结果的出现与图 3、图 4现象的发生是紧密关联的。

4 结语

收起

针对GNSS/SINS组合导航系统目前采用的Sage-Husa自适应滤波算法在自适应估计测量噪声统计特性时与故障检测相互冲突的问题，本文在设计故障检测函数的基础上，设计控制因子对Sage-Husa自适应滤波算法的测量噪声方差估计模型进行在线调整；并根据Sage-Husa自适应滤波算法对噪声特性的估计性能严重依赖遗忘因子的问题，构建动态遗忘因子，进而提出基于双调节因子的Sage-Husa自适应滤波(DRSHAKF)算法。控制因子在DRSHAKF中可解决Sage-Husa自适应滤波算法的测量噪声方差估计功能与故障检测功能相互冲突的问题，动态遗忘因子在DRSHAKF中可精确地跟踪突变、缓变和平稳的测量噪声均方差。

将DRSHAKF算法应用于GNSS/SINS组合导航系统的仿真结果表明，该算法能够将测量噪声方差估计功能与故障检测功能相互促进、相互融合，相对于传统算法，可以有效提高导航系统的滤波精度及可靠性。

基金

收起

国家自然科学基金(62076249)
山东省自然科学基金(ZR2020MF154)

参考文献

收起

文献

收起

参考文献引证文献

排序方式：

Wang

,Xu

X S

,Hou

L H

.An Improved Adaptive Kalman Filter for Underwater SINS/DVL System[J].Mathematical Problems in Engineering,2020,2020

S Q

,Zhou

H Y

,Wang

J Q

,et al.SINS/CNS/GNSS Integrated Navigation Based on an Improved Federated Sage-Husa Adaptive Filter[J].Sensors,2019,19(17)

荆蕾,孙炜玮,乔玉新,等.GNSS/SINS组合导航系统的自适应UKF算法[J].大地测量与地球动力学,2023,43(3):255-258 http://www.jgg09.com/CN/Y2023/V43/I3/255

Jing

Lei

,Sun

Weiwei

,Qiao

Yuxin

,et al.Adaptive UKF Algorithm for GNSS/SINS Integrated Navigation System[J].Journal of Geodesy and Geodynamics,2023,43(3):255-258 http://www.jgg09.com/CN/Y2023/V43/I3/255

林雪原,刘丽丽,董云云,等.改进的GNSS/SINS组合导航系统自适应滤波算法[J].武汉大学学报: 信息科学版,2023,48(1):127-134

Lin

Xueyuan

,Liu

Lili

,Dong

Yunyun

,et al.Improved Adaptive Filtering Algorithm for GNSS/SINS Integrated Navigation System[J].Geomatics and Information Science of Wuhan University,2023,48(1):127-134

胡晓梅,潘新龙,刘成铭.GNSS/SINS组合导航的改进Sage-Husa自适应滤波算法[J].大地测量与地球动力学,2023,43(8):791-794 http://www.jgg09.com/CN/Y2023/V43/I8/791

Xiaomei

,Pan

Xinlong

,Liu

Chengming

.An Improved Sage-Husa Adaptive Filtering Algorithm for GNSS/SINS Integrated Navigation[J].Journal of Geodesy and Geodynamics,2023,43(8):791-794 http://www.jgg09.com/CN/Y2023/V43/I8/791

路小燕,杨柳庆,郭锦,等.改进的Sage-Husa自适应滤波算法在MEMS航姿参考系统中的应用[J].导航与控制,2019,18(2):105-112

https://doi.org/10.3969/j.issn.1674-5558.2019.02.016

Xiaoyan

,Yang

Liuqing

,Guo

Jin

,et al.Application of Improved Sage-Husa Adaptive Filtering Algorithm in MEMS AHRS[J].Navigation and Control,2019,18(2):105-112

https://doi.org/10.3969/j.issn.1674-5558.2019.02.016

孙淑光,陈建达.改进调节因子的GPS/SINS组合导航自适应滤波算法[J].航天控制,2022,40(4):53-60

https://doi.org/10.3969/j.issn.1006-3242.2022.04.008

Sun

Shuguang

,Chen

Jianda

.GPS/SINS Integrated Navigation Adaptive Filtering Algorithm with Improved Adjustment Factor[J].Aerospace Control,2022,40(4):53-60

https://doi.org/10.3969/j.issn.1006-3242.2022.04.008

Song

S P

,Wu

.Motion State Estimation of Target Vehicle under Unknown Time-Varying Noises Based on Improved Square-Root Cubature Kalman Filter[J].Sensors,2020,20(9)

Huang X L, Chen G S, Liu Z P. Sage Husa Adaptive Integrated Navigation Algorithm Based on Variable Fading Factor[C]. 2020 International Conference on Computer Network, Electronic and Automation(ICCNEA), Xi'an, 2020

曾庆化,赵天钰,赵宾,等.基于指数渐消遗忘因子的组合导航自适应滤波算法[J].中国惯性技术学报,2021,29(3):307-313

Zeng

Qinghua

,Zhao

Tianyu

,Zhao

Bin

,et al.Adaptive Kalman Filter Algorithm Based on Exponential Attenuating Factor for Integrated Navigation System[J].Journal of Chinese Inertial Technology,2021,29(3):307-313

林雪原,李荣冰,高青伟.组合导航及其信息融合方法[M].北京:国防工业出版社,2017

Lin

Xueyuan

,Li

Rongbing

,Gao

Qingwei

.Integrated Navigation and Its Information Fusion Method[M].Beijing:National Defense Industry Press,2017

高社生,洪根元,高广乐,等.基于马氏距离的联邦卡尔曼滤波在SINS/SRS/CNS导航中的应用[J].中国惯性技术学报,2021,29(2):141-146

Gao

Shesheng

,Hong

Genyuan

,Gao

Guangle

,et al.Mahalanobis Distance Based Federated Kalman Filter for SINS/SRS/CNS Integrated Navigation System[J].Journal of Chinese Inertial Technology,2021,29(2):141-146

Chang

G B

.Robust Kalman Filtering Based on Mahalanobis Distance as Outlier Judging Criterion[J].Journal of Geodesy,2014,88(4):391-401

2025年第45卷第7期

引用本文

BibTeX

文章信息

doi: 10.14075/j.jgg.2024.08.391

接收时间：2024-08-15
首发时间：2026-10-29
出版时间：2025-07-15

补充材料

相关文章

文章信息

作者

出版历史

收稿日期：2024-08-15

基金

National Natural Science Foundation of China(62076249)

国家自然科学基金(62076249)

Natural Science Foundation of Shandong Province(ZR2020MF154)

山东省自然科学基金(ZR2020MF154)

作者信息

1 山东外事职业大学人工智能学院, 山东省威海市长江路788号, 264504

2 海军航空大学, 山东省烟台市, 264001

通讯作者:

孙炜玮，副教授，主要从事组合导航技术研究，E-mail: s353375092@126.com

SUN Weiwei, associate professor, majors in integrated navigation technology, E-mail: s353375092@126.com

参考文献

分享链接

https://castjournals.cast.org.cn/joweb/ddcl/CN/10.14075/j.jgg.2024.08.391

分享至

全文二维码

扫描看全文

引用本文

BibTeX

本文的引用情况

2种不同金属材料的力学参数

科 Family	属数 Number of genus	种数 Number of species	占总种数比例 Percentage of total species (%)	属 Genus	种数 Number of species	占总种数比例 Percentage of total species (%)
鹅膏菌科Amanitaceae	2	11	5.26	鹅膏菌属 Amanita	10	4.78
小菇科 Mycenaceae	2	12	5.74	丝盖伞属 Inocybe	5	2.39
多孔菌科 Polyporaceae	8	14	6.70	蜡蘑属 Laccaria	5	2.39
红菇科 Russulaceae	3	23	11.00	小皮伞属 Marasmius	6	2.87
				小菇属 Mycena	11	5.26
				光柄菇属 Pluteus	5	2.39
				红菇属 Russula	17	8.13
				栓菌属 Trametes	5	2.39

关闭全屏

BibTeX
EndNote
RefWorks
TxT