CN111854728B - 一种基于广义相对熵的容错滤波方法 - Google Patents

一种基于广义相对熵的容错滤波方法 Download PDF

Info

Publication number
CN111854728B
CN111854728B CN202010431864.4A CN202010431864A CN111854728B CN 111854728 B CN111854728 B CN 111854728B CN 202010431864 A CN202010431864 A CN 202010431864A CN 111854728 B CN111854728 B CN 111854728B
Authority
CN
China
Prior art keywords
fault
measurement
state
sins
representing
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
CN202010431864.4A
Other languages
English (en)
Other versions
CN111854728A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010431864.4A priority Critical patent/CN111854728B/zh
Publication of CN111854728A publication Critical patent/CN111854728A/zh
Application granted granted Critical
Publication of CN111854728B publication Critical patent/CN111854728B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

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)
  • Navigation (AREA)

Abstract

本发明的目的在于提供一种基于广义相对熵的容错滤波方法，建立SINS/GPS、SINS/地磁匹配和SINS/重力辅助系统的状态空间模型，利用联邦滤波方法和改进故障检测方法对故障进行容错处理。改进的故障检测方法利用了广义相对熵(也称KL散度)原理，使用预测信息和更新信息对KL散度和马氏距离的计算结果进行权值分配，将两者的加权和作为传感器故障检测指标。经判断无故障的状态估计正常进入主滤波器进行信息融合，而有故障的状态估计所在子滤波器通道会关闭以避免故障信息污染其他子系统。本发明有更好的故障检测精度和更高的运算效率，对进一步提高复杂条件下多源信息融合系统的容错能力具有一定的参考意义。

Description

