CN115468559A - 一种多源联邦滤波组合导航系统的自适应容错方法 - Google Patents

一种多源联邦滤波组合导航系统的自适应容错方法 Download PDF

Info

Publication number
CN115468559A
CN115468559A CN202211123293.3A CN202211123293A CN115468559A CN 115468559 A CN115468559 A CN 115468559A CN 202211123293 A CN202211123293 A CN 202211123293A CN 115468559 A CN115468559 A CN 115468559A
Authority
CN
China
Prior art keywords
fault
filter
navigation system
source
subsystem
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.)
Pending
Application number
CN202211123293.3A
Other languages
English (en)
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.)
Air Force Engineering University of PLA
Original Assignee
Air Force Engineering University of PLA
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 Air Force Engineering University of PLA filed Critical Air Force Engineering University of PLA
Priority to CN202211123293.3A priority Critical patent/CN115468559A/zh
Publication of CN115468559A publication Critical patent/CN115468559A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
    • 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
    • G01C5/00—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels
    • G01C5/06—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels by using barometric means
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H21/0025—Particular filtering methods
    • H03H21/0029—Particular filtering methods based on statistics
    • H03H21/003—KALMAN filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Astronomy & Astrophysics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Navigation (AREA)

Abstract

本公开是关于一种多源联邦滤波组合导航系统的自适应容错方法，包括：选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统模型，该系统模型包括主系统和多个子系统，主系统对应主滤波器，每个子系统对应一个子滤波器；在一预设时刻，主滤波器对每个子滤波器进行置信检验；然后，对子滤波器经置信检验后的观测量估计值进行故障检测和容错，并根据检测和容错结果，主滤波器对无故障的子滤波器的估计信息进行融合，得到下一时刻的信息分配因子；最后，根据得到的信息分配因子进行量测更新，继续进行下一时刻的卡尔曼滤波过程。本公开关能够提高导航系统的全局估计精度和系统容错性能。

Description

一种多源联邦滤波组合导航系统的自适应容错方法
技术领域
本公开涉及无线网络技术领域，尤其涉及一种多源联邦滤波组合导航系统的自适应容错方法。
背景技术
相比于单卫星导航系统与惯导系统的组合，融合了不同工作特点的导航传感器，构成基于多源信息融合的联邦滤波组合导航系统无疑具有更高的观测冗余度，可以获得更高的精度和可靠性。联邦滤波作为分布式结构的典型代表，具有设计灵活、计算量小等优势，被广泛应用于多传感器容错组合导航中。联邦滤波器的各子滤波器的信息分配和故障检测方法会直接影响到全局滤波器的估计精度与容错性能。
目前主流的组合导航系统多采用卡尔曼滤波算法，信息分配与故障检测方法大多数是以残差检测为基础。但这些难以检测到缓变故障以及小幅值误差；并且近年来由于通信技术的飞速进步，不少电子设备具有了一定的欺骗干扰能力，而欺骗信号多属于类似缓变故障的形式，加入随时间推移的小幅值的观测偏差会逐步影响到后续状态估计值，会使残差持续保持较小值，难以被用户检测到。
自主完好性监测外推(Autonomous Integrity Monitoring Extrapolation,AIME)法，虽然提高了缓变误差检测能力，但AIME法本身存在检测时延，在故障排出后无法迅速恢复到告警之前的状态，漏警、误警概率较大，这会影响到主滤波器的全局最优估计。因此，有必要改善上述相关技术方案中存在的一个或者多个问题，以提高导航系统全局估计精度和系统容错性能。
需要说明的是，在上述背景技术部分公开的信息仅用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
发明内容
本公开实施例的目的在于提供一种多源联邦滤波组合导航系统的自适应容错方法，以提高导航系统全局估计精度和系统容错性能。
本公开实施例提供一种多源联邦滤波组合导航系统的自适应容错方法，该方法包括以下步骤：
选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型，其中，所述多源联邦滤波组合导航系统包括主系统和多个子系统，所述主系统对应主滤波器，每个所述子系统对应一个子滤波器；
在一预设时刻，所述主滤波器对每个所述子滤波器进行置信检验；
对所述子滤波器经置信检验后的观测量估计值进行故障检测和容错；
根据所述故障检测和容错的结果，所述主滤波器对无故障的所述子滤波器的估计信息进行融合，得到下一时刻的信息分配因子；
根据得到的所述信息分配因子进行量测更新，继续进行下一时刻的卡尔曼滤波过程。
本公开的一示例性实施例中，在选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型的步骤中，
所述多源联邦滤波组合导航系统的状态方程包括：
Figure BDA0003847293460000021
其中，Xk-1表示k-1时刻的状态向量，Ak,k-1表示状态转移矩阵，Wk表示状态噪声矩阵；Γk表示转移矩阵；
所述多源联邦滤波组合导航系统的状态向量包括：
Figure BDA0003847293460000022
其中，φE,φN,φU表示导航坐标系的姿态角误差；δvE,δvN,δvU分别表示载体东向、北向和天向速度误差；δL,δλ,δh分别表示纬度、经度和高度误差；εx,εy,εz表示陀螺仪常值零偏；
Figure BDA0003847293460000023
表示加速度计随机游走误差，T表示转置。
本公开的一示例性实施例中，在选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型的步骤中，多个所述子系统包括：
第一子系统，所述第一子系统包括捷联惯导/全球导航卫星子系统，所述第一子系统对应第一子滤波器；
第二子系统，所述第二子系统包括捷联惯导/天文导航子系统，所述第二子系统对应第二子滤波器；
第三子系统，所述第三子系统包括捷联惯导/气压高度计子系统，所述第三子系统对应第三子滤波器。
本公开的一示例性实施例中，所述第一子系统的量测方程包括：
Z1(t)＝H1(t)X(t)+V1(t) (3)
其中，Z1(t)表示t时刻量测值，H1(t)表示第一子系统的状态转移矩阵；V1(t)表示第一子系统的量测噪声矩阵；
所述第一子系统的量测转移矩阵包括：
Figure BDA0003847293460000031
其中，上标v表示速度、上标p表示位置相关，h为载体距离地表的高度，L为纬度，I为单位矩阵，RM表示地球子午圈；RN表示地球卯酉圈半径，
Figure BDA0003847293460000032
分别表示k时刻的速度状态转移矩阵、
Figure BDA0003847293460000033
表示k时刻的位置状态转移矩阵；
所述第二子系统的量测方程包括：
Z2(t)＝H2(t)X(t)+V2(t) (5)
其中，Z2(t)表示t时刻量测值，H2(t)表示第二子系统的状态转移矩阵；V2(t)表示第二子系统的量测噪声矩阵；
所述第二子系统的量测转移矩阵包括：
H2(t)＝[Hφ 03×12] (6)
其中，
Figure BDA0003847293460000034
Figure BDA0003847293460000035
分别表示载体航向角；θ表示载体俯仰角；03×12代表3行12列的零矩阵；Hφ表示载体俯仰角的状态转移矩阵；
所述第三子系统的量测方程包括：
Z3(t)＝hSINS-hA＝H3(t)X(t)+V3(t) (7)
其中，hSINS表示估计高度；hA表示气压高度计测得的高度；Z3(t)表示t时刻量测值，H3(t)表示第三子系统的状态转移矩阵；V3(t)表示第三子系统的量测噪声矩阵。
本公开的一示例性实施例中，在一预设时刻所述主滤波器对每个所述子滤波器进行置信检验的步骤包括：
在卡尔曼滤波器中，一步状态预测值包括：
Figure BDA0003847293460000041
其中，
Figure BDA0003847293460000042
为一步状态预测值，Φk+1,k为状态转移矩阵；
根据Γk+1,kWk估计预测向量中
Figure BDA0003847293460000043
第某一维度的置信度，由导航系统噪声Wk的高斯特性，下一时刻的最优估计值服从均值为
Figure BDA0003847293460000044
方差为σ2的高斯分布；
根据标准正态分布的分布函数φ(x)，量测值的边界、均值和置信概率α至少满足：
Figure BDA0003847293460000045
无故障的子传感器在每次测量时，其真实值都应服从正态分布，根据传感器的测量特性，有
Figure BDA0003847293460000046
则置信规则至少满足：
Figure BDA0003847293460000047
其中，若系统判断为不可靠预测，则利用量测方程获得第某一维度的量测值
Figure BDA0003847293460000048
本公开的一示例性实施例中，所述置信概率α设置为0.27％。
本公开的一示例性实施例中，对所述子滤波器经置信检验后的观测量估计值进行故障检测和容错的步骤包括：
在卡尔曼滤波过程中，计算k时刻所述多源联邦滤波组合导航系统的残差矢量，并计算所述多源联邦滤波组合导航系统的标准化残差
Figure BDA0003847293460000049
当所述标准化残差
Figure BDA00038472934600000410
满足
Figure BDA00038472934600000411
时，所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测；否则，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错。
本公开的一示例性实施例中，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错的步骤包括：
当所述标准化残差
Figure BDA0003847293460000051
时，所述多源联邦滤波组合导航系统判断存在故障，进行局部故障隔离；
当所述标准化残差
Figure BDA0003847293460000052
时，所述多源联邦滤波组合导航系统判断不存在故障，则计算等价权函数。
本公开的一示例性实施例中，当所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测时，对应的标准化残差公式包括：
Figure BDA0003847293460000053
其中，
Figure BDA0003847293460000054
表示基于滑动窗口滤波的残差向量；
Figure BDA0003847293460000055
表示基于滑动窗口滤波的协方差矩阵；
当所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错时，对应的标准化残差公式包括：
Figure BDA0003847293460000056
其中，rk j表示基于当前历元的残差向量；Vk jj表示基于当前历元的协方差矩阵；
引入等价权函数，所述等价权函数包括三段抗差等价权函数，其计算公式包括：
Figure BDA0003847293460000057
其中，t1,t2为固定常量，t1,t2的值根据实际工作情况确定；
经所述三段抗差等价权函数的调节后，观测噪声的方差阵公式包括：
Figure BDA0003847293460000058
其中，
Figure BDA0003847293460000059
为观测噪声方差阵Rk的对角线元素；
相应的，对应的自适应增益矩阵公式包括：
Figure BDA0003847293460000061
其中，Kk表示卡尔曼增益，Hk表示状态转移矩阵；Pk表示及均方差矩阵。
本公开的一示例性实施例中，根据检测和容错结果，所述主滤波器对无故障的所述子滤波器的估计信息进行融合，得到下一时刻的信息分配因子的步骤包括：
利用一预设时间内的残差序列，得到所述多源联邦滤波组合导航系统的协方差的极大似然估计值公式包括：
Figure BDA0003847293460000062
其中，M代表预设时间序列所包含的时段数量(M<k)，rk为残差向量，下标k表示第k时段，i表示数列序号，取值1～M-1，
Figure BDA0003847293460000063
为rk-i的转置向量；
利用公式(11)和所述多源联邦滤波组合导航系统的协方差理论值公式：
Figure BDA0003847293460000064
构造自适应分配系数，包括：
Figure BDA0003847293460000065
Figure BDA0003847293460000066
Figure BDA0003847293460000067
其中，Vk表示残差均方差；tr(Vk)、
Figure BDA0003847293460000068
分别为矩阵Vk、
Figure BDA0003847293460000069
的迹，即该矩阵对角线元素之和，c为常数，其值通常由实验测得；εrk用来反映子滤波器的故障程度，βk表示自适应分配系数，单位化处理后得到
Figure BDA00038472934600000610
本公开提供的技术方案可以包括以下有益效果：
本公开实施例中，提出一种多源联邦滤波组合导航系统的自适应容错方法能够降低了故障告警时延，提升了无故障的子滤波器的精度和多源联邦滤波组合导航系统的重构能力；通过子滤波器的故障检测函数值的可动态调节的信息分配因子，并加入置信检验步骤，防止了观测向量因某一维度故障而造成的直接隔离，减少了主滤波器最优估计的精度损失。本方法可有效降低多源联邦滤波组合导航系统故障检测的误警率和告警时间，相比传统的联邦滤波具有更好的全局估计精度和缓变误差检测性能。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。显而易见的，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出本公开示例性实施例中多源联邦滤波组合导航系统的自适应容错方法的步骤示意图；
图2示出本公开示例性实施例中联邦滤波器原理的结构示意图；
图3示出本公开示例性实施例的多源联邦滤波组合导航系统中多源联邦滤波器整体结构示意图；
图4示出本公开示例性实施例中多源联邦组合导航系统中自适应故障检测和容错方法的框图；
图5示出本公开示例性实施例的仿真实验中无人机飞行轨迹示意图；
图6示出本公开示例性实施例的仿真实验中传统AIME算法和改进后的AIME算法的检验统计量对比示意图；
图7示出本公开示例性实施例的仿真实验中传统AIME算法和改进后的AIME算法的水平位置整体估计误差对比示意图；
图8示出本公开示例性实施例的仿真实验中传统AIME算法和改进后的AIME算法的水平速度整体估计误差对比示意图。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本公开将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。
此外，附图仅为本公开的示意性图解，并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分，因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。
本示例实施方式中提供了一种多源联邦滤波组合导航系统的自适应容错方法，参考附图1和图3中所示，该方法可以包括以下步骤：
步骤S101：选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型，其中，该多源联邦滤波组合导航系统包括主系统和多个子系统，主系统对应主滤波器，每个子系统对应一个子滤波器；
步骤S102：在一预设时刻，主滤波器对每个子滤波器进行置信检验；
步骤S103：对子滤波器经置信检验后的观测量估计值进行故障检测和容错；
步骤S104：根据检测和容错的结果，主滤波器对无故障的子滤波器的估计信息进行融合，得到下一时刻的信息分配因子；
步骤S105：根据得到的信息分配因子进行量测更新，继续进行下一时刻的卡尔曼滤波过程。
本示例实施例中，能够降低了故障告警时延，提升了无故障的子滤波器的精度和多源联邦滤波组合导航系统的重构能力；通过子滤波器的故障检测函数值的可动态调节的信息分配因子，并加入置信检验步骤，防止了观测向量因某一维度故障而造成的直接隔离，减少了主滤波器最优估计的精度损失。本公开实施例可有效降低多源联邦滤波组合导航系统故障检测的误警率和告警时间，相比传统的联邦滤波具有更好的全局估计精度和缓变误差检测性能。
下面，对本示例实施例中的上述方法的各个步骤进行更详细的说明。
联邦滤波器属于一种并行两级结构的分散化滤波器，由一个主滤波器和多个子滤波器构成。每个子滤波器将对应的局部传感器的量测信息与参考系统状态估计信息进行滤波处理，并进一步将滤波信息输入到主滤波器中，由主滤波器按最优融合算法进行合成，同时根据信息分配原理对每个子滤波器进行重置，最终获得基于所有量测信息的全局最优估计，这就是联邦滤波器的一般原理。
参照图2，若有N个子系统，主滤波器和子滤波器共用参考系统的状态变量，则系统的状态方程包括：
Xk+1＝Φk+1,kXk+Γk+1,kWk，
其中，Φk+1,k表示主滤波器状态转移矩阵，Xk为状态向量，Wk,Γk表示状态噪声矩阵及其转移矩阵。
相应的，第i个子系统的量测方程包括：
Figure BDA0003847293460000091
这里，Wk的协方差阵用Q表示，
Figure BDA0003847293460000092
的协方差阵用Ri表示。
联邦卡尔曼滤波理论包括了信息分配、子滤波器时间更新和主滤波器信息融合三个过程，具体如下：
1)信息分配
Figure BDA0003847293460000093
利用上一时刻主滤波器的最优估计值来重置子滤波器对应的状态信息。
2)子滤波器更新
时间更新：
Figure BDA0003847293460000094
Figure BDA0003847293460000095
量测更新：
Figure BDA0003847293460000096
Figure BDA0003847293460000097
Figure BDA0003847293460000101
3)主滤波器信息融合
Figure BDA0003847293460000102
Figure BDA0003847293460000103
将各子滤波器输出信息进行最优融合得到主滤波器对应的输出值。
结合本示例实施例，参照图1，
步骤S101包括选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型，其中，该多源联邦滤波组合导航系统包括主系统和多个子系统，主系统对应主滤波器，每个子系统对应一个子滤波器。
本示例中，选取东北天坐标系作为n系，即导航坐标系，则状态向量包括了捷联惯导(strapdown intertial navigation system,SINS)参考系统的陀螺仪、加速度计等误差量，得到多源联邦滤波组合导航系统的状态方程包括：
Figure BDA0003847293460000104
其中，Xk-1表示k-1时刻的状态向量，Ak,k-1表示状态转移矩阵，Wk表示状态噪声矩阵；Γk表示转移矩阵。
所述多源联邦滤波组合导航系统的状态向量包括：
Figure BDA0003847293460000105
其中，φE,φN,φU表示导航坐标系的姿态角误差；δvE,δvN,δvU分别表示载体东向、北向和天向速度误差；δL,δλ,δh分别表示纬度、经度和高度误差；εx,εy,εz表示陀螺仪常值零偏；
Figure BDA0003847293460000106
表示加速度计随机游走误差，T表示转置。
进一步的，在本示例中，包括了
第一子系统包括捷联惯导(SINS)/全球导航卫星系统(global navigationsatellite system,GNSS)子系统，即SINS/GNSS子系统；第一子系统对应第一子滤波器。
第一子系统的量测方程包括：
Z1(t)＝H1(t)X(t)+V1(t) (3)
其中，Z1(t)表示t时刻量测值，H1(t)表示第一子系统的状态转移矩阵；V1(t)表示第一子系统的量测噪声矩阵。
第一子系统的量测转移矩阵包括：
Figure BDA0003847293460000111
其中，上标v表示速度、上标p表示位置相关，h为载体距离地表的高度，L为纬度，I为单位矩阵，RM表示地球子午圈；RN表示地球卯酉圈半径，
Figure BDA0003847293460000112
分别表示k时刻的速度状态转移矩阵、
Figure BDA0003847293460000113
表示k时刻的位置状态转移矩阵。
第二子系统包括捷联惯导(SINS)/天文导航子系统(celestial navigationsystem,CNS)，即SINS/CNS子系统；第二子系统对应第二子滤波器。
第二子系统的量测方程包括：
Z2(t)＝H2(t)X(t)+V2(t) (5)
其中，Z2(t)表示t时刻量测值，H2(t)表示第二子系统的状态转移矩阵；V2(t)表示第二子系统的量测噪声矩阵；在式(5)中，第二子系统的量测转移矩阵为惯导数学平台误差角到姿态误差角之间的转换矩阵，包括：
H2(t)＝[Hφ 03×12] (6)
其中，
Figure BDA0003847293460000114
Figure BDA0003847293460000115
分别表示载体航向角；θ表示载体俯仰角；Hφ表示载体俯仰角的状态转移矩阵；03×12代表3行12列的零矩阵。
第三子系统包括捷联惯导(SINS)/气压高度计子系统(barometric altimeter,PA)，即SINS/PA子系统；第三子系统对应第三子滤波器。
第三子系统的量测方程包括：
Z3(t)＝hSINS-hA＝H3(t)X(t)+V3(t) (7)
其中，hSINS表示估计高度；hA表示气压高度计测得的高度；Z3(t)表示t时刻量测值，H3(t)表示第三子系统的状态转移矩阵；V3(t)表示第三子系统的量测噪声矩阵；
气压高度计内包含气压敏感元件，可根据气压与高度的函数关系间接测定高度值，因此可用来测量载体的绝对高度和标准气压高度。
步骤S102包括在一预设时刻，所述主滤波器对每个所述子滤波器进行置信检验。
为了提升故障检测的效率，减少因任意一个传感器发生故障或观测值不准确时，直接隔离导致对主滤波器输出的最优估计值精度损失，本示例实施例中采用置信检验自适应联邦卡尔曼滤波(CC-AFKF)算法，对每一时刻的量测值都进行依次置信检验，对不满足条件的量测值根据量测方程进行修正，保证最优估计的精度。
在卡尔曼滤波器中，一步状态预测值包括：
Figure BDA0003847293460000121
其中，
Figure BDA0003847293460000122
为一步状态预测值，Φk+1,k为状态转移矩阵；
根据Γk+1,kWk估计预测向量中
Figure BDA0003847293460000123
第某一维度的置信度，由导航系统噪声Wk的高斯特性，下一时刻的最优估计值服从均值为
Figure BDA0003847293460000124
方差为σ2的高斯分布；
根据标准正态分布的分布函数φ(x)，量测值的边界、均值和置信概率α至少满足：
Figure BDA0003847293460000125
无故障的子传感器在每次测量时，其真实值都应服从正态分布，根据传感器的测量特性，有
Figure BDA0003847293460000126
则置信规则至少满足：
Figure BDA0003847293460000127
其中，若系统判断为不可靠预测，则利用量测方程获得第某一维度的量测值
Figure BDA0003847293460000128
具体的，根据整天分布的“3σ”法则，当α设置为0.27％时，置信检验可以过滤掉传感器在出厂误差范围之外的量测值。实际应用场合中，为提高融合精度，可通过重复试验合理选取α值。
步骤S103包括对子滤波器经置信检验后的观测量估计值进行故障检测和容错；
参照图4，步骤S103包含以下步骤：
S1031：在卡尔曼滤波过程中，计算k时刻所述多源联邦滤波组合导航系统的残差矢量，并计算所述多源联邦滤波组合导航系统的标准化残差
Figure BDA0003847293460000131
S1032：当所述标准化残差
Figure BDA0003847293460000132
满足
Figure BDA0003847293460000133
时，所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测；否则，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错。
这里需要说明的是，传统的AIME算法较传统的差卡方检测法提高了对缓变误差的检测效率，但是单一的检测门限对于小幅值故障检测容易造成漏警现象，因此本示例实施例中，为了提升小幅值故障的检测和容错能力，对AIME算法进行改进，引入了等价权函数，特别是三段抗差等价权函数。
在多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错时，包括：
当标准化残差
Figure BDA0003847293460000134
时。该多源联邦滤波组合导航系统判断存在故障，进行局部故障隔离；
当标准化残差
Figure BDA0003847293460000135
时，该多源联邦滤波组合导航系统判断不存在故障，则计算等价权函数。
进一步的，当多源联邦滤波组合导航系统判断采用对滑动窗口进行检测时，对应的标准化残差公式包括：
Figure BDA0003847293460000136
其中，
Figure BDA0003847293460000137
表示基于滑动窗口滤波的残差向量；
Figure BDA0003847293460000138
表示基于滑动窗口滤波的协方差矩阵；
当多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错时，对应的标准化残差公式包括：
Figure BDA0003847293460000141
其中，rk j表示基于当前历元的残差向量；Vk jj表示基于当前历元的协方差矩阵；
引入等价权函数，所述等价权函数包括三段抗差等价权函数，其计算公式包括：
Figure BDA0003847293460000142
其中，t1,t2为固定常量，t1,t2的值根据实际工作情况确定；
经过等价权函数的调节，每一个维度观测量都将根据自身观测质量被赋予其不同的权重，并直接影响其在卡尔曼滤波过程中的权重，观测信息残差越小，所得权重越大。当观测数据存在小幅值故障偏差但检测函数值小于t2时，该故障数据的权重降至1以下，从而减少了该维度观测在后续卡尔曼滤波量测更新中的占比在提升滤波精度的同时增强后续故障检测的灵敏度。
经所述三段抗差等价权函数的调节后，观测噪声的方差阵公式包括：
Figure BDA0003847293460000143
其中，
Figure BDA0003847293460000144
为观测噪声方差阵Rk的对角线元素；相应的，对应的自适应增益矩阵公式包括：
Figure BDA0003847293460000145
其中，Kk表示卡尔曼增益，Hk表示状态转移矩阵；Pk表示及均方差矩阵。
这里需要说明的是，当无故障发生时，滑动窗口检测得到的检验统计量λk一般大于基于当前历元针对局部残差检测结果，这有可能会导致对正常数据的误判；并且若全程采用滑动窗口检测还会导致大量历史数据的堆积和协方差矩阵的计算，带来较大的计算负担。因此，在使用滑动窗口检测应遵循一定的准则，防止其负面效果影响到多源联邦滤波器的性能。
在式(13)中，标准化残差
Figure BDA0003847293460000151
与当前历元检测相关，而当前历元检测取决于
Figure BDA0003847293460000152
当所述标准化残差
Figure BDA0003847293460000153
满足
Figure BDA0003847293460000154
时，所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测，标准化残差
Figure BDA0003847293460000155
依照式(11)计算；否则，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错，标准化残差
Figure BDA0003847293460000156
依照式(12)计算。通过这两种检测方法的适时切换，得到改进后的AIME算法，如图4所示。
步骤S104包括根据检测和容错的结果，主滤波器对无故障的子滤波器的估计信息进行融合，得到下一时刻的信息分配因子。
在该步骤中，利用一预设时间内的残差序列，得到多源联邦滤波组合导航系统的协方差的极大似然估计值公式包括：
Figure BDA0003847293460000157
其中，M代表预设时间序列所包含的时段数量(M<k)，rk为残差向量，下标k表示第k时刻，i表示数列序号，取值1～M-1，
Figure BDA0003847293460000158
为rk-i的转置向量。
利用公式(11)和多源联邦滤波组合导航系统的协方差理论值公式：
Figure BDA0003847293460000159
构造自适应分配系数，包括：
Figure BDA00038472934600001510
Figure BDA00038472934600001511
Figure BDA00038472934600001512
其中，Vk表示残差均方差；tr(Vk)、
Figure BDA00038472934600001513
分别为矩阵Vk、
Figure BDA00038472934600001514
的迹，即该矩阵对角线元素之和，c为常数，其值通常由实验测得；εrk用来反映子滤波器的故障程度，βk表示自适应分配系数，单位化处理后得到
Figure BDA0003847293460000161
对于发生故障的子滤波器，信息分配因子会相应减小，这样该子滤波器的状态估计更多依赖于上一时刻的整体融合估计，对当前自身包含故障偏差的量测信息利用程度会相应的减小，这意味着此时该子滤波器获得了对于故障观测更佳的容错性能。
与此同时，子滤波器受到主滤波器反馈重置的影响程度与信息分配因子大小正相关，当子滤波器发生故障时，增加其信息分配因子的大小可以减少该子滤波器对自身量测的利用程度，避免故障信息影响整体滤波估计，并且还能提升故障检测效率。具体解释如下：
根据AIME算法，导航系统残差公式包括：rk＝Zk-HkXk/k-1
其中，Zk,Hk为k时刻量测值及状态转移矩阵，Xk/k-1为该时刻一步状态预测估计。
其协方差理论值公式包括：
Figure BDA0003847293460000162
其中，Pk,Rk分别为均方差矩阵及观测噪声均方差。
则得到检验统计量公式包括：
Figure BDA0003847293460000163
其中，
Figure BDA0003847293460000164
ravg,Vavg分别为基于滑动窗口滤波的残差向量及其协方差矩阵M为估计窗口的长度；可以看出滑动窗口检测中信道额残差向量是之前预设时间内卡尔曼滤波残差的加权平均；
这样，可以得出子滤波器i在k时刻的残差向量公式包括：
Figure BDA0003847293460000165
其中，
Figure BDA0003847293460000166
为k时刻子滤波器i的量测值及状态转移矩阵，
Figure BDA0003847293460000167
为该时刻子滤波器i对应一步状态预测估计。
子滤波器i在k时刻的协方差矩阵公式包括：
Figure BDA0003847293460000168
其中，
Figure BDA0003847293460000169
分别为子滤波器i的均方差矩阵及观测噪声均方差。
相应的，根据上述公式可知，故障检测函数公式为：
Figure BDA00038472934600001610
由此，可以看出，故障检测函数与残差及协方差矩阵相关，残差越大，协方差矩阵越小，故障检测函数数值越大，这表示故障更容易被检测出来。在故障检测函数公式中，残差矢量仅仅与上一时刻整体融合估计相关，不受信息分配因子影响；但信息分配因子越大，残差协方差越小，
Figure BDA0003847293460000171
的值就越大，从而越有利于检测出故障异常。
需要说明的是，由残差的统计特性可知，Λ服从自由度n为的χ2分布，这里n为观测向量的维数，设虚警概率为τ，则有:
Figure BDA0003847293460000172
其中，Td为虚警概率的门限；故障判决需满足：
Figure BDA0003847293460000173
若把虚警概率τ设置为0.001，则故障检测门限Td与观测向量维数n的关系如下表1所示：
表1：故障检测门限Td与观测向量维数n的关系
Figure BDA0003847293460000174
步骤S105包括根据得到的所述信息分配因子进行量测更新，继续进行下一时刻的卡尔曼滤波过程。
综上所述，本公开实施例中，提出一种多源联邦滤波组合导航系统的自适应容错方法能够降低了故障告警时延，该方法对传统AIME算法进行改进，引入抗差等价权函数对量测噪声进行自适应调节，本剧标准化残差实时切换不同的故障检测方式，降低了故障告警时延，提升了无故障的子滤波器的精度和多源联邦滤波组合导航系统的重构能力；通过子滤波器的故障检测函数值的可动态调节的信息分配因子，并加入置信检验步骤，防止了观测向量因某一维度故障而造成的直接隔离，减少了主滤波器最优估计的精度损失。本方法可有效降低多源联邦滤波组合导航系统故障检测的误警率和告警时间，相比传统的联邦滤波具有更好的全局估计精度和缓变误差检测性能。
为了验证本公开实施例的优异性，进行了如下仿真实验。
仿真实验中模拟飞机飞行状态，仿真轨迹包括匀加速、爬升、转弯、匀速平飞和俯冲等运动状态。这里假设陀螺仪随机常值漂移为0.1°/h，加速度计随机游走误差为10μg，惯导参考系统采样频率为100Hz，CNS输出频率为1Hz，姿态测量误差为5″，气压计测量误差为0.5m，GNSS位置测量误差为2m，速度测量误差为0.5m/s，卡尔曼滤波周期为0.5s，滑动窗口数设为10，仿真飞行时间为400s。
参照图5，示出了仿真实验中无人机飞行轨迹示意图。为了验证本公开实施例中算法的有效性，假设惯导参考系统全程无故障发生，大气压强不发生剧烈变化，GNSS子系统水平位置在150s～250s产生软故障，件图5中飞行轨迹的虚线段，其故障速率为0.2m/s；同时由于天气原因，GNSS与CNS子系统均产生一定的突变故障,GNSS子系统位置测量误差下降至10m，速度测量误差下降至2m/s，CNS子系统姿态测量误差下降至20″。经故障信息处理后的检验统计量可以参看图6所示。
参照图6，示出了仿真实验中传统AIME算法和改进后的AIME算法的检验统计量对比。假设误警概率为0.01,则由假设检验相关理论得到子系统对应检测门限为16.812；由图6可以看到，在150s～250s故障期间，传统AIME算法与本文改进后的算法都能够对缓变故障及时做出响应，而且均大于检测门限；但改进后的算法在故障结束后能更快结束告警，相比之下传统AIME算法则需要10s左右才能恢复到正常统计量。
参照图7，示出了传统AIME算法和改进后的AIME算法的水平位置整体估计误差对比；图8示出了传统AIME算法和改进后的AIME算法的水平速度整体估计误差对比。
由图7和图8在两种故障检测模式下，分别根据多源联邦滤波器的水平位置和水平速度的整体估计误差对比可以看出，采用传统AIME算法时，联邦滤波器的水平位置和水平速度的误差随时间推移变化较大，分别达到了±8m和±0.5m/s；而采用本公开实施例改进的AIME算法时，水平位置和水平速度误差分别缩小到±5m与±0.3m/s左右，因此本公开实施例有效的降低了故障信息对整体估计的影响。
根据以上两组仿真对比实验，可以看到，当多源联邦滤波器的某一子系统发生缓变误差与突变误差时，本公开实施例改进后的AIME算法能够有效弥补传统AIME算法告警响应与回复的时延问题，同时加入基于检验统计量的信息分配因子和置信检验规则，能够提高多源联邦组合导航系统的容错能力和估计精度，从而提高了多源俩那帮滤波器对缓变故障和突变故障的容错性能。
综上所述，本公开实施例提出的一种多源联邦滤波组合导航系统的自适应容错方法，基于SINS/GNSS/CNS/PA的联邦滤波容错算法，针对传统残差检验算法对缓变误差不敏感，以及AIME算法存在告警延长等问题，利用分段抗差函数对卡尔曼滤波相关参数进行调整，根据当前历元的观测量来灵活切换故障监测算法，并通过各子滤波器的故障检验统计量来动态调节信息分配因子，加入置信先验规则，降低故障信息对全局滤波估计的影响，尽可能保留正确信息，进一步提高故障检测效率。通过仿真实验表明，本公开实施例具备较高的位置与速度估计精度，对于缓变故障与突变故障具有较好的响应能力，对于提升复杂环境下无人机组合导航系统的自主容错能力具有一定的参考价值。
需要说明的是，尽管在附图中以特定顺序描述了本公开中方法的各个步骤，但是，这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。另外，也易于理解的是，这些步骤可以是例如在多个模块/进程/线程中同步或异步执行。
应当注意，尽管在上文详细描述中提及了用于动作执行的系统的若干单元，但是这种划分并非强制性的。实际上，根据本公开的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之，上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。可以根据实际的需要选择其中的部分或者全部单元来实现本公开方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由所附的权利要求指出。

Claims (10)

1.一种多源联邦滤波组合导航系统的自适应容错方法，其特征在于，包括以下步骤：
选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型，其中，所述多源联邦滤波组合导航系统包括主系统和多个子系统，所述主系统对应主滤波器，每个所述子系统对应一个子滤波器；
在一预设时刻，所述主滤波器对每个所述子滤波器进行置信检验；
对所述子滤波器经置信检验后的观测量估计值进行故障检测和容错；
根据所述故障检测和容错的结果，所述主滤波器对无故障的所述子滤波器的估计信息进行融合，得到下一时刻的信息分配因子；
根据得到的所述信息分配因子进行量测更新，继续进行下一时刻的卡尔曼滤波过程。
2.根据权利要求1所述自适应容错方法，其特征在于，在选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型的步骤中，
所述多源联邦滤波组合导航系统的状态方程包括：
Figure FDA0003847293450000011
其中，Xk-1表示k-1时刻的状态向量，Ak,k-1表示状态转移矩阵，Wk表示状态噪声矩阵；Γk表示转移矩阵；
所述多源联邦滤波组合导航系统的状态向量包括：
X＝[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,▽x,▽y,▽z]T (2)
其中，φE,φN,φU表示导航坐标系的姿态角误差；δvE,δvN,δvU分别表示载体东向、北向和天向速度误差；δL,δλ,δh分别表示纬度、经度和高度误差；εx,εy,εz表示陀螺仪常值零偏；▽x,▽y,▽z表示加速度计随机游走误差，T表示转置。
3.根据权利要求2所述自适应容错方法，其特征在于，在选取东北天坐标系作为导航坐标系，建立多源联邦滤波组合导航系统的模型的步骤中，多个所述子系统包括：
第一子系统，所述第一子系统包括捷联惯导/全球导航卫星子系统，所述第一子系统对应第一子滤波器；
第二子系统，所述第二子系统包括捷联惯导/天文导航子系统，所述第二子系统对应第二子滤波器；
第三子系统，所述第三子系统包括捷联惯导/气压高度计子系统，所述第三子系统对应第三子滤波器。
4.根据权利要求3所述自适应容错方法，其特征在于，
所述第一子系统的量测方程包括：
Z1(t)＝H1(t)X(t)+V1(t) (3)
其中，Z1(t)表示t时刻量测值，H1(t)表示第一子系统的状态转移矩阵；V1(t)表示第一子系统的量测噪声矩阵；
所述第一子系统的量测转移矩阵包括：
Figure FDA0003847293450000021
其中，上标v表示速度、上标p表示位置相关，h为载体距离地表的高度，L为纬度，I为单位矩阵，RM表示地球子午圈；RN表示地球卯酉圈半径，
Figure FDA0003847293450000022
分别表示k时刻的速度状态转移矩阵、
Figure FDA0003847293450000023
表示k时刻的位置状态转移矩阵；
所述第二子系统的量测方程包括：
Z2(t)＝H2(t)X(t)+V2(t) (5)
其中，Z2(t)表示t时刻量测值，H2(t)表示第二子系统的状态转移矩阵；V2(t)表示第二子系统的量测噪声矩阵；
所述第二子系统的量测转移矩阵包括：
H2(t)＝[Hφ 03×12] (6)
其中，
Figure FDA0003847293450000024
Figure FDA0003847293450000025
分别表示载体航向角；θ表示载体俯仰角；03×12代表3行12列的零矩阵；Hφ表示载体俯仰角的状态转移矩阵；
所述第三子系统的量测方程包括：
Z3(t)＝hSINS-hA＝H3(t)X(t)+V3(t) (7)
其中，hSINS表示估计高度；hA表示气压高度计测得的高度；Z3(t)表示t时刻量测值，H3(t)表示第三子系统的状态转移矩阵；V3(t)表示第三子系统的量测噪声矩阵。
5.根据权利要求1所述自适应容错方法，其特征在于，在一预设时刻所述主滤波器对每个所述子滤波器进行置信检验的步骤包括：
在卡尔曼滤波器中，一步状态预测值包括：
Figure FDA0003847293450000031
其中，
Figure FDA0003847293450000032
为一步状态预测值，Φk+1,k为状态转移矩阵；
根据Γk+1,kWk估计预测向量中
Figure FDA0003847293450000033
第某一维度的置信度，由导航系统噪声Wk的高斯特性，下一时刻的最优估计值服从均值为
Figure FDA0003847293450000034
方差为σ2的高斯分布；
根据标准正态分布的分布函数φ(x)，量测值的边界、均值和置信概率α至少满足：
Figure FDA0003847293450000035
无故障的子传感器在每次测量时，其真实值都应服从正态分布，根据传感器的测量特性，有
Figure FDA0003847293450000036
则置信规则至少满足：
Figure FDA0003847293450000037
其中，若系统判断为不可靠预测，则利用量测方程获得第某一维度的量测值
Figure FDA0003847293450000038
6.根据权利要求5所述自适应容错方法，其特征在于，所述置信概率α设置为0.27％。
7.根据权利要求1所述自适应容错方法，其特征在于，对所述子滤波器经置信检验后的观测量估计值进行故障检测和容错的步骤包括：
在卡尔曼滤波过程中，计算k时刻所述多源联邦滤波组合导航系统的残差矢量，并计算所述多源联邦滤波组合导航系统的标准化残差
Figure FDA0003847293450000039
当所述标准化残差
Figure FDA0003847293450000041
满足
Figure FDA0003847293450000042
时，所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测；否则，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错。
8.根据权利要求7所述自适应容错方法，其特征在于，所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错的步骤包括：
当所述标准化残差
Figure FDA0003847293450000043
时，所述多源联邦滤波组合导航系统判断存在故障，进行局部故障隔离；
当所述标准化残差
Figure FDA0003847293450000044
时，所述多源联邦滤波组合导航系统判断不存在故障，则计算等价权函数。
9.根据权利要求8所述自适应容错方法，其特征在于，当所述多源联邦滤波组合导航系统判断采用对滑动窗口进行检测时，对应的标准化残差公式包括：
Figure FDA0003847293450000045
其中，
Figure FDA0003847293450000046
表示基于滑动窗口滤波的残差向量；
Figure FDA0003847293450000047
表示基于滑动窗口滤波的协方差矩阵；
当所述多源联邦滤波组合导航系统判断采用对当前历元进行检测和容错时，对应的标准化残差公式包括：
Figure FDA0003847293450000048
其中，rk j表示基于当前历元的残差向量；Vk jj表示基于当前历元的协方差矩阵；
引入等价权函数，所述等价权函数包括三段抗差等价权函数，其计算公式包括：
Figure FDA0003847293450000049
其中，t1,t2为固定常量，t1,t2的值根据实际工作情况确定；
经所述三段抗差等价权函数的调节后，观测噪声的方差阵公式包括：
Figure FDA0003847293450000051
其中，
Figure FDA0003847293450000052
为观测噪声方差阵Rk的对角线元素；
相应的，对应的自适应增益矩阵公式包括：
Figure FDA0003847293450000053
其中，Kk表示卡尔曼增益，Hk表示状态转移矩阵；Pk表示及均方差矩阵。
10.根据权利要求1所述自适应容错方法，其特征在于，根据检测和容错结果，所述主滤波器对无故障的所述子滤波器的估计信息进行融合，得到下一时刻的信息分配因子的步骤包括：
利用一预设时间内的残差序列，得到所述多源联邦滤波组合导航系统的协方差的极大似然估计值公式包括：
Figure FDA0003847293450000054
其中，M代表预设时间序列所包含的时段数量(M<k)，rk为残差向量，下标k表示第k时段，i表示数列序号，取值1～M-1，
Figure FDA0003847293450000055
为rk-i的转置向量；
利用公式(11)和所述多源联邦滤波组合导航系统的协方差理论值公式：
Figure FDA0003847293450000056
构造自适应分配系数，包括：
Figure FDA0003847293450000057
Figure FDA0003847293450000058
Figure FDA0003847293450000059
其中，Vk表示残差均方差；tr(Vk)、
Figure FDA0003847293450000061
分别为矩阵Vk、
Figure FDA0003847293450000062
的迹，即该矩阵对角线元素之和，c为常数，其值通常由实验测得；εrk用来反映子滤波器的故障程度，βk表示自适应分配系数，单位化处理后得到
Figure FDA0003847293450000063
CN202211123293.3A 2022-09-15 2022-09-15 一种多源联邦滤波组合导航系统的自适应容错方法 Pending CN115468559A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211123293.3A CN115468559A (zh) 2022-09-15 2022-09-15 一种多源联邦滤波组合导航系统的自适应容错方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211123293.3A CN115468559A (zh) 2022-09-15 2022-09-15 一种多源联邦滤波组合导航系统的自适应容错方法

