CN111175795B - Gnss/ins组合导航系统的两步抗差滤波方法及系统 - Google Patents

Gnss/ins组合导航系统的两步抗差滤波方法及系统 Download PDF

Info

Publication number
CN111175795B
CN111175795B CN202010004513.5A CN202010004513A CN111175795B CN 111175795 B CN111175795 B CN 111175795B CN 202010004513 A CN202010004513 A CN 202010004513A CN 111175795 B CN111175795 B CN 111175795B
Authority
CN
China
Prior art keywords
matrix
gnss
representing
ins
fading factor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010004513.5A
Other languages
English (en)
Other versions
CN111175795A (zh
Inventor
易清明
陆景龙
石敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jinan University
Original Assignee
Jinan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jinan University filed Critical Jinan University
Priority to CN202010004513.5A priority Critical patent/CN111175795B/zh
Publication of CN111175795A publication Critical patent/CN111175795A/zh
Application granted granted Critical
Publication of CN111175795B publication Critical patent/CN111175795B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/421—Determining position by combining or switching between position solutions or signals derived from different satellite radio beacon positioning systems; by combining or switching between position solutions or signals derived from different modes of operation in a single system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/07—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing data for correcting measured positioning data, e.g. DGPS [differential GPS] or ionosphere corrections
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种GNSS/INS组合导航系统的两步抗差滤波方法及系统，该方法的步骤为：根据新息构建渐消因子矩阵，根据残差构建增益系数矩阵，构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据；检测动力学模型是否异常，若超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新；检测观测量是否正常，若超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新；更新卡尔曼滤波过程，输出GNSS/INS组合导航结果。本发明可充分利用组合导航系统的冗余信息，同时从标量因子拓展到对角线矩阵，减少了算法复杂度，增强了组合导航系统的定位精度、跟踪性能和稳定性。

Description

GNSS/INS组合导航系统的两步抗差滤波方法及系统
技术领域
本发明涉及导航技术领域，具体涉及一种GNSS/INS组合导航系统的两步抗差滤波方法及系统。
背景技术
全球卫星导航系统(Global Navigation Satellite System,GNSS)在军事、生活服务扮演重要的角色，它可以全天时、全天候进行全球定位和授时，但在森林、多山地区、隧道、城市峡谷等场景下，存在信号被遮挡、导航不连续的问题。另一方面，惯性导航系统(Inertial Navigation System,INS)理论上可以自主定位，不依赖外部信息，但是定位误差随时间累积，需要外部信息进行误差校正才能保持连续、高精度导航。GNSS可以作为外部信息提供给INS，消除时间累积误差；同时，INS可以在GNSS信号盲区持续工作；卡尔曼滤波器被广泛应用于GNSS/INS组合导航中，它在系统模型和噪声模型确定的情况下，利用贝叶斯估计理论得到最小均方误差(MMSE)意义下的最优估计，然而，GNSS/INS组合导航系统模型为非线性和噪声不是高斯白噪声，这将影响参数估计的精度和可靠性，严重时甚至导致滤波结果发散。现有的卡尔曼滤波方法采用单渐消因子以时间指数渐消加权平均代替集总平均，可以减少噪声粗差的干扰，但是不能对各个通道进行精确滤波控制，当系统模型偏差或噪声模型不确定时，卡尔曼滤波器无法实现最优滤波效果,严重时甚至导致滤波发散。
发明内容
为了克服现有技术存在的缺陷与不足，解决系统噪声和测量噪声存在粗差的问题，本发明提供一种GNSS/INS组合导航系统的两步抗差滤波方法及系统，可以同时减小系统噪声和测量噪声的影响，该方法通过渐消因子矩阵和增益系数矩阵分别调整误差协方差和增益矩阵，分步限制噪声对系统的干扰，提高组合导航系统的定位精度和抗干扰能力。
为了达到上述目的，本发明采用以下技术方案：
本发明提供一种GNSS/INS组合导航系统的两步抗差滤波方法，包括下述步骤：
根据新息构建渐消因子矩阵，根据残差构建增益系数矩阵；
构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据；
检测动力学模型是否异常，若判定为超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新；
检测观测量是否正常，若判定为超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新；
更新卡尔曼滤波过程，输出GNSS/INS组合导航结果。
作为优选的技术方案，根据新息构建渐消因子矩阵，具体构建步骤为：
所述新息定义为：
vk＝zk-Hkxk,k-1
Figure BDA0002354740730000021
Figure BDA0002354740730000022
引入标量渐消因子λk后状态误差协方差矩阵
Figure BDA0002354740730000023
变为如下形式：
Figure BDA0002354740730000024
其中，标量渐消因子λk的定义如下：
Figure BDA0002354740730000031
Figure BDA0002354740730000032
Figure BDA0002354740730000033
其中，m表示测量向量的维数；
由标量渐消因子拓展为矩阵渐消因子后，状态误差协方差矩阵
Figure BDA0002354740730000034
表示成：
Figure BDA0002354740730000035
矩阵渐消因子为对角矩阵，具体表示如下：
λk＝diag(λ11 λ22 … λnn)；
则λk对角线上的元素的λii与1比较大小进行选择，表示为：
Figure BDA0002354740730000036
若λk≡1，表示滤波器工作在理想状态。
作为优选的技术方案，所述根据残差构建增益系数矩阵，具体构建步骤为：定义残差为：εk＝zk-Hkxk
引入增益系数矩阵后，增益矩阵为：
Figure BDA0002354740730000037
在k时刻的增益系数矩阵αk为对角矩阵，表示为：
αk＝diag[αk(1) αk(2) … αk(n)]
对增益矩阵变形，表示为比值形式，具体为：
Figure BDA0002354740730000038
对增益矩阵Kk求极限：
Figure BDA0002354740730000041
/>
Figure BDA0002354740730000042
依据平滑原理得到增益系数矩阵的定义如下：
Figure BDA0002354740730000043
其中，diag(·)表示对角矩阵。
作为优选的技术方案，所述构建GNSS/INS组合导航系统的状态方程和测量方程，具体的状态方程和测量方程分别为：
xk＝Φk-1xk-1+wk
zk＝Hkxk+vk
其中，xk表示被估计的状态向量，Φk-1表示k-1到k时刻的一步状态转移矩阵，zk表示观测向量，Hk表示观测矩阵，wk和vk分别表示系统噪声和观测噪声。
作为优选的技术方案，所述构建GNSS/INS组合导航系统的状态方程，所述状态方程的状态向量xk选取如下：
Figure BDA0002354740730000044
Figure BDA0002354740730000045
其中，
Figure BDA0002354740730000046
表示载体相对导航坐标系的姿态误差，/>
Figure BDA0002354740730000047
表示速度误差，δpb表示位置误差，ba表示陀螺仪误差，bg表示加速度计误差，La、λa、ha分别表示运动载体的维度、经度和高度，/>
Figure BDA0002354740730000051
表示载体的速度，δρrc、/>
Figure BDA0002354740730000052
分别表示伪距和伪距率。
作为优选的技术方案，所述更新卡尔曼滤波过程，具体更新过程为：
Figure BDA0002354740730000053
Figure BDA0002354740730000054
Figure BDA0002354740730000055
Figure BDA0002354740730000056
Figure BDA0002354740730000057
其中，
Figure BDA0002354740730000058
表示k-1时刻最优的状态向量估计，/>
Figure BDA0002354740730000059
表示k时刻的状态向量，/>
Figure BDA00023547407300000510
表示k时刻最优的状态向量估计，Φk-1表示k-1到k时刻的一步状态转移矩阵，Hk表示观测矩阵，
Figure BDA00023547407300000511
分别表示k-1、k、k+1时刻的状态误差协方差矩阵，zk表示观测向量，Qk和Rk分别表示系统噪声矩阵和观测噪声协方差矩阵，λk和αk分别表示渐消因子矩阵和增益系数矩阵。
本发明还提供一种GNSS/INS组合导航系统的两步抗差滤波系统，包括：
渐消因子矩阵构建模块、增益系数矩阵构建模块、GNSS/INS数据融合模块、动力学模型检测模块、观测量检测模块和卡尔曼滤波更新模块；
所述渐消因子矩阵构建模块用于根据新息构建渐消因子矩阵；
所述增益系数矩阵构建模块用于根据残差构建增益系数矩阵；
所述GNSS/INS数据融合模块用于构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据；
所述动力学模型检测模块用于检测动力学模型是否异常，若判定为超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新；
所述观测量检测模块用于检测观测量是否正常，若判定为超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新；
所述卡尔曼滤波更新模块用于更新卡尔曼滤波过程，输出GNSS/INS组合导航结果。
本发明与现有技术相比，具有如下优点和有益效果：
(1)本发明采用两步滤波的方法，首先构造渐消因子矩阵减少对过去信息的记忆长度，降低系统噪声的影响；再利用增益系数矩阵调整卡尔曼增益，降低测量噪声干扰，克服噪声粗差在传播过程造成的影响。
(2)本发明为了保证误差协方差的对称性，基于新息，由标量因子拓展成矩阵形式，构造新的渐消因子矩阵，在误差协方差矩阵两边都乘上渐消因子矩阵和其转置矩阵，减少计算过程由协方差矩阵不对称带来的计算误差，采用渐消因子矩阵到第一步滤波过程中，达到精确调节各个通道系统噪声，减少系统噪声的干扰的效果。
(3)本发明从极限的角度得到增益矩阵K与P/R比率关系，然后依据残差构造增益系数矩阵，通过增益系数矩阵调节增益矩阵K，减少测量噪声干扰的影响。
(4)本发明构造基于新息协方差估计的多重渐消因子和基于抗差因子，从单因子形式以对角线矩阵的形式，分别从新息和残差来构造矩阵因子，充分利用两种数据，减少两种构造矩阵的耦合程度。
附图说明
图1为本实施例GNSS/INS组合导航系统的两步抗差滤波方法的流程示意图；
图2为本实施例紧耦合开环输出校正结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例用以解释本发明，并不用于限定本发明。
实施例
如图1所示，本实施例提供一种基于紧耦合方式的GNSS/INS组合导航系统的两步抗差滤波方法，包括下述步骤：
首先初始化各个矩阵变量，初始化影响定位启动时间，本实施例的误差协方差矩阵代表着实际测量值与真实值之间的误差程度，因此其对角元素不能全为零；
然后建立GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据，以惯性导航状态向量xINS和全球卫星导航系统状态向量xGNSS作为误差状态向量x进行数据融合，相应的其他变量也包含着两种数据。
在本实施例中，GNSS/INS组合导航系统的状态方程和测量方程为：
xk＝Φk-1xk-1+wk
zk＝Hkxk+vk
其中，xk∈Rn是被估计状态向量，Φk-1∈Rn×n是k-1到k时刻的一步状态转移矩阵，zk∈Rm是观测向量，Hk∈Rm×n是观测矩阵，wk和vk分别是系统噪声和观测噪声，均服从零均值高斯分布，其对应的协方差分别是Qk和Rk，其中要求xk，wk和vk两两互不相关。
对于一个实际系统而言，比如车载导航系统，状态转移矩阵Φk-1和观测矩阵Hk可以认为是与时间相关的确定的矩阵。
误差状态向量xk选取包括姿态、速度和位置误差，以及加速度计和陀螺仪零偏，表示如下：
Figure BDA0002354740730000081
其中
Figure BDA0002354740730000082
其中，
Figure BDA0002354740730000083
表示载体(车辆)相对当地导航坐标系的姿态误差；/>
Figure BDA0002354740730000084
表示速度误差；δpb表示位置误差；ba表示陀螺仪误差；bg表示加速度计误差；La、λa、ha分别表示运动载体的维度、经度和高度；/>
Figure BDA0002354740730000085
表示载体的速度；δρrc、/>
Figure BDA0002354740730000086
表示伪距和伪距率。
多源数据融合的组合导航系统一般有三种组合方式，松耦合、紧耦合和深耦合，GNSS/INS组合导航系统的松耦合方式两种数据相对独立，工程实现简单，但是滤波精度不高；紧耦合方式下，两种数据源相互矫正，滤波结果不再单独输出，滤波精度高，深耦合方式是在紧耦合方式的基础上进一步加深耦合程度，本实施例采用紧耦合方式。
如图2所示，卫星导航接收机(GNSS接收机)和惯性导航接收机(IMU)分别采集车辆行驶数据，然后进行预处理，之后把数据输入INS/GNSS组合卡尔曼滤波器，依据卡尔曼滤波过紧耦合过程进行滤波，最后经过校正，输出结果，基于紧耦合结构的GNSS/INS组合导航系统的观测向量表示如下：
Figure BDA0002354740730000091
式中
Figure BDA0002354740730000092
表示伪距测量，/>
Figure BDA0002354740730000093
表示伪距率测量。
在本实施例中，求解多重渐消因子矩阵λk的方法为：
新息定义为：vk＝zk-Hkxk,k-1
Figure BDA0002354740730000094
Figure BDA0002354740730000095
引入标量因子λk后误差协方差矩阵
Figure BDA0002354740730000096
变为如下形式：
Figure BDA0002354740730000097
其中，标量渐消因子λk的定义如下：
Figure BDA0002354740730000098
其中
Figure BDA0002354740730000099
Figure BDA00023547407300000910
式中m是测量向量的维数。
由标量渐消因子拓展为矩阵渐消因子后，同时考虑对称性，状态误差协方差
Figure BDA00023547407300000911
写成：
Figure BDA00023547407300000912
矩阵渐消因子为对角矩阵，形式如下：
λk＝diag(λ11 λ22 … λnn)
为了方便表示，记：
Figure BDA0002354740730000101
则λk对角线上的元素的λii与1比较大小进行选择，表示为：
Figure BDA0002354740730000102
如果λk≡1则退化为经典的卡尔曼滤波方程，即说明滤波器工作在理想状态；
由于仅对误差协方差矩阵的对角元素进行加权，其仍然满足MMSE准则，因此仍然有MMSE条件下的最优解。不论是单一渐消因子还是多重渐消因子都是依据残差定义，单一因子仅能对整体进行平均调整，而多重自适应因子可以精确调整各个通道的权重，可以获得精确的滤波效果。
在本实施例中，为了保证误差协方差的对称性，基于新息，由标量因子拓展成矩阵形式，构造新的渐消因子矩阵，在误差协方差矩阵两边都乘上渐消因子矩阵和其转置矩阵，减少计算过程由协方差矩阵不对称带来的计算误差，采用渐消因子矩阵到第一步滤波过程中，达到精确调节各个通道系统噪声，减少系统噪声的干扰的效果。
进一步地，求解增益系数矩阵αk：
定义残差为：εk＝zk-Hkxk
引入增益系数矩阵后，增益矩阵变为如下形式：
Figure BDA0002354740730000103
在k时刻的增益系数矩阵αk为对角矩阵表示为：
αk＝diag[αk(1) αk(2) … αk(n)]
对增益矩阵变形，写成比值形式为：
Figure BDA0002354740730000111
求对增益矩阵Kk求极限
Figure BDA0002354740730000112
Figure BDA0002354740730000113
从极限结果可以看到增益矩阵K与误差协方差/噪声协方差(P/R)比率有关，它决定了观测信息在状态向量更新时的权重；
依据平滑原理得到增益系数矩阵的定义如下：
Figure BDA0002354740730000114
其中diag(·)是对角矩阵，对于矩阵而言，diag(·)即把矩阵主对角元素构成对角矩阵。
本实施例从极限的角度得到增益矩阵K与P/R比率关系，然后依据残差构造增益系数矩阵，通过增益系数矩阵调节增益矩阵K，减少测量噪声干扰的影响。
在本实施例中，求解得到渐消因子矩阵，增益系数矩阵之后，经典卡尔曼滤波方程变为如下形式：
Figure BDA0002354740730000115
Figure BDA0002354740730000116
Figure BDA0002354740730000117
/>
Figure BDA0002354740730000118
Figure BDA0002354740730000119
其中，
Figure BDA00023547407300001110
表示k-1时刻最优的状态向量估计；/>
Figure BDA00023547407300001111
表示k时刻的状态向量；/>
Figure BDA0002354740730000121
表示k时刻最优的状态向量估计；Φk-1表示k-1到k时刻的一步状态转移矩阵，Hk表示观测矩阵；
Figure BDA0002354740730000122
分别表示k-1、k、k+1时刻的状态误差协方差矩阵；zk表示观测向量；Qk和Rk分别表示系统噪声矩阵和观测噪声协方差矩阵；λk和αk分别表示渐消因子矩阵和增益系数矩阵。
按照以上步骤，执行卡尔曼滤波器迭代更新，可以得到GNSS/INS组合导航姿态、速度和位置的三维输出结果。值得注意的是，惯性导航模块传感器的精度对数据采集有很大影响，应选高精度惯性导航模块；除此之外，应检查数据，剔除偏离正常范围的数据，对模型进行校正、误差补偿，使导航结果精度更高、稳定更强。
本实施例采用两步滤波的方法，首先构造渐消因子矩阵减少对过去信息的记忆长度，降低系统噪声的影响；再利用增益系数矩阵调整卡尔曼增益，降低测量噪声干扰，克服噪声粗差在传播过程造成的影响。
本实施例还提供一种GNSS/INS组合导航系统的两步抗差滤波系统，包括：
渐消因子矩阵构建模块、增益系数矩阵构建模块、GNSS/INS数据融合模块、动力学模型检测模块、观测量检测模块和卡尔曼滤波更新模块；
渐消因子矩阵构建模块用于根据新息构建渐消因子矩阵，增益系数矩阵构建模块用于根据残差构建增益系数矩阵，GNSS/INS数据融合模块用于构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据，动力学模型检测模块用于检测动力学模型是否异常，若判定为超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新，观测量检测模块用于检测观测量是否正常，若判定为超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新，卡尔曼滤波更新模块用于更新卡尔曼滤波过程，输出GNSS/INS组合导航结果。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

1.一种GNSS/INS组合导航系统的两步抗差滤波方法，其特征在于，包括下述步骤：
根据新息构建渐消因子矩阵，根据残差构建增益系数矩阵；
构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据；
检测动力学模型是否异常，若判定为超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新；
检测观测量是否正常，若判定为超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新；
更新卡尔曼滤波过程，输出GNSS/INS组合导航结果；
根据新息构建渐消因子矩阵，具体构建步骤为：
所述新息定义为：
vk＝zk-Hkxk,k-1
Figure FDA0004185707870000011
Figure FDA0004185707870000012
其中，zk表示观测向量，Hk表示观测矩阵；
引入标量渐消因子λk后状态误差协方差矩阵Pk -变为如下形式：
Figure FDA0004185707870000013
其中，Φk-1表示k-1到k时刻的一步状态转移矩阵；
标量渐消因子λk的定义如下：
Figure FDA0004185707870000014
Figure FDA0004185707870000021
Figure FDA0004185707870000022
其中，m表示测量向量的维数；
由标量渐消因子拓展为矩阵渐消因子后，状态误差协方差矩阵
Figure FDA0004185707870000023
表示成：
Figure FDA0004185707870000024
矩阵渐消因子为对角矩阵，具体表示如下：
λk＝diag(λ11 λ22 … λnn)；
则λk对角线上的元素的λii与1比较大小进行选择，表示为：
Figure FDA0004185707870000025
若λk≡1，表示滤波器工作在理想状态。
2.根据权利要求1所述的GNSS/INS组合导航系统的两步抗差滤波方法，其特征在于，所述根据残差构建增益系数矩阵，具体构建步骤为：
定义残差为：εk＝zk-Hkxk
引入增益系数矩阵后，增益矩阵为：
Figure FDA0004185707870000026
在k时刻的增益系数矩阵αk为对角矩阵，表示为：
αk＝diag[αk(1) αk(2) … αk(n)]
对增益矩阵变形，表示为比值形式，具体为：
Figure FDA0004185707870000027
对增益矩阵Kk求极限：
Figure FDA0004185707870000031
Figure FDA0004185707870000032
依据平滑原理得到增益系数矩阵的定义如下：
Figure FDA0004185707870000033
其中，diag(·)表示对角矩阵。
3.根据权利要求1所述的GNSS/INS组合导航系统的两步抗差滤波方法，其特征在于，所述构建GNSS/INS组合导航系统的状态方程和测量方程，具体的状态方程和测量方程分别为：
xk＝Φk-1xk-1+wk
zk＝Hkxk+vk
其中，xk表示被估计的状态向量，wk和vk分别表示系统噪声和观测噪声。
4.根据权利要求1所述的GNSS/INS组合导航系统的两步抗差滤波方法，其特征在于，所述构建GNSS/INS组合导航系统的状态方程，所述状态方程的状态向量xk选取如下：
Figure FDA0004185707870000034
Figure FDA0004185707870000035
其中，
Figure FDA0004185707870000036
表示载体相对导航坐标系的姿态误差，/>
Figure FDA0004185707870000037
表示速度误差，δpb表示位置误差，ba表示陀螺仪误差，bg表示加速度计误差，La、λa、ha分别表示运动载体的维度、经度和高度，/>
Figure FDA0004185707870000041
表示载体的速度，δρrc、/>
Figure FDA0004185707870000042
分别表示伪距和伪距率。
5.根据权利要求1所述的GNSS/INS组合导航系统的两步抗差滤波方法，其特征在于，所述更新卡尔曼滤波过程，具体更新过程为：
Figure FDA0004185707870000043
Figure FDA0004185707870000044
Figure FDA0004185707870000045
Figure FDA0004185707870000046
Figure FDA0004185707870000047
其中，
Figure FDA0004185707870000048
表示k-1时刻最优的状态向量估计，/>
Figure FDA0004185707870000049
表示k时刻的状态向量，/>
Figure FDA00041857078700000410
表示k时刻最优的状态向量估计，/>
Figure FDA00041857078700000411
分别表示k-1、k、k+1时刻的状态误差协方差矩阵，Qk表示系统噪声矩阵αk表示增益系数矩阵。
6.一种GNSS/INS组合导航系统的两步抗差滤波系统，其特征在于，包括：
渐消因子矩阵构建模块、增益系数矩阵构建模块、GNSS/INS数据融合模块、动力学模型检测模块、观测量检测模块和卡尔曼滤波更新模块；
所述渐消因子矩阵构建模块用于根据新息构建渐消因子矩阵；
根据新息构建渐消因子矩阵，具体构建步骤为：
所述新息定义为：
vk＝zk-Hkxk,k-1
Figure FDA00041857078700000412
Figure FDA0004185707870000051
其中，zk表示观测向量，Hk表示观测矩阵；
引入标量渐消因子λk后状态误差协方差矩阵
Figure FDA0004185707870000059
变为如下形式：
Figure FDA0004185707870000052
其中，Φk-1表示k-1到k时刻的一步状态转移矩阵；
标量渐消因子λk的定义如下：
Figure FDA0004185707870000053
Figure FDA0004185707870000054
Figure FDA0004185707870000055
/>
其中，m表示测量向量的维数；
由标量渐消因子拓展为矩阵渐消因子后，状态误差协方差矩阵
Figure FDA0004185707870000056
表示成：
Figure FDA0004185707870000057
矩阵渐消因子为对角矩阵，具体表示如下：
λk＝diag(λ11 λ22 … λnn)；
则λk对角线上的元素的λii与1比较大小进行选择，表示为：
Figure FDA0004185707870000058
若λk≡1，表示滤波器工作在理想状态；
所述增益系数矩阵构建模块用于根据残差构建增益系数矩阵；
所述GNSS/INS数据融合模块用于构建GNSS/INS组合导航系统的状态方程和测量方程，按照卡尔曼滤波紧耦合方式融合GNSS/INS数据；
所述动力学模型检测模块用于检测动力学模型是否异常，若判定为超过设定阈值范围，通过渐消因子矩阵调整，进入时间更新；
所述观测量检测模块用于检测观测量是否正常，若判定为超过设定阈值范围，通过增益系数矩阵多通道调整观测向量，进入测量更新；
所述卡尔曼滤波更新模块用于更新卡尔曼滤波过程，输出GNSS/INS组合导航结果。
CN202010004513.5A 2020-01-03 2020-01-03 Gnss/ins组合导航系统的两步抗差滤波方法及系统 Active CN111175795B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010004513.5A CN111175795B (zh) 2020-01-03 2020-01-03 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010004513.5A CN111175795B (zh) 2020-01-03 2020-01-03 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Publications (2)

Publication Number Publication Date
CN111175795A CN111175795A (zh) 2020-05-19
CN111175795B true CN111175795B (zh) 2023-05-26

Family

ID=70654381

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010004513.5A Active CN111175795B (zh) 2020-01-03 2020-01-03 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Country Status (1)

Country Link
CN (1) CN111175795B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112230227B (zh) * 2020-09-25 2022-06-28 浙江大学 基于近场测量数据的阵列诊断方法
CN112229392B (zh) * 2020-09-25 2022-11-18 福建华电可门发电有限公司 一种高冗余的室内煤场导航方法及系统
CN112269201B (zh) * 2020-10-23 2024-04-16 北京云恒科技研究院有限公司 一种gnss/ins紧耦合时间分散滤波方法
CN112698374A (zh) * 2020-12-17 2021-04-23 广州中海达卫星导航技术股份有限公司 基于bds-3/磁力计的多频双天线车载测姿方法和设备
CN112946711B (zh) * 2021-01-29 2022-11-25 中国人民解放军国防科技大学 一种gnss/ins组合导航系统的导航方法
CN113670337B (zh) * 2021-09-03 2023-05-26 东南大学 一种用于gnss/ins组合导航卫星缓变故障检测方法
CN113780849B (zh) * 2021-09-16 2023-11-28 湖南航天机电设备与特种材料研究所 Ins/gnss组合导航系统性能评估方法、系统、设备及存储介质
CN114659496B (zh) * 2022-05-25 2022-08-02 南京恒舟准导航科技有限公司 一种用于船载北斗一体机倾斜监测的方法
CN114779307B (zh) * 2022-06-17 2022-09-13 武汉理工大学 面向港区的uwb/ins/gnss的无缝定位方法
CN116165690B (zh) * 2023-04-21 2023-07-07 山西省娄烦县皇姑山矿业有限责任公司 一种基于gnss/ins的双自适应因子组合导航定位方法
CN116608853B (zh) * 2023-07-21 2023-09-29 广东智能无人系统研究院(南沙) 载体动态姿态估计方法、设备、存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN107643534A (zh) * 2017-09-11 2018-01-30 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109163720A (zh) * 2018-08-27 2019-01-08 广西科技大学 基于渐消记忆指数加权的卡尔曼滤波跟踪方法
CN110061716A (zh) * 2019-01-15 2019-07-26 河海大学 一种基于最小二乘和多重渐消因子的改进kalman滤波方法
CN110196443A (zh) * 2019-06-06 2019-09-03 中国人民解放军战略支援部队信息工程大学 一种飞行器的容错组合导航方法及系统

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8600660B2 (en) * 2006-09-29 2013-12-03 Honeywell International Inc. Multipath modeling for deep integration
CN106291645B (zh) * 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107643534A (zh) * 2017-09-11 2018-01-30 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109163720A (zh) * 2018-08-27 2019-01-08 广西科技大学 基于渐消记忆指数加权的卡尔曼滤波跟踪方法
CN110061716A (zh) * 2019-01-15 2019-07-26 河海大学 一种基于最小二乘和多重渐消因子的改进kalman滤波方法
CN110196443A (zh) * 2019-06-06 2019-09-03 中国人民解放军战略支援部队信息工程大学 一种飞行器的容错组合导航方法及系统

Also Published As

Publication number Publication date
CN111175795A (zh) 2020-05-19

Similar Documents

Publication Publication Date Title
CN111175795B (zh) Gnss/ins组合导航系统的两步抗差滤波方法及系统
US10877059B2 (en) Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method
CN108873038B (zh) 自主泊车定位方法及定位系统
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
US8510044B2 (en) Position sensing device and method
US9939532B2 (en) Heading for a hybrid navigation solution based on magnetically calibrated measurements
US6577952B2 (en) Position and heading error-correction method and apparatus for vehicle navigation systems
AU2009200190B2 (en) Methods and systems for underwater navigation
CN112507281B (zh) 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN111189441A (zh) 一种多源自适应容错联邦滤波组合导航系统及导航方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN111721290B (zh) 一种多源传感器信息融合定位切换方法
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN104181574A (zh) 一种捷联惯导系统/全球导航卫星系统组合导航滤波系统及方法
CN102436004A (zh) 定位系统及定位方法
WO2007059134A1 (en) Dead reckoning system
CN113063429A (zh) 一种自适应车载组合导航定位方法
CN103697894A (zh) 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN110208843B (zh) 一种基于增广伪距信息辅助的容错导航方法
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
CN106886037A (zh) 适用于弱gnss信号条件的pos数据纠偏方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant