CN116358566B - 一种基于抗差自适应因子的粗差探测组合导航方法 - Google Patents

一种基于抗差自适应因子的粗差探测组合导航方法 Download PDF

Info

Publication number
CN116358566B
CN116358566B CN202310636490.3A CN202310636490A CN116358566B CN 116358566 B CN116358566 B CN 116358566B CN 202310636490 A CN202310636490 A CN 202310636490A CN 116358566 B CN116358566 B CN 116358566B
Authority
CN
China
Prior art keywords
representing
state
robust
adaptive
matrix
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
CN202310636490.3A
Other languages
English (en)
Other versions
CN116358566A (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.)
Shandong University of Science and Technology
Original Assignee
Shandong University of Science and Technology
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 Shandong University of Science and Technology filed Critical Shandong University of Science and Technology
Priority to CN202310636490.3A priority Critical patent/CN116358566B/zh
Publication of CN116358566A publication Critical patent/CN116358566A/zh
Application granted granted Critical
Publication of CN116358566B publication Critical patent/CN116358566B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Mathematical Analysis (AREA)
  • Databases & Information Systems (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种基于抗差自适应因子的粗差探测组合导航方法，属于导航技术领域，用于对导航数据进行处理，包括：根据捷联惯性导航系统与GNSS系统的组合，构建位置量测方程与状态方程；构成判断函数，使用判断函数对粗差进行初步检测；进行前向抗差自适应扩展卡尔曼滤波，存储所需的状态转移矩阵、状态协方差矩阵、量测协方差矩阵，重新构建增益矩阵，进行固定区间反向平滑处理，得到平滑后的状态估计结果，并对惯导输出的位置、速度、姿态进行校正。本发明通过检测观测向量中是否有粗差，获得相应的抗差自适应函数，对扩展卡尔曼滤波进行及时修正，提高卡尔曼滤波的性能，实现抗差估计。

Description

一种基于抗差自适应因子的粗差探测组合导航方法
技术领域
本发明公开一种基于抗差自适应因子的粗差探测组合导航方法，属于导航技术领域。
背景技术
捷联惯性导航系统辅助的伪距定位技术具有较高的可靠性和良好的实时性，已广泛应用于常规陆地车辆的导航定位中，但其导航解算结果严重依赖数学模型的准确程度以及观测数据的质量，抗干扰能力有待加强。车载系统中受到外界环境的干扰因素较大，信号容易受到干扰或者遮挡导致观测条件不佳，观测质量就会降低，影响卡尔曼滤波器的性能甚至会导致发散。
运动物体一般难以确保规则运动，因而精确的函数模型的构造十分困难而且在载体运动过程中，载体不可避免受到外界异常干扰影响，导致载体状态模型不能真实反映载体的运动规律。在车载GNSS/MIMU组合导航系统中，惯性测量器件的质量不稳定，GNSS测量误差受外界环境的影响，使得在导航数据获取过程中，粗差不可避免。它的存在势必影响数据处理质量，对于抗差能力不强的常规Kalman滤波影响非常显著。Kalman滤波的这种特性使其在模型具有不确定性或观测序列中有野值点时，由于缺乏对测量误差的抗干扰能力，导致受“过时”信息影响严重，状态向量的滤波估计值精度和可靠性便会明显变差，甚至会导致滤波发散。
在进行Kalman滤波时，残差序列都是基于正态分布。但是，正态分布只是一种理想的分布。大量的实验证明，严格地服从正态分布的观测数据几乎没有。而且，统计学家根据大量数据分析指出，在生产实践和科学实验中，粗差出现的概率约为1%～10%。粗差的出现往往会带来不良的后果，影响结论的正确性。且Kalman滤波对含粗差的观测量相当敏感，个别粗差就会对参数的估值产生较大的影响。因此，Kalman滤波的最优性只是定义在一个非常狭隘的范围。
发明内容
本发明的目的在于提供一种基于抗差自适应因子的粗差探测组合导航方法，以解决现有技术中，Kalman滤波解算导航数据时抗差能力弱的问题。
一种基于抗差自适应因子的粗差探测组合导航方法，包括：S1.根据捷联惯性导航系统与GNSS系统的组合，构建位置量测方程与状态方程；S2.通过卫星分布的空间几何强度因子PDOP、高精度解算软件中反映观测质量的观测因子Q、卫星的数量SN以及观测量的预测残差统计量构成判断函数，使用判断函数对粗差进行初步检测；S3.设定抗差自适应因子AK=0，通过判断函数进行判断，若符合适用性高的自适应滤波条件，则将抗差自适应因子KA设为1，进入自适应滤波阶段，通过获得其预测残差的统计量构造两段式自适应函数，对粗差进行二次检测，获得自适应因子；若符合适用性高的抗差卡尔曼滤波条件，将抗差自适应因子KA设为2，进入抗差卡尔曼滤波阶段，通过获得其标准化残差，根据IGGIII法构造三段式抗差权函数，对粗差进行二次检测，获得等价权；S4.进行前向抗差自适应扩展卡尔曼滤波，存储所需的状态转移矩阵、状态协方差矩阵、量测协方差矩阵，重新构建增益矩阵，进行固定区间反向平滑处理，得到平滑后的状态估计结果，并对惯导输出的位置、速度、姿态进行校正。
S1中的状态方程为：
；
式中，δxk+1表示k+1时刻的误差，Φk+1/k表示k+1时刻和k时刻的状态一步转移矩阵， wk表示k时刻的系统状态噪声向量；δxk+1 ；表示东方向、北方向、天方向的位置误差在导航坐标系n系中的位置误差向量；表 示东方向、北方向、天方向的速度误差在导航坐标系n系中的速度误差向量；表示在俯仰 角、横滚角、航向角上的姿态误差向量；表示捷联惯性导航系统在载体坐标系右、前、上三 个方向的陀螺仪零偏的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的 加速度计零偏的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪 比例因子的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的加速度计比 例因子的转置。
S1中的位置量测方程为：
。
其中，表示量测向量，、、分别表示惯性导航的纬度经度高度；、、分别表示GNSS的纬度经度高度；其中表示量测矩阵；，表示量测噪声向量，、分别表示子 午圈曲率半径和卯酉圈曲率半径，表示当地纬度，表示由载体坐标系转换到导航坐标系 的方向余弦矩阵。
S2中的判断函数为：
；
式中抗差自适应因子为KA，判断函数为，自适应卡尔曼滤波条件为A，抗差卡尔曼 滤波条件为B，扩展卡尔曼滤波的条件为，判断函数。
自适应卡尔曼滤波条件：
；
抗差卡尔曼滤波条件：
；
若抗差自适应因子KA=1，则构造预测残差统计量为：
；
其中表示k时刻的残差向量，即，用量测估计向量的值与真实值之 间的差值表示残差；表示量测向量的协方差矩阵；tr表示矩阵迹的运算；表示状态估计 值。
构造的自适应因子为：；
其中为常量，设置为1.0~1.5，为残差统计量。
S3构造的权函数为：
；
其中为标准化残差的绝对值；和为常量，其中范围为1.0-1.5，范围为 2.5-8.0。
S3构造的等价权为：
。
S4中使用前向抗差自适应扩展卡尔曼滤波公式，包括状态一步预测与状态量测更新，其方程为：
状态一步预测方程：；状态一步预测均方差误差阵为：；滤波增益方程为：，式中的为抗差等价权的值； 状态估计方程为：；状态估计均方误差阵为：。
表示k时刻的自适应因子；表示k-1时刻对k时刻的状态估计值；表示 状态转移矩阵；表示状态噪声矩阵；R表示量测噪声矩阵；下标k表示k时刻。
S4中固定区间反向平滑处理包括：
存储前向抗差自适应扩展卡尔曼滤波的状态转移矩阵、状态一步预测均方 误差阵、状态估计均方误差阵，执行如下RTS平滑算法：
；
式中，下标s表示RTS固定区间反向平滑算法获得的结果，下标f表示前向抗差自适 应扩展卡尔曼滤波获得的结果。表示k时刻反向平滑的增益矩阵；表示k时刻反向平 滑的状态估计值；表示k时刻前向滤波的状态估计值；M表示前向滤波中的任意选中的时 刻；表示k时刻反向平滑的状态估计均方误差阵；表示k时刻前向滤波的状态估计均方 误差阵；上标T表示矩阵的转置；上标-1表示矩阵的逆。
相对比现有技术，本发明具有以下有益效果：本发明通过检测观测向量中是否有粗差，获得相应的抗差自适应函数，对扩展卡尔曼滤波进行及时修正，提高卡尔曼滤波的性能，实现抗差估计；本发明实时修改导航输出值，提高状态估计的定位精度、完整性和鲁棒性，北方向位置精度没有提高，东方向位置精度提高19.0271%；天方向位置精度提高58.1015%；东方向速度精度提高30.5014%；北方向速度精度提高70.4845%；天方向速度精度提高54.1877%；俯仰角精度提高44.5820%；横滚角精度提高26.7322%；航向角精度提高33.0805%。
附图说明
图1为本发明的基于抗差自适应因子的抗差自适应卡尔曼滤波方法的流程图；
图2为本发明的固定区间反向平滑算法的流程图；
图3为本发明的基于抗差自适应因子的固定区间反向平滑抗差自适应滤波的流程图；
图4为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在东方向上的位置对比图；
图5为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在北方向上的位置对比图；
图6为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在天方向上的位置对比图；
图7为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在东方向上的速度对比图；
图8为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在北方向上的速度对比图；
图9为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在天方向上的速度对比图；
图10为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在俯仰方向上的姿态对比图；
图11为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在横滚方向上的姿态对比图；
图12为本发明实时例中基于抗差自适应因子的固定区间反向平滑抗差自适应卡尔曼滤波（RARKF）、参考真值（Ref）、扩展卡尔曼滤波（EKF）与基于抗差因子的抗差卡尔曼滤波（ARKF）在航向方向上的姿态对比图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
一种基于抗差自适应因子的粗差探测组合导航方法，包括：S1.根据捷联惯性导航系统与GNSS系统的组合，构建位置量测方程与状态方程；S2.通过卫星分布的空间几何强度因子PDOP、高精度解算软件中反映观测质量的观测因子Q、卫星的数量SN以及观测量的预测残差统计量构成判断函数，使用判断函数对粗差进行初步检测；S3.设定抗差自适应因子AK=0，通过判断函数进行判断，若符合适用性高的自适应滤波条件，则将抗差自适应因子KA设为1，进入自适应滤波阶段，通过获得其预测残差的统计量构造两段式自适应函数，对粗差进行二次检测，获得自适应因子；若符合适用性高的抗差卡尔曼滤波条件，将抗差自适应因子KA设为2，进入抗差卡尔曼滤波阶段，通过获得其标准化残差，根据IGGIII法构造三段式抗差权函数，对粗差进行二次检测，获得等价权；S4.进行前向抗差自适应扩展卡尔曼滤波，存储所需的状态转移矩阵、状态协方差矩阵、量测协方差矩阵，重新构建增益矩阵，进行固定区间反向平滑处理，得到平滑后的状态估计结果，并对惯导输出的位置、速度、姿态进行校正。
S1中的状态方程为：
；
式中，δxk+1表示k+1时刻的误差，Φk+1/k表示k+1时刻和k时刻的状态一步转移矩阵， wk表示k时刻的系统状态噪声向量；δxk+1 ；表示东方向、北方向、天方向的位置误差在导航坐标系n系中的位置误差向量；表 示东方向、北方向、天方向的速度误差在导航坐标系n系中的速度误差向量；表示在俯仰 角、横滚角、航向角上的姿态误差向量；表示捷联惯性导航系统在载体坐标系右、前、上三 个方向的陀螺仪零偏的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的 加速度计零偏的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪 比例因子的转置；表示捷联惯性导航系统在载体坐标系右、前、上三个方向的加速度计比 例因子的转置。
S1中的位置量测方程为：
。
其中，表示量测向量，、、分别表示惯性导航的纬度经度高度；、、分别表示GNSS的纬度经度高度；其中表示量测矩阵；，表示量测噪声向量，、分别表示子 午圈曲率半径和卯酉圈曲率半径，表示当地纬度，表示由载体坐标系转换到导航坐标系 的方向余弦矩阵。
S2中的判断函数为：
；
式中抗差自适应因子为KA，判断函数为，自适应卡尔曼滤波条件为A，抗差卡尔曼 滤波条件为B，扩展卡尔曼滤波的条件为，判断函数。
自适应卡尔曼滤波条件：
；
抗差卡尔曼滤波条件：
；
若抗差自适应因子KA=1，则构造预测残差统计量为：
；
其中表示k时刻的残差向量，即，用量测估计向量的值与真实值之 间的差值表示残差；表示量测向量的协方差矩阵；tr表示矩阵迹的运算；表示状态估计 值。
构造的自适应因子为：；
其中为常量，设置为1.0~1.5，为残差统计量。
S3构造的权函数为：
；
其中为标准化残差的绝对值；和为常量，其中范围为1.0-1.5，范围为 2.5-8.0。
S3构造的等价权为：
。
S4中使用前向抗差自适应扩展卡尔曼滤波公式，包括状态一步预测与状态量测更新，其方程为：
状态一步预测方程：；状态一步预测均方差误差阵为：；滤波增益方程为：，式中的为抗差等价权的值； 状态估计方程为：；状态估计均方误差阵为：。
表示k时刻的自适应因子；表示k-1时刻对k时刻的状态估计值；表示 状态转移矩阵；表示状态噪声矩阵；R表示量测噪声矩阵；下标k表示k时刻。
S4中固定区间反向平滑处理包括：
存储前向抗差自适应扩展卡尔曼滤波的状态转移矩阵、状态一步预测均方 误差阵、状态估计均方误差阵，执行如下RTS平滑算法：
；
式中，下标s表示RTS固定区间反向平滑算法获得的结果，下标f表示前向抗差自适 应扩展卡尔曼滤波获得的结果。表示k时刻反向平滑的增益矩阵；表示k时刻反向平 滑的状态估计值；表示k时刻前向滤波的状态估计值；M表示前向滤波中的任意选中的时 刻；表示k时刻反向平滑的状态估计均方误差阵；表示k时刻前向滤波的状态估计均方 误差阵；上标T表示矩阵的转置；上标-1表示矩阵的逆。
状态方程和位置量测方程构建模块，用于根据捷联惯性导航系统建立状态方程和位置量测方程；扩展卡尔曼滤波模块，用于列立初步卡尔曼矩阵，并将状态方程进行线性化表示；构建抗差自适应因子与判断函数模块，用于初步探测粗差，并获得抗差自适应因子KA值，根据抗差自适应因子KA值，确定进行自适应卡尔曼滤波模块还是抗差卡尔曼滤波模块；自适应卡尔曼滤波模块，用于确定残差统计量，获得相应的自适应因子，对粗差进行初步检测；抗差卡尔曼滤波模块，用于确定标准化残差，获得相应的权函数以及等价权，进一步对粗差进行检测；固定区间反向平滑模块，用于对前向抗差自适应扩展卡尔曼滤波进行反向平滑，进一步提高GNSS中断时期、存在粗差时期以及正常状态时期的估计精度；导航校正模块，用于根据RTS固定区间反向平滑算法获得的状态改正量对导航输出的位置、速度、姿态进行校正；信号传输顺序依次为状态方程和位置速度量测方程的构建、自适应卡尔曼滤波模块、抗差卡尔曼滤波模块、RTS固定区间反向平滑模块以及导航校正模块。
基于抗差自适应因子粗差探测算法中的抗差自适应算法流程图如图1所示，通过由卫星分布的空间几何强度因子、卫星数与观测因子构成的判断函数，获得抗差自适应因子，根据抗差自适应因子进行相应滤波处理。
反向平滑滤波算法的流程如图2所示，通过前向滤波存储状态估计值，状态估计均方误差阵，进行反向平滑处理，获得状态估计值、状态估计均方误差阵。
基于抗差自适应因子粗差探测算法的流程图如图3所示，通过由卫星分布的空间几何强度因子、卫星数与观测因子构成的判断函数，获得抗差自适应因子，根据抗差自适应因子进行相应滤波处理，最后通过存储前向滤波状态估计值、状态估计均方误差阵，进行反向平滑处理。
图4、图5和图6表示参考真值、扩展卡尔曼滤波、基于残差统计量的抗差自适应滤波与基于抗差自适应因子的反向平滑算法在东方向、北方向、天方向上位置变化对比图，通过图可以得知基于抗差自适应因子的反向平滑滤波算法位置变化图与参考真值重合度高于扩展卡尔曼滤波与基于残差统计量的抗差自适应滤波，即基于抗差自适应因子的反向平滑滤波算法在位置上精度高，抗干扰能力强，鲁棒性好。
图7、图8、图9表示参考真值、扩展卡尔曼滤波、基于残差统计量的抗差自适应滤波与基于抗差自适应因子的反向平滑算法在东方向、北方向、天方向上速度变化对比图，通过图可以得知基于抗差自适应因子的反向平滑滤波算法速度变化图与参考真值重合度高于扩展卡尔曼滤波与基于残差统计量的抗差自适应滤波，即基于抗差自适应因子的反向平滑滤波算法在速度上精度高，抗干扰能力强，鲁棒性好。
图10、图11、图12表示参考真值、扩展卡尔曼滤波、基于残差统计量的抗差自适应滤波与基于抗差自适应因子的反向平滑算法在俯仰角、横滚角和航向角上变化对比图，通过图可以得知基于抗差自适应因子的反向平滑滤波算法姿态变化图与参考真值重合度高于扩展卡尔曼滤波与基于残差统计量的抗差自适应滤波，即基于抗差自适应因子的反向平滑滤波算法在姿态上精度高，抗干扰能力强，鲁棒性好。
以上实施例仅用于说明本发明的技术方案，而非对其限制，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (3)

1.一种基于抗差自适应因子的粗差探测组合导航方法，其特征在于，包括：
S1.根据捷联惯性导航系统与GNSS系统的组合，构建位置量测方程与状态方程；
S2.通过卫星分布的空间几何强度因子PDOP、高精度解算软件中反映观测质量的观测因子Q、卫星的数量SN以及观测量的预测残差统计量构成判断函数，使用判断函数对粗差进行初步检测；
S2中的判断函数为：
；
式中抗差自适应因子为KA，判断函数为，自适应卡尔曼滤波条件为A，抗差卡尔曼滤波条件为B，扩展卡尔曼滤波的条件为/>，判断函数/>；
自适应卡尔曼滤波条件：
；
抗差卡尔曼滤波条件：
；
若抗差自适应因子KA=1，则构造预测残差统计量为：
；
其中表示残差向量，即/>，用量测估计向量的值与真实值之间的差值表示残差；/>表示量测向量的协方差矩阵；tr表示矩阵迹的运算；
构造的自适应因子为：/>；
其中 2为常量，设置为1.0~1.5；
S3.设定抗差自适应因子AK=0，通过判断函数进行判断，若符合适用性高的自适应滤波条件，则将抗差自适应因子KA设为1，进入自适应滤波阶段，通过获得其预测残差的统计量构造两段式自适应函数，对粗差进行二次检测，获得自适应因子；
若符合适用性高的抗差卡尔曼滤波条件，将抗差自适应因子KA设为2，进入抗差卡尔曼滤波阶段，通过获得其标准化残差，根据IGGIII法构造三段式抗差权函数，对粗差进行二次检测，获得等价权；
S3构造的权函数为：
；
其中为标准化残差；/>和/>为常量，其中/>范围为1.0-1.5，/>范围为2.5-8.0；
S3构造的等价权为：
；
S4.进行前向抗差自适应扩展卡尔曼滤波，存储所需的状态转移矩阵、状态协方差矩阵、量测协方差矩阵，重新构建增益矩阵，进行固定区间反向平滑处理，得到平滑后的状态估计结果，并对惯导输出的位置、速度、姿态进行校正；
S4中固定区间反向平滑处理包括：
存储前向抗差自适应扩展卡尔曼滤波的状态转移矩阵、状态一步预测均方误差阵、状态估计均方误差阵/>，执行如下RTS平滑算法：
；
式中，下标s表示RTS固定区间反向平滑算法获得的结果，下标f表示前向抗差自适应扩展卡尔曼滤波获得的结果；表示k时刻反向平滑的增益矩阵；/>表示k时刻反向平滑的状态估计值；/>表示k时刻前向滤波的状态估计值；M表示前向滤波中的任意选中的时刻；/>表示k时刻反向平滑的状态估计均方误差阵；/>表示k时刻前向滤波的状态估计均方误差阵。
2.根据权利要求1所述的一种基于抗差自适应因子的粗差探测组合导航方法，其特征在于，S1中的状态方程为：
；
式中，δxk+1表示k+1时刻的误差，Φk+1/k表示k+1时刻和k时刻的状态一步转移矩阵，wk表示k时刻的系统状态噪声向量；
δxk+1 ；
表示东方向、北方向、天方向的位置误差在导航坐标系n系中的位置误差向量；
表示东方向、北方向、天方向的速度误差在导航坐标系n系中的速度误差向量；
表示在俯仰角、横滚角、航向角上的姿态误差向量；
表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪零偏的转置；
表示捷联惯性导航系统在载体坐标系右、前、上三个方向的加速度计零偏的转置；
表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪比例因子的转置；
表示捷联惯性导航系统在载体坐标系右、前、上三个方向的加速度计比例因子的转置；
S1中的位置量测方程为：
；
其中，表示量测向量，/>、/>、/>分别表示惯性导航的纬度经度高度；/>、/>、分别表示GNSS的纬度经度高度；其中/>表示量测矩阵；
，/>表示量测噪声向量，/>、/>分别表示子午圈曲率半径和卯酉圈曲率半径，/>表示当地纬度。
3.根据权利要求1所述的一种基于抗差自适应因子的粗差探测组合导航方法，其特征在于，S4中使用前向抗差自适应扩展卡尔曼滤波公式，包括状态一步预测与状态量测更新，其方程为：
状态一步预测方程：；
状态一步预测均方差误差阵为：；
滤波增益方程为：，式中的/>为抗差等价权/>的值；
状态估计方程为：；
状态估计均方误差阵为：；
表示k时刻的自适应因子；
表示k-1时刻对k时刻的状态估计值；
表示状态转移矩阵；
表示状态噪声矩阵；R表示量测噪声矩阵；下标k表示k时刻。
CN202310636490.3A 2023-06-01 2023-06-01 一种基于抗差自适应因子的粗差探测组合导航方法 Active CN116358566B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310636490.3A CN116358566B (zh) 2023-06-01 2023-06-01 一种基于抗差自适应因子的粗差探测组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310636490.3A CN116358566B (zh) 2023-06-01 2023-06-01 一种基于抗差自适应因子的粗差探测组合导航方法

Publications (2)

Publication Number Publication Date
CN116358566A CN116358566A (zh) 2023-06-30
CN116358566B true CN116358566B (zh) 2023-08-11

Family

ID=86941933

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310636490.3A Active CN116358566B (zh) 2023-06-01 2023-06-01 一种基于抗差自适应因子的粗差探测组合导航方法

Country Status (1)

Country Link
CN (1) CN116358566B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117570988B (zh) * 2023-11-21 2024-08-16 苏州星幕航天科技有限公司 一种惯性坐标系zem制导闭环净加速度指令幅值的解析方法
CN117647251A (zh) * 2024-01-30 2024-03-05 山东科技大学 基于观测噪声协方差矩阵的抗差自适应组合导航方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108646277A (zh) * 2018-05-03 2018-10-12 山东省计算中心（国家超级计算济南中心） 基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法
CN113916225A (zh) * 2021-10-09 2022-01-11 哈尔滨工业大学 一种基于稳健权因子系数的组合导航粗差抗差估计方法
WO2022073376A1 (zh) * 2020-10-10 2022-04-14 东南大学 一种基于多渐消因子自适应动机座旋转调制精对准方法
CN115451952A (zh) * 2022-08-29 2022-12-09 南京航空航天大学 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN116027376A (zh) * 2023-01-31 2023-04-28 东南大学 一种基于抗差估计的rtk/ins嵌入式实时组合定位系统
CN116150565A (zh) * 2023-03-31 2023-05-23 武汉大学 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108646277A (zh) * 2018-05-03 2018-10-12 山东省计算中心（国家超级计算济南中心） 基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法
WO2022073376A1 (zh) * 2020-10-10 2022-04-14 东南大学 一种基于多渐消因子自适应动机座旋转调制精对准方法
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN113916225A (zh) * 2021-10-09 2022-01-11 哈尔滨工业大学 一种基于稳健权因子系数的组合导航粗差抗差估计方法
CN115451952A (zh) * 2022-08-29 2022-12-09 南京航空航天大学 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置
CN116027376A (zh) * 2023-01-31 2023-04-28 东南大学 一种基于抗差估计的rtk/ins嵌入式实时组合定位系统
CN116150565A (zh) * 2023-03-31 2023-05-23 武汉大学 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种基于改进自适应卡尔曼滤波的GNSS/INS组合导航算法;吴晓倩;卢秀山;王胜利;王鸣鹤;柴大帅;;科学技术与工程(第03期);全文 *

Also Published As

Publication number Publication date
CN116358566A (zh) 2023-06-30

Similar Documents

Publication Publication Date Title
CN116358566B (zh) 一种基于抗差自适应因子的粗差探测组合导航方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN115096303B (zh) 一种gnss多天线与ins紧组合定位定姿方法和设备
EP4143507B1 (en) Navigation apparatus and method in which measurement quantization errors are modeled as states
CN110395297B (zh) 列车定位方法
CN113916225B (zh) 一种基于稳健权因子系数的组合导航粗差抗差估计方法
CN110006427A (zh) 一种低动态高振动环境下的bds/ins紧组合导航方法
CN110207721B (zh) 一种考虑残差分布的无效地形匹配结果识别方法
CN115902963A (zh) 一种单点定位数据处理方法、装置、电子设备及存储介质
CN118276135A (zh) 一种基于自适应最大熵准则卡尔曼滤波的组合导航方法
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN113959433A (zh) 一种组合导航方法及装置
CN113791432A (zh) 一种提高ais定位精度的组合导航定位方法
CN113434806A (zh) 一种抗差自适应多模型滤波方法
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN114018262B (zh) 一种改进的衍生容积卡尔曼滤波组合导航方法
CN113819911B (zh) Gnss失锁下基于自适应容错滤波的导航方法
US20230078005A1 (en) Navigation assistance method for a mobile carrier
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
CN115616641B (zh) 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法
CN118408541A (zh) 基于广义最大相关熵的自适应鲁棒gnss/ins组合导航信息融合方法
CN117647251A (zh) 基于观测噪声协方差矩阵的抗差自适应组合导航方法
CN118688839A (zh) 一种低轨卫星星座辅助的多级联合增强定位方法及系统

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