一种基于广义相对熵的容错滤波方法
技术领域
本发明涉及的是一种组合导航故障检测方法，具体地说是容错滤波方法。
背景技术
捷联式惯性导航系统(Strapdown Inertial Navigation System,SINS)定位过程不依赖外界信息，具有高隐蔽性，可自主导航，但导航定位误差会随时间累积。卫星导航系统定位精度高，长期稳定性好，但容易受到干扰、屏蔽。地磁匹配导航可以全天候工作，导航误差不累积，但其导航精度受载体高度限制，仅在低轨道卫星高度以下有较高精度。重力辅助导航不受时间限制，对外无辐射，但其导航精度受地形起伏度影响，在海洋、沙漠等平坦地形区精度较差。根据SINS、卫星导航、地磁匹配导航和重力辅助导航特点，将各系统进行组合，可以在很大程度上提高组合导航性能。联邦滤波由于设计灵活、计算量小、容错性能好而得到了广泛的应用。系统的结构特点决定了故障概率必然增加，一旦发生故障，就会影响系统性能，甚至会对系统造成不可估量的后果。联邦滤波的故障检测一般使用χ2检测法，而状态χ2检测虽然检测精度较高，但需要计算两个状态估计值，计算量较大，残差χ2检测计算量虽有显著减小，但对软故障情况检测精度低。
在已发表文章中，杨春等人(杨春,张磊,郭健,等.采用双状态传播卡方检验和模糊自适应滤波的容错组合导航算法[J].控制理论与应用,2016,33(04):500-511.)提出利用模糊自适应滤波输出的导航信息与双状态递推器进行定期交替校正，从而达到故障检测的目的。状态递推器校正时间间隔的确定非常重要，如果过小，则故障时，经过状态递推器两次重置肯仍未被检测出来；如果过大，会影响全局检测精度，但该方法只是采用经验值，没有具体方法。张华强等人(张华强,李东兴,张国强.混合χ2检测法在组合导航系统故障检测中的应用[J].中国惯性技术学报,2016,24(05):696-700.)在前一种方法的基础上进行了进一步研究，将残差卡方检测方法与双状态卡方检测结合，用残差卡方检测结果决定双状态卡方检测的递推操作。该方法虽然改善了卡方检测的性能，但一方面该方法计算量较大，另一方面残差卡方检测自身对软故障检测精度较低，由其决定双状态卡方检测的更新操作必会引入误差，两个状态递推值偏离实际值，从而对全局精度造成影响。
发明内容
本发明的目的在于提供对故障进行容错处理以提高导航系统稳定性的一种基于广义相对熵的容错滤波方法。
本发明的目的是这样实现的：
本发明一种基于广义相对熵的容错滤波方法，其特征是：
(1)获取SINS系统与卫星导航、地磁匹配导航和重力辅助导航输出的数据；
(2)分别建立SINS/GPS、SINS/地磁匹配和SINS/重力辅助系统的状态空间模型，选择状态量和观测量；
(3)系统初始化；
(4)根据输入子滤波器的k时刻的数据估计k+1时刻状态，求解子系统的状态一步预测Xi,k+1|k和一步预测协方差矩阵Pi,k+1|k，进行时间更新和量测更新；
(5)进行故障参数判断：如果子滤波器对k+1时刻的估计状态经故障参数判断后显示为故障，则关闭此通道，不再向主滤波器输入信息，返回步骤(4)；如果子滤波器的估计状态显示为正常，则正常进行步骤(6)；
(6)利用k时刻的数据对估计的k+1时刻进行信息分配因子设计，并在主滤波器中进行信息融合；
(7)根据主滤波器的输出进行导航参数校正；
(8)进入下一时刻的滤波过程。
所述步骤(2)具体过程为：
建立组合导航系统模型：
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成，对于坐标系而言，选择东-北-天方向的地理坐标系作为导航坐标系，状态方程如下：
Figure GDA0002687761520000021
选取子系统状态变量为：
Figure GDA0002687761520000031
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、εby、εbz表示陀螺东北天的零漂，
Figure GDA0002687761520000032
表示加速度计东北天的偏置；
SINS/GPS量测方程：
在该子系统中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼滤波方法，实时估计出该子系统的状态信息：
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure GDA0002687761520000033
表示观测量；
Figure GDA0002687761520000034
表示量测量矩阵；
Figure GDA0002687761520000035
表示卫星接收机的速度测量白噪声和位置测量白噪声；
SINS/地磁导航系统量测方程：
磁传感器的量测输出可以表示为：
y＝Ba(r)+Bd(r,t)+v
其中，v表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t) 不仅包括地磁场的干扰磁场，还包括外干扰场，外干扰场包括载体自身磁场、太阳日变化、磁暴，外干扰场的影响通过建模补偿等方式消减；
设
Figure GDA0002687761520000036
为根据导航系统指示的位置从地磁图上读取的地磁异常强度， Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure GDA0002687761520000037
的差值作为观测量
Figure GDA0002687761520000038
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r 之间是非线性关系，h2(r)表示地磁异常图上位置r处的地磁场值；
SINS/重力辅助系统量测方程：
利用局部重力异常图与重力传感器量测值来建立量测方程：
重力传感器的输出表示为
y＝Ga(r)+Gd(r,t)+v
其中，v表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括地壳变化、地形起伏等重力干扰场，其影响通过建模补偿等方式消减；
设
Figure GDA0002687761520000041
为根据导航系统指示的位置从重力异常图上读取的重力异常强度， Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure GDA0002687761520000042
的差值作为观测量：
Figure GDA0002687761520000043
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r 之间是非线性关系。
所述步骤(4)中：
Figure GDA0002687761520000044
Figure GDA0002687761520000045
子滤波器时间更新和量测更新：
时间更新：
Figure GDA0002687761520000046
Figure GDA0002687761520000047
量测更新：
Figure GDA0002687761520000048
Figure GDA0002687761520000049
Figure GDA00026877615200000410
所述步骤(5)具体为：
通过马氏距离获得的数据分布均值：
Figure GDA00026877615200000411
Figure GDA00026877615200000412
为信息滤波形式的协方差矩阵，即
Figure GDA0002687761520000051
通过Burg矩阵获得协方差矩阵散度：
Figure GDA0002687761520000052
其中，M表示状态向量的维数；
获取FKLD：
Qi,k表示第i个子系统在第k时刻的预测增益，如下所示：
Figure GDA0002687761520000053
其中，Ki,k表示第i个子系统在第k时刻的卡尔曼增益，Hi,k表示第i个子系统在第k时刻的量测矩阵，Ni表示第i个子系统的量测维数；
Ri,k表示第i个子系统在第k时刻的量测增益，如下所示
Ri,k＝1-Qi,k
Figure GDA0002687761520000054
故障检测阈值
Figure GDA0002687761520000057
Figure GDA0002687761520000055
其中，
Figure GDA0002687761520000056
是第i个滤波器故障检测的阈值，Ni表示第i个子系统的量测维数，ri为第i个子系统的量测噪声矩阵，k表示量测更新频率；
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure GDA0002687761520000061
其中，i＝1、2、3，根据检测向量F来判断组合导航系统的运行状态，如果 F是零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器出现故障。
所述步骤(6)具体为：
信息分配因子：
Figure GDA0002687761520000062
信息分配：
Figure GDA0002687761520000063
主滤波器信息融合：
Figure GDA0002687761520000064
Figure GDA0002687761520000065
本发明的优势在于：
1、本发明设计简单，计算量适中，相较于现有其他方法，更适用于工程应用；
2、本发明解决了传统故障检测方法对软故障检测精度低、实时性差的问题；
3、相较于其他方法，本发明通过设置动态矢量形式的故障检测模块，可以更便捷的处理故障子系统。
附图说明
图1为本发明的流程图；
图2为本发明的飞行器仿真轨迹；
图3为仿真实验位置误差对比曲线图；
图4为仿真实验速度误差对比曲线图；
图5为仿真实验姿态误差对比曲线图。
具体实施方式
下面结合附图举例对本发明做更详细地描述：
结合图1-5，本发明采用的技术方案包括以下步骤：
(1)获取SINS系统、卫星导航、地磁匹配导航和重力辅助导航输出的数据；
(2)分别建立SINS/GPS、SINS/地磁匹配和SINS/重力辅助系统的状态空间模型，选择状态量和观测量；
建立组合导航系统模型
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成。对于坐标系而言，选择东-北-天(ENU)方向的地理坐标系作为导航坐标系。
状态方程如下：
Figure GDA0002687761520000071
选取子系统状态变量为：
Figure GDA0002687761520000072
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、εby、εbz表示陀螺东北天的零漂，
Figure GDA0002687761520000073
表示加速度计东北天的偏置。
SINS/GPS量测方程
在该子系统中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼滤波方法，实时估计出该子系统的状态信息。
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure GDA0002687761520000074
表示观测量；
Figure GDA0002687761520000075
表示量测量矩阵；
Figure GDA0002687761520000076
表示卫星接收机的速度测量白噪声和位置测量白噪声。
SINS/地磁导航系统量测方程
磁传感器的量测输出可以表示为：
y＝Ba(r)+Bd(r,t)+v
其中，v表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t) 不仅包括地磁场的干扰磁场，还包括载体自身磁场、太阳日变化、磁暴等外干扰场。外干扰场的影响可以通过建模补偿等方式消减。
设
Figure GDA0002687761520000081
为根据导航系统指示的位置从地磁图上读取的地磁异常强度， Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure GDA0002687761520000082
的差值作为观测量
Figure GDA0002687761520000083
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r 之间是非线性关系。一般而言，h2(r)表示地磁异常图上位置r处的地磁场值。 SINS/重力辅助系统量测方程
由于重力场是大波长，位置变化引起的重力场强度变化不明显，且重力异常图上细节信息丰富，故利用局部重力异常图与重力传感器量测值来建立量测方程。
重力传感器的输出可以表示为
y＝Ga(r)+Gd(r,t)+v
其中，v表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括地壳变化、地形起伏等重力干扰场，其影响可以通过建模补偿等方式消减。
设
Figure GDA0002687761520000084
为根据导航系统指示的位置从重力异常图上读取的重力异常强度， Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure GDA0002687761520000085
的差值作为观测量
Figure GDA0002687761520000086
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r 之间是非线性关系。
(3)系统初始化；
(4)根据输入子滤波器的k时刻的数据估计k+1时刻状态：
求解子系统的状态一步预测Xi,k+1|k和一步预测协方差矩阵Pi,k+1|k：
Figure GDA0002687761520000091
Figure GDA0002687761520000092
子滤波器时间更新和量测更新：
时间更新
Figure GDA0002687761520000093
Figure GDA0002687761520000094
量测更新
Figure GDA0002687761520000095
Figure GDA0002687761520000096
Figure GDA0002687761520000097
(5)进行故障参数判断：如果子滤波器对k+1时刻的估计状态经故障参数判断后显示为故障，则关闭此通道，不再向主滤波器输入信息，返回步骤(4)；如果子滤波器的估计状态显示为正常，则正常进行步骤(6)；
通过马氏距离获得的数据分布均值：
Figure GDA0002687761520000098
Figure GDA0002687761520000099
为信息滤波形式的协方差矩阵，即
Figure GDA00026877615200000910
通过Burg矩阵获得协方差矩阵散度：
Figure GDA00026877615200000911
其中，M表示状态向量的维数。
FKLD获取：
Qi,k表示第i个子系统在第k时刻的预测增益，如下所示：
Figure GDA00026877615200000912
其中，Ki,k表示第i个子系统在第k时刻的卡尔曼增益，Hi,k表示第i个子系统在第k时刻的量测矩阵，Ni表示第i个子系统的量测维数。
Ri,k表示第i个子系统在第k时刻的量测增益，如下所示
Ri,k＝1-Qi,k
Figure GDA0002687761520000101
故障检测阈值
Figure GDA0002687761520000102
Figure GDA0002687761520000103
其中，
Figure GDA0002687761520000104
是第i个滤波器故障检测的阈值，Ni表示第i个子系统的量测维数，ri为第i个子系统的量测噪声矩阵，k表示量测更新频率。
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure GDA0002687761520000105
其中，i＝1,2,3。根据检测向量F来判断组合导航系统的运行状态，如果F 是零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器出现故障。
(6)利用k时刻的数据对估计的k+1时刻进行信息分配，并在主滤波器中进行信息融合；
信息分配因子设计：
Figure GDA0002687761520000106
信息分配：
Figure GDA0002687761520000111
主滤波器信息融合：
Figure GDA0002687761520000112
Figure GDA0002687761520000113
(7)根据主滤波器的输出进行导航参数校正；
(8)进入下一时刻的滤波过程。
本发明采用联邦滤波算法，针对现有故障检测算法对软故障检测精度低的问题，利用预测信息和更新信息对KL散度和马氏距离的计算结果进行权值分配，将两者的加权和作为传感器故障检测指标。所以本发明利用MATLAB仿真软件进行仿真实验，将本发明与现有的自适应分配原则的传统联邦滤波方法(信息在各子滤波器间通过协方差阵进行自适应分配，检测到故障后隔离故障子系统)进行了对比。
本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。
下面结合具体事例进行仿真分析：
方案I自适应分配原则的传统联邦滤波(信息在各子滤波器间通过协方差阵进行自适应分配，检测到故障后隔离故障子系统)。
方案II本发明提出的故障检测算法。
考虑GPS系统存在故障。假设GPS系统位置输出在400s时刻开始发生缓变故障，故障速率为80m/s，持续时间为100s。误警概率α＝0.01,n＝6，则 SINS/GPS子系统的检测门限为TD＝16.812。
在这种故障条件下，2种方案对故障的处理效果如图3、图4和图5所示。
从图3中可以看出，为应对400s加入的纬度故障数据对整个系统的影响，三种方案均进行了故障检测和故障隔离措施。方案I在第400s时的纬度误差为15.81，在第408.1s达到峰值30.46，变化量为14.65；方案在第400s时的纬度误差为15.84，在第408.1s达到峰值28.15，变化量为12.31。即方案II能更早的检测出软故障，继而更早的关闭故障子系统，避免其他子系统受到更严重的污染。而从图4、图5中也可看出，由于更早的检测出了故障信息，方案III的估计精度更高一些。
表1仿真实验位置误差参数
Tab.1Error parameters of position in simulation experiment
Figure GDA0002687761520000121
表2仿真实验速度误差参数
Tab.2Error parameters of speed in simulation experiment
Figure GDA0002687761520000122
表3仿真实验姿态误差参数
Tab.3Error parameters of attitude in simulation experiment
Figure GDA0002687761520000123
上表为各导航参数误差均值和均方差数据。通过对比数据情况，可以发现，对于软故障情况，FKLD方法优势更明显。
综上，为解决现有故障检测方法计算复杂，对软故障检测精度不高、实时性差的问题，本发明提出了一种基于广义相对熵的容错滤波算法设计方法。仿真结果表明，相比于现有滤波方法，本发明有更好的故障检测精度和更高的运算效率，对进一步提高复杂条件下多源信息融合系统的容错能力具有一定的参考意义。