Publications (1)

Publication Number Publication Date
CN115468559A true CN115468559A (zh) 2022-12-13

Family

ID=84333405

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211123293.3A Pending CN115468559A (zh) 2022-09-15 2022-09-15 一种多源联邦滤波组合导航系统的自适应容错方法

Country Status (1)

Country Link
CN (1) CN115468559A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116086466A (zh) * 2022-12-28 2023-05-09 淮阴工学院 一种提高ins误差精度的方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116086466A (zh) * 2022-12-28 2023-05-09 淮阴工学院 一种提高ins误差精度的方法
CN116086466B (zh) * 2022-12-28 2024-03-26 淮阴工学院 一种提高ins误差精度的方法

Similar Documents

Publication Publication Date Title
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111928846B (zh) 一种基于联邦滤波的多源融合即插即用组合导航方法
US6417802B1 (en) Integrated inertial/GPS navigation system
US6757624B1 (en) Synthetic pressure altitude determining system and method of integrity monitoring from a pressure sensor
US5760737A (en) Navigation system with solution separation apparatus for detecting accuracy failures
Zhang et al. Improved fault detection method based on robust estimation and sliding window test for INS/GNSS integration
CN109471143B (zh) 自适应容错的列车组合定位方法
CN113670337B (zh) 一种用于gnss/ins组合导航卫星缓变故障检测方法
CN112902967A (zh) 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法
CN112650281B (zh) 多传感器三余度系统、控制方法、无人机、介质及终端
CN111366156A (zh) 基于神经网络辅助的变电站巡检机器人导航方法及系统
CN114966762B (zh) 一种gnss/ins紧组合导航系统的故障检测方法
CN107121684B (zh) 一种基于残差卡方检验法的gps诱骗识别和阈值决策方法
CN115468559A (zh) 一种多源联邦滤波组合导航系统的自适应容错方法
CN115047496A (zh) 一种用于gnss/ins组合导航卫星同步多故障检测方法
CN113984054A (zh) 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
Lee et al. Optimal combination of fault detection and isolation methods of integrated navigation algorithm for UAV
Yi-ting et al. A fast gradual fault detection method for underwater integrated navigation systems
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
CN112179347B (zh) 一种基于光谱红移误差观测方程的组合导航方法
CN110779549A (zh) 一种水下组合导航系统突变型故障诊断方法
CN116756686A (zh) 一种飞行器的强抗扰高度状态估计方法及系统
CN115291253A (zh) 一种基于残差检测的车辆定位完好性监测方法及系统
CN115061173A (zh) 一种基于联邦卡尔曼滤波的gps/aoa/sins组合导航方法
Bo et al. A method for fault detection and isolation in the integrated navigation system for UAV

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