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

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

Info

Publication number
CN111854728A
CN111854728A CN202010431864.4A CN202010431864A CN111854728A CN 111854728 A CN111854728 A CN 111854728A CN 202010431864 A CN202010431864 A CN 202010431864A CN 111854728 A CN111854728 A CN 111854728A
Authority
CN
China
Prior art keywords
fault
measurement
sins
gravity
geomagnetic
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.)
Granted
Application number
CN202010431864.4A
Other languages
English (en)
Other versions
CN111854728B (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、根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是： 所述步骤(2)具体过程为：
建立组合导航***模型：
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成，对于坐 标系而言，选择东-北-天方向的地理坐标系作为导航坐标系，状态方程如下：
Figure BDA0002500872930000021
选取子***状态变量为：
Figure BDA0002500872930000022
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表 示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、 εby、εbz表示陀螺东北天的零漂，▽x、▽y、▽z表示加速度计东北天的偏置；
SINS/GPS量测方程：
在该子***中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼 滤波方法，实时估计出该子***的状态信息：
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure BDA0002500872930000031
表示观测量；
Figure BDA0002500872930000032
表示量测量矩 阵；
Figure BDA0002500872930000033
表示卫星接收机的速度测量白噪声和位置测 量白噪声；
SINS/地磁导航***量测方程：
磁传感器的量测输出可以表示为：
y＝Ba(r)+Bd(r,t)+v
其中，v表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t) 不仅包括地磁场的干扰磁场，还包括外干扰场，外干扰场包括载体自身磁场、太 阳日变化、磁暴，外干扰场的影响通过建模补偿等方式消减；
设
Figure BDA0002500872930000034
为根据导航***指示的位置从地磁图上读取的地磁异常强度， Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure BDA0002500872930000035
的差值作为观 测量
Figure BDA0002500872930000036
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r 之间是非线性关系，h2(r)表示地磁异常图上位置r处的地磁场值；
SINS/重力辅助***量测方程：
利用局部重力异常图与重力传感器量测值来建立量测方程：
重力传感器的输出表示为
y＝Ga(r)+Gd(r,t)+v
其中，v表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括 地壳变化、地形起伏等重力干扰场，其影响通过建模补偿等方式消减；
设
Figure BDA0002500872930000041
为根据导航***指示的位置从重力异常图上读取的重力异常强度， Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure BDA0002500872930000042
的差值作为观 测量：
Figure BDA0002500872930000043
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r 之间是非线性关系。
3、根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是： 所述步骤(4)中：
Figure BDA0002500872930000044
Figure BDA0002500872930000045
子滤波器时间更新和量测更新：
时间更新：
Figure BDA0002500872930000046
Figure BDA0002500872930000047
量测更新：
Figure BDA0002500872930000048
Figure BDA0002500872930000049
Figure BDA00025008729300000410
4、根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是： 所述步骤(5)具体为：
通过马氏距离获得的数据分布均值：
Figure RE-GDA00026877615200000411
Figure BDA00025008729300000412
为信息滤波形式的协方差矩阵，即
Figure BDA0002500872930000051
通过Burg矩阵获得协方差矩阵散度：
Figure BDA0002500872930000052
其中，M表示状态向量的维数；
获取FKLD：
Qi,k表示第i个子***在第k时刻的预测增益，如下所示：
Figure BDA0002500872930000053
其中，Ki,k表示第i个子***在第k时刻的卡尔曼增益，Hi,k表示第i个子系 统在第k时刻的量测矩阵，Ni表示第i个子***的量测维数；
Ri,k表示第i个子***在第k时刻的量测增益，如下所示
Ri,k＝1-Qi,k
Figure BDA0002500872930000054
故障检测阈值
Figure BDA0002500872930000055
Figure BDA0002500872930000056
其中，
Figure BDA0002500872930000057
是第i个滤波器故障检测的阈值，Ni表示第i个子***的量测维数， ri为第i个子***的量测噪声矩阵，k表示量测更新频率；
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure BDA0002500872930000061
其中，i＝1、2、3，根据检测向量F来判断组合导航***的运行状态，如果F 是零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器 出现故障。
5、根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是： 所述步骤(6)具体为：
信息分配因子：
Figure BDA0002500872930000062
信息分配：
Figure BDA0002500872930000063
主滤波器信息融合：
Figure BDA0002500872930000064
Figure BDA0002500872930000065
本发明的优势在于：
1、本发明设计简单，计算量适中，相较于现有其他方法，更适用于工程应 用；
2、本发明解决了传统故障检测方法对软故障检测精度低、实时性差的问题；
3、相较于其他方法，本发明通过设置动态矢量形式的故障检测模块，可以 更便捷的处理故障子***。
附图说明
图1为本发明的流程图；
图2为本发明的飞行器仿真轨迹；
图3为仿真实验位置误差对比曲线图；
图4为仿真实验速度误差对比曲线图；
图5为仿真实验姿态误差对比曲线图。
具体实施方式
下面结合附图举例对本发明做更详细地描述：
结合图1-5，本发明采用的技术方案包括以下步骤：
(1)获取SINS***、卫星导航、地磁匹配导航和重力辅助导航输出的数 据；
(2)分别建立SINS/GPS、SINS/地磁匹配和SINS/重力辅助***的状态空 间模型，选择状态量和观测量；
建立组合导航***模型
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成。对于坐 标系而言，选择东-北-天(ENU)方向的地理坐标系作为导航坐标系。
状态方程如下：
Figure BDA0002500872930000071
选取子***状态变量为：
Figure BDA0002500872930000072
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表 示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、 εby、εbz表示陀螺东北天的零漂，▽x、▽y、▽z表示加速度计东北天的偏置。
SINS/GPS量测方程
在该子***中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼 滤波方法，实时估计出该子***的状态信息。
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure BDA0002500872930000073
表示观测量；
Figure BDA0002500872930000074
表示量测量矩 阵；
Figure BDA0002500872930000075
表示卫星接收机的速度测量白噪声和位置测 量白噪声。
SINS/地磁导航***量测方程
磁传感器的量测输出可以表示为：
y＝Ba(r)+Bd(r,t)+v
其中，v表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t) 不仅包括地磁场的干扰磁场，还包括载体自身磁场、太阳日变化、磁暴等外干扰 场。外干扰场的影响可以通过建模补偿等方式消减。
设
Figure BDA0002500872930000081
为根据导航***指示的位置从地磁图上读取的地磁异常强度， Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure BDA0002500872930000082
的差值作为观 测量
Figure BDA0002500872930000083
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r 之间是非线性关系。一般而言，h2(r)表示地磁异常图上位置r处的地磁场值。
SINS/重力辅助***量测方程
由于重力场是大波长，位置变化引起的重力场强度变化不明显，且重力异常 图上细节信息丰富，故利用局部重力异常图与重力传感器量测值来建立量测方程。
重力传感器的输出可以表示为
y＝Ga(r)+Gd(r,t)+v
其中，v表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括 地壳变化、地形起伏等重力干扰场，其影响可以通过建模补偿等方式消减。
设
Figure BDA0002500872930000084
为根据导航***指示的位置从重力异常图上读取的重力异常强度， Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure BDA0002500872930000085
的差值作为观 测量
Figure BDA0002500872930000086
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r 之间是非线性关系。
(3)***初始化；
(4)根据输入子滤波器的k时刻的数据估计k+1时刻状态：
求解子***的状态一步预测Xi,k+1|k和一步预测协方差矩阵Pi,k+1|k：
Figure BDA0002500872930000091
Figure BDA0002500872930000092
子滤波器时间更新和量测更新：
时间更新
Figure BDA0002500872930000093
Figure BDA0002500872930000094
量测更新
Figure BDA0002500872930000095
Figure BDA0002500872930000096
Figure BDA0002500872930000097
(5)进行故障参数判断：如果子滤波器对k+1时刻的估计状态经故障参数 判断后显示为故障，则关闭此通道，不再向主滤波器输入信息，返回步骤(4)； 如果子滤波器的估计状态显示为正常，则正常进行步骤(6)；
通过马氏距离获得的数据分布均值：
Figure RE-GDA0002687761520000098
Figure BDA0002500872930000099
为信息滤波形式的协方差矩阵，即
Figure BDA00025008729300000910
通过Burg矩阵获得协方差矩阵散度：
Figure BDA00025008729300000911
其中，M表示状态向量的维数。
FKLD获取：
Qi,k表示第i个子***在第k时刻的预测增益，如下所示：
Figure BDA00025008729300000912
其中，Ki,k表示第i个子***在第k时刻的卡尔曼增益，Hi,k表示第i个子系 统在第k时刻的量测矩阵，Ni表示第i个子***的量测维数。
Ri,k表示第i个子***在第k时刻的量测增益，如下所示
Ri,k＝1-Qi,k
Figure BDA0002500872930000101
故障检测阈值
Figure BDA0002500872930000102
Figure BDA0002500872930000103
其中，
Figure BDA0002500872930000104
是第i个滤波器故障检测的阈值，Ni表示第i个子***的量测维数， ri为第i个子***的量测噪声矩阵，k表示量测更新频率。
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure BDA0002500872930000105
其中，i＝1,2,3。根据检测向量F来判断组合导航***的运行状态，如果F是 零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器出 现故障。
(6)利用k时刻的数据对估计的k+1时刻进行信息分配，并在主滤波器中 进行信息融合；
信息分配因子设计：
Figure BDA0002500872930000106
信息分配：
Figure BDA0002500872930000111
主滤波器信息融合：
Figure BDA0002500872930000112
Figure BDA0002500872930000113
(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.1 Error parameters of position insimulation experiment
Figure BDA0002500872930000121
表2仿真实验速度误差参数 Tab.2 Error parameters of speed in simulationexperiment
Figure BDA0002500872930000122
表3仿真实验姿态误差参数 Tab.3 Error parameters of attitude insimulation experiment
Figure BDA0002500872930000123
上表为各导航参数误差均值和均方差数据。通过对比数据情况，可以发现， 对于软故障情况，FKLD方法优势更明显。
综上，为解决现有故障检测方法计算复杂，对软故障检测精度不高、实时性 差的问题，本发明提出了一种基于广义相对熵的容错滤波算法设计方法。仿真结 果表明，相比于现有滤波方法，本发明有更好的故障检测精度和更高的运算效率， 对进一步提高复杂条件下多源信息融合***的容错能力具有一定的参考意义。

Claims (5)

1.一种基于广义相对熵的容错滤波方法，其特征是：
(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.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(2)具体过程为：
建立组合导航***模型：
选取间接法建立状态方程，状态变量由SINS的导航参数误差构成，对于坐标系而言，选择东-北-天方向的地理坐标系作为导航坐标系，状态方程如下：
Figure FDA0002500872920000011
选取子***状态变量为：
Figure FDA0002500872920000012
其中：φE、φN、φU表示俯仰角、横滚角和航向角误差，δvE、δvN、δvU表示东向、北向和天向速度误差，δL、δλ、δh表示纬度、经度和高度误差，εbx、εby、εbz表示陀螺东北天的零漂，
Figure FDA0002500872920000013
表示加速度计东北天的偏置；
SINS/GPS量测方程：
在该子***中，将SINS与GPS的速度差、位置差作为观测量，通过卡尔曼滤波方法，实时估计出该子***的状态信息：
SINS/GPS的量测方程如下：
z1＝h1(t)X(t)+v1(t)
其中，
Figure FDA0002500872920000021
表示观测量；
Figure FDA0002500872920000022
表示量测量矩阵；
Figure FDA0002500872920000023
表示卫星接收机的速度测量白噪声和位置测量白噪声；
SINS/地磁导航***量测方程：
磁传感器的量测输出可以表示为：
y＝Ba(r)+Bd(r,t)+v
其中，v表示磁传感器量测噪声；Ba(r)表示地磁异常值；干扰磁场Bd(r,t)不仅包括地磁场的干扰磁场，还包括外干扰场，外干扰场包括载体自身磁场、太阳日变化、磁暴，外干扰场的影响通过建模补偿等方式消减；
设
Figure FDA0002500872920000024
为根据导航***指示的位置从地磁图上读取的地磁异常强度，Ba(r)为在载体的实际位置处测得的地磁异常值，取Ba(r)与
Figure FDA0002500872920000025
的差值作为观测量
Figure FDA0002500872920000026
其中，噪声v2包含地磁异常提取噪声和地磁异常图噪声，观测量z2与位置r之间是非线性关系，h2(r)表示地磁异常图上位置r处的地磁场值；
SINS/重力辅助***量测方程：
利用局部重力异常图与重力传感器量测值来建立量测方程：
重力传感器的输出表示为
y＝Ga(r)+Gd(r,t)+v
其中，v表示重力传感器的量测噪声；Ga(r)表示重力异常值；Gd(r,t)包括地壳变化、地形起伏等重力干扰场，其影响通过建模补偿等方式消减；
设
Figure FDA0002500872920000027
为根据导航***指示的位置从重力异常图上读取的重力异常强度，Ga(r)为在载体的实际位置处测得的重力异常值，取Ga(r)与
Figure FDA0002500872920000028
的差值作为观测量：
Figure FDA0002500872920000031
其中，噪声v3包含重力异常提取噪声和重力异常图噪声，观测量z3与位置r之间是非线性关系。
3.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(4)中：
Figure FDA0002500872920000032
Figure FDA0002500872920000033
子滤波器时间更新和量测更新：
时间更新：
Figure FDA0002500872920000034
Figure FDA0002500872920000035
量测更新：
Figure FDA0002500872920000036
Figure FDA0002500872920000037
Figure FDA0002500872920000038
4.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(5)具体为：
通过马氏距离获得的数据分布均值：
Figure DEST_PATH_FDA0002687761510000039
Figure FDA00025008729200000310
为信息滤波形式的协方差矩阵，即
Figure FDA00025008729200000311
通过Burg矩阵获得协方差矩阵散度：
Figure FDA00025008729200000312
其中，M表示状态向量的维数；
获取FKLD：
Qi,k表示第i个子***在第k时刻的预测增益，如下所示：
Figure FDA0002500872920000041
其中，Ki,k表示第i个子***在第k时刻的卡尔曼增益，Hi,k表示第i个子***在第k时刻的量测矩阵，Ni表示第i个子***的量测维数；
Ri,k表示第i个子***在第k时刻的量测增益，如下所示
Ri,k＝1-Qi,k
Figure FDA0002500872920000042
故障检测阈值
Figure FDA0002500872920000043
Figure FDA0002500872920000044
其中，
Figure FDA0002500872920000045
是第i个滤波器故障检测的阈值，Ni表示第i个子***的量测维数，ri为第i个子***的量测噪声矩阵，k表示量测更新频率；
故障检测向量F：
令检测向量F如下所示：
F＝[f1,f2,f3]
检测标准为：
Figure FDA0002500872920000046
其中，i＝1、2、3，根据检测向量F来判断组合导航***的运行状态，如果F是零向量，则所有量测状态良好；如果F是非零向量，则非零元素对应的滤波器出现故障。
5.根据权利要求1所述的一种基于广义相对熵的容错滤波方法，其特征是：所述步骤(6)具体为：
信息分配因子：
Figure FDA0002500872920000051
信息分配：
Figure FDA0002500872920000052
主滤波器信息融合：
Figure FDA0002500872920000053
Figure FDA0002500872920000054
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 true CN111854728A (zh) 2020-10-30
CN111854728B 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 (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101319902A (zh) * 2008-07-18 2008-12-10 哈尔滨工程大学 一种低成本组合式定位定向装置及组合定位方法
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 哈尔滨工业大学（威海） 基于在线度量学习的自适应视觉跟踪算法
EP3293549A1 (en) * 2016-09-09 2018-03-14 Trimble Inc. Advanced navigation satellite system positioning method and system using delayed precise information
US20180281991A1 (en) * 2015-10-30 2018-10-04 China Academy Of Space Technology Menu-type design method for geo satellite control system based on optimized information integration
CN108759922A (zh) * 2018-06-12 2018-11-06 哈尔滨工程大学 基于小型无人车的防化探测***及其组合导航方法
CN110285815A (zh) * 2019-05-28 2019-09-27 山东航天电子技术研究所 一种可在轨全程应用的微纳卫星多源信息姿态确定方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101319902A (zh) * 2008-07-18 2008-12-10 哈尔滨工程大学 一种低成本组合式定位定向装置及组合定位方法
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 南京信息工程大学 一种四旋翼飞行器的控制***及其控制方法
US20180281991A1 (en) * 2015-10-30 2018-10-04 China Academy Of Space Technology Menu-type design method for geo satellite control system based on optimized information integration
EP3293549A1 (en) * 2016-09-09 2018-03-14 Trimble Inc. Advanced navigation satellite system positioning method and system using delayed precise information
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
GE MINGTAO 等: "Mechanical fauU diagnosis method based on mahalanobis distance and LMD", 《2015 IEEE 12TH INTERNATIONAL CONFERENCE ON ELECTRONIC MEASUREMENT & INSTRUMENTS》 *
张樨 等: "基于MIMU／GPS／EC的联邦卡尔曼滤波器设计", 《测试技术学报》 *
王伟 等: "四旋翼飞行器导航***的数据融合方法", 《电光与控制》 *

Also Published As

Publication number Publication date
CN111854728B (zh) 2022-12-13

Similar Documents

Publication Publication Date Title
CN109781099B (zh) 一种自适应ukf算法的导航方法及***
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
Brenner Integrated GPS/inertial fault detection availability
CN110779518B (zh) 一种具有全局收敛性的水下航行器单信标定位方法
CN103323007B (zh) 一种基于时变量测噪声的鲁棒联邦滤波方法
CN112525218B (zh) 一种ins/dvl组合导航***鲁棒智能协同校准方法
CN106885576B (zh) 一种基于多点地形匹配定位的auv航迹偏差估计方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN106679693A (zh) 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN110231636B (zh) Gps与bds双模卫星导航***的自适应无迹卡尔曼滤波方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110954132B (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN110779519B (zh) 一种具有全局收敛性的水下航行器单信标定位方法
CN111024124B (zh) 一种多传感器信息融合的组合导航故障诊断方法
CN113916225B (zh) 一种基于稳健权因子系数的组合导航粗差抗差估计方法
CN113984054A (zh) 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
CN110555398A (zh) 一种基于滤波最优平滑确定故障首达时刻的故障诊断方法
CN104613966A (zh) 一种地籍测量事后数据处理方法
CN114252077B (zh) 基于联邦滤波器的双gps/sins的组合导航方法及***
CN112179347B (zh) 一种基于光谱红移误差观测方程的组合导航方法
CN113900069A (zh) 一种基于干涉成像高度计的垂线偏差计算方法及其***
CN111854728B (zh) 一种基于广义相对熵的容错滤波方法
CN115468559A (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