Claims (3)

1.一种基于广义相对熵的容错滤波方法，其特征是：包括以下步骤
(1)获取SINS系统与卫星导航、地磁匹配导航和重力辅助导航输出的数据；
(2)分别建立SINS/GPS、SINS/地磁匹配和SINS/重力辅助系统的状态空间模型，选择状态量和观测量；
(3)系统初始化；
(4)根据输入子滤波器的k时刻的数据估计k+1时刻状态，求解子系统的状态一步预测
Figure FDA0003888992390000011
和一步预测协方差矩阵
Figure FDA0003888992390000012
进行时间更新和量测更新；
(5)进行故障参数判断：如果子滤波器对k+1时刻的估计状态经故障参数判断后显示为故障，则关闭此通道，不再向主滤波器输入信息，返回步骤(4)；如果子滤波器的估计状态显示为正常，则正常进行步骤(6)；
(6)利用k时刻的数据对估计的k+1时刻进行信息分配因子设计，并在主滤波器中进行信息融合；
(7)根据主滤波器的输出进行导航参数校正；
(8)进入下一时刻的滤波过程；
所述步骤(5)具体为：
通过马氏距离获得的数据分布均值：
Figure FDA0003888992390000013
Figure FDA0003888992390000014
为信息滤波形式的协方差矩阵，即
Figure FDA0003888992390000015
通过Burg矩阵获得协方差矩阵散度：
Figure FDA0003888992390000016
其中，M表示状态向量的维数；
获取FKLD：
Figure FDA0003888992390000021
表示第i个子系统在第k时刻的预测增益，如下所示：
Figure FDA0003888992390000022
其中，
Figure FDA0003888992390000023
表示第i个子系统在第k时刻的卡尔曼增益，
Figure FDA0003888992390000024
表示第i个子系统在第k时刻的量测矩阵，Ni表示第i个子系统的量测维数；
Figure FDA0003888992390000025
表示第i个子系统在第k时刻的量测增益，如下所示
Figure FDA0003888992390000026
Figure FDA0003888992390000027
故障检测阈值
Figure FDA0003888992390000028
Figure FDA0003888992390000029
其中，
Figure FDA00038889923900000210
是第i个滤波器故障检测的阈值，Ni表示第i个子系统的量测维数，ri为第i个子系统的量测噪声矩阵，h表示量测更新频率；
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure FDA00038889923900000211
其中，i＝1、2、3，根据检测向量F来判断组合导航系统的运行状态，如果F是零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器出现故障；
所述步骤(6)具体为：
信息分配因子：
Figure FDA0003888992390000031
信息分配：
Figure FDA0003888992390000032
其中
Figure FDA0003888992390000033
表示主系统的在k时刻的预测增益，
Figure FDA0003888992390000034
表示主系统在k时刻的状态估计协方差矩阵。其中，
Figure FDA0003888992390000035
表示主系统在k+1时刻的状态估计值，
Figure FDA0003888992390000036
表示第i个子系统在k+1时刻的状态估计值；
主滤波器信息融合：
Figure FDA0003888992390000037
Figure FDA0003888992390000038
。
2.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(2)具体过程为：
建立组合导航系统模型：
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成，对于坐标系而言，选择东-北-天方向的地理坐标系作为导航坐标系，状态方程如下：
Figure FDA0003888992390000039
选取子系统状态变量为：
Figure FDA00038889923900000310
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、εby、εbz表示陀螺东北天的零漂，
Figure FDA00038889923900000311
表示加速度计东北天的偏置；
SINS/GPS量测方程：
在该子系统中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼滤波方法，实时估计出该子系统的状态信息：
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure FDA0003888992390000041
表示观测量；
Figure FDA0003888992390000042
表示量测量矩阵；
Figure FDA0003888992390000043
表示卫星接收机的速度测量白噪声和位置测量白噪声；
SINS/地磁导航系统量测方程：
磁传感器的量测输出表示为：
y＝Ba(r)+Bd(r，t)+vc
其中，vc表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t)不仅包括地磁场的干扰磁场，还包括外干扰场，外干扰场包括载体自身磁场、太阳日变化、磁暴，外干扰场的影响通过建模补偿方式消减；
设
Figure FDA0003888992390000044
为根据导航系统指示的位置从地磁图上读取的地磁异常强度，Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure FDA0003888992390000045
的差值作为观测量
Figure FDA0003888992390000046
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r之间是非线性关系，h2(r)表示地磁异常图上位置r处的地磁场值；
SINS/重力辅助系统量测方程：
利用局部重力异常图与重力传感器量测值来建立量测方程：
重力传感器的输出表示为
y＝Ga(r)+Gd(r，t)+vz
其中，vz表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括地壳变化、地形起伏重力干扰场，其影响通过建模补偿方式消减；
设
Figure FDA0003888992390000047
为根据导航系统指示的位置从重力异常图上读取的重力异常强度，Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure FDA0003888992390000048
的差值作为观测量：
Figure FDA0003888992390000051
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r之间是非线性关系。
3.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(4)中，子滤波器时间更新和量测更新：
时间更新：
Figure FDA0003888992390000052
Figure FDA0003888992390000053
量测更新：
Figure FDA0003888992390000054
Figure FDA0003888992390000055
Figure FDA0003888992390000056
CN202010431864.4A 2020-05-20 2020-05-20 一种基于广义相对熵的容错滤波方法 Active CN111854728B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010431864.4A CN111854728B (zh) 2020-05-20 2020-05-20 一种基于广义相对熵的容错滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010431864.4A CN111854728B (zh) 2020-05-20 2020-05-20 一种基于广义相对熵的容错滤波方法

Publications (2)

Publication Number Publication Date
CN111854728A CN111854728A (zh) 2020-10-30
CN111854728B true CN111854728B (zh) 2022-12-13

Family

ID=72985154

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010431864.4A Active CN111854728B (zh) 2020-05-20 2020-05-20 一种基于广义相对熵的容错滤波方法

Country Status (1)

Country Link
CN (1) CN111854728B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN102252677A (zh) * 2011-04-18 2011-11-23 哈尔滨工程大学 一种基于时间序列分析的变比例自适应联邦滤波方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN102506868A (zh) * 2011-11-21 2012-06-20 清华大学 基于联邦滤波的sins/smans/trns组合导航方法及系统
CN104460685A (zh) * 2014-11-21 2015-03-25 南京信息工程大学 一种四旋翼飞行器的控制系统及其控制方法
CN107341817A (zh) * 2017-06-16 2017-11-10 哈尔滨工业大学（威海） 基于在线度量学习的自适应视觉跟踪算法
CN108759922A (zh) * 2018-06-12 2018-11-06 哈尔滨工程大学 基于小型无人车的防化探测系统及其组合导航方法
CN110285815A (zh) * 2019-05-28 2019-09-27 山东航天电子技术研究所 一种可在轨全程应用的微纳卫星多源信息姿态确定方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101319902B (zh) * 2008-07-18 2010-09-08 哈尔滨工程大学 一种低成本组合式定位定向装置及组合定位方法
CN105253330B (zh) * 2015-10-30 2017-04-05 中国空间技术研究院 一种基于优化的信息融合geo卫星控制系统菜单式设计方法
EP3293549B1 (en) * 2016-09-09 2020-03-11 Trimble Inc. Advanced navigation satellite system positioning method and system using delayed precise information

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN102252677A (zh) * 2011-04-18 2011-11-23 哈尔滨工程大学 一种基于时间序列分析的变比例自适应联邦滤波方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN102506868A (zh) * 2011-11-21 2012-06-20 清华大学 基于联邦滤波的sins/smans/trns组合导航方法及系统
CN104460685A (zh) * 2014-11-21 2015-03-25 南京信息工程大学 一种四旋翼飞行器的控制系统及其控制方法
CN107341817A (zh) * 2017-06-16 2017-11-10 哈尔滨工业大学（威海） 基于在线度量学习的自适应视觉跟踪算法
CN108759922A (zh) * 2018-06-12 2018-11-06 哈尔滨工程大学 基于小型无人车的防化探测系统及其组合导航方法
CN110285815A (zh) * 2019-05-28 2019-09-27 山东航天电子技术研究所 一种可在轨全程应用的微纳卫星多源信息姿态确定方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Mechanical fauU diagnosis method based on mahalanobis distance and LMD;Ge Mingtao 等;《2015 IEEE 12th International Conference on Electronic Measurement & Instruments》;20151231;第20-24页 *
四旋翼飞行器导航系统的数据融合方法;王伟 等;《电光与控制》;20190331;第26卷(第3期);第84-88页 *
基于MIMU／GPS／EC的联邦卡尔曼滤波器设计;张樨 等;《测试技术学报》;20121231;第26卷(第4期);第332-336页 *

Also Published As

Publication number Publication date
CN111854728A (zh) 2020-10-30

Similar Documents

Publication Publication Date Title
CN110196443B (zh) 一种飞行器的容错组合导航方法及系统
CN111780755B (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
Brenner Integrated GPS/inertial fault detection availability
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
US9547086B2 (en) Selected aspects of advanced receiver autonomous integrity monitoring application to kalman filter based navigation filter
CN101354253B (zh) 一种基于匹配度的地磁辅助导航方法
CN103323007B (zh) 一种基于时变量测噪声的鲁棒联邦滤波方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN106885576B (zh) 一种基于多点地形匹配定位的auv航迹偏差估计方法
CN111024124B (zh) 一种多传感器信息融合的组合导航故障诊断方法
CN113916225B (zh) 一种基于稳健权因子系数的组合导航粗差抗差估计方法
CN113984054A (zh) 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
CN104613966A (zh) 一种地籍测量事后数据处理方法
Kim et al. Compensation of gyroscope errors and gps/dr integration
CN114252077B (zh) 基于联邦滤波器的双gps/sins的组合导航方法及系统
CN109974695B (zh) 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
CN101545778B (zh) 一种基于非线性滤波的电控罗经初始对准方法
CN112179347B (zh) 一种基于光谱红移误差观测方程的组合导航方法
CN113900069A (zh) 一种基于干涉成像高度计的垂线偏差计算方法及其系统
CN111854728B (zh) 一种基于广义相对熵的容错滤波方法
CN112697154A (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