CN109373999B - 基于故障容错卡尔曼滤波的组合导航方法 - Google Patents

基于故障容错卡尔曼滤波的组合导航方法 Download PDF

Info

Publication number
CN109373999B
CN109373999B CN201811236937.3A CN201811236937A CN109373999B CN 109373999 B CN109373999 B CN 109373999B CN 201811236937 A CN201811236937 A CN 201811236937A CN 109373999 B CN109373999 B CN 109373999B
Authority
CN
China
Prior art keywords
fault
filter
state
error
kalman
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
CN201811236937.3A
Other languages
English (en)
Other versions
CN109373999A (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 CN201811236937.3A priority Critical patent/CN109373999B/zh
Publication of CN109373999A publication Critical patent/CN109373999A/zh
Application granted granted Critical
Publication of CN109373999B publication Critical patent/CN109373999B/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/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
    • 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
    • G01S19/49—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 whereby the further system is an inertial position system, e.g. loosely-coupled

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子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。

Description

基于故障容错卡尔曼滤波的组合导航方法
技术领域
本发明属于组合导航技术领域，具体涉及一种基于故障容错卡尔曼滤波的组合导航方法。
背景技术
现代科学技术已经能够给航行体提供了多种导航设备，如惯性导航系统(INS)，多普勒(Doppler)雷达导航系统，全球定位系统(GPS)。这些导航设备各有优缺点，精度和成本也大不相同。其中惯性导航系统是一种不依赖外部信息，不向外辐射能量的自主式导航系统，而且能提供多种导航参数(位置，速度，姿态)。作为惯性导航系统的一种——捷联式惯性导航系统(SINS)正在得到越来越广泛地应用。将惯性导航系统与其他导航系统适当地组合起来，可取长补短，大大提高导航的精度，也可以大大降低惯导系统的成本。所以，组合导航系统已经成为一种趋势。
利用卡尔曼滤波技术对组合导航系统进行最优组合，一种是集中式卡尔曼滤波，另一种是分散化卡尔曼滤波(即联邦卡尔曼滤波)，由于集中式卡尔曼滤波器的状态维数较高，计算负担重，不利于滤波的实时运行，而且容错性能很差。联邦卡尔曼滤波由于设计的灵活性，计算量小，容错性能好，而受到广泛的应用。
传统的联邦卡尔曼容错系统，是在子滤波器往主滤波器输出信息时进行一次故障检测，一旦有故障则对其进行隔离，无故障时再送往主滤波器进行信息融合，得到最佳的导航估计。随着多种导航系统的出现，以及人们对导航精度要求不断提高，提高卡尔曼组合导航系统的容错性能已成为组合导航技术领域主要的研究课题之一。
关于故障容错与联邦卡尔曼的文献有很多，其中张海龙等的《联邦卡尔曼滤波在捷联惯导/全球定位/里程仪组合导航系统中的优化设计》(探测与控制学报，2009)，王鹏等的《惯导系统/全球定位系统/多普勒计程仪的船舶组合导航滤波算法研究》(科学技术与工程，2014)，上述两篇文献讲述了联邦卡尔曼滤波的原理及其特点，并进行了仿真设计，证明了联邦卡尔曼滤波器在不组合导航系统应用中的可行性和优越性，但是系统采用的是联邦卡尔曼的一般结构，其抗干扰能力较弱，容错性能有待改善。本发明是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的SINS/GPS子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。
发明内容
本发明的目的在于提供一种基于故障容错卡尔曼滤波的组合导航方法。
本发明的目的是这样实现的：
基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：
步骤1：选择
Figure BDA0001838408340000021
作为状态变量，
其中：
Figure BDA0001838408340000022
δλ：惯性导航系统纬度和经度误差；
δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；
εxεyεz：陀螺一阶马尔可夫过程随机误差；
αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；
然后根据系统特性，列写出系统的状态方程：
Figure BDA0001838408340000023
SINS/GPS子系统选择位置和速度作为量测量，
即
Figure BDA0001838408340000024
式中：
Figure BDA0001838408340000025
λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；
Figure BDA0001838408340000026
λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度；
SINS/Doppler子系统选择选择速度作为量测量，
即
Figure BDA0001838408340000027
式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度；
分别进行离散化处理得到：
Figure BDA0001838408340000028
式中：Φi k/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；
步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测
Figure BDA0001838408340000029
和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值
Figure BDA0001838408340000031
和方差的估计值Pk；具体过程如下：
状态一步预测方程：
Figure BDA0001838408340000032
状态估值计算方程：
Figure BDA0001838408340000033
滤波增益方程：
Figure BDA0001838408340000034
一步预测均方差方程：
Figure BDA0001838408340000035
估计均方差方程：
Figure BDA0001838408340000036
步骤3：构建故障检测函数进行检测：
定义故障检测函数
Figure BDA0001838408340000037
其中：rk为残差，即
Figure BDA0001838408340000038
E{rk}＝μ,E{[rk-μ][rk-μ]T}＝Ak；
λk是服从自由度为m的χ2分布，即λk～χ2(m)；m为量测Zk的维数；
故障判定准则为：
Figure BDA0001838408340000039
其中，TD为预先设置的门限值，在给定误警率Pf时，由χn分布来确定；
步骤4：构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ，γ是反映滤波稳健程度的参数，取值为2.5，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计；
对于
Figure BDA00018384083400000310
其中，Tk为满秩阵，Yk是状态变量Xk的一种线性组合；
进行如下递推估计：
Figure BDA0001838408340000041
Figure BDA0001838408340000042
Figure BDA0001838408340000043
Figure BDA0001838408340000044
其中：
Figure BDA0001838408340000045
步骤5：将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：
Figure BDA0001838408340000046
Figure BDA0001838408340000047
其中：
Figure BDA0001838408340000048
分别为子滤波器1、子滤波器2的状态估计；
P1，P2分别为子滤波器1、子滤波器2的方差估计；
Figure BDA0001838408340000049
为全局状态估计，Pg为全局状态估计协方差阵；
步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。
与现有技术相比，本发明的有益效果在于：
本发明是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的SINS/GPS子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。
附图说明
图1为本发明的系统结构图；
图2为有重置的联邦卡尔曼滤波器的一般结构；
图3为无重置的联邦卡尔曼滤波器的一般结构。
具体实施方式
下面结合附图对本发明作出详细说明：
具体实施例一：
本发明的目的在于提供基于故障容错卡尔曼滤波的组合导航方法。针对的是SINS/GPS/Doppler多组合导航系统，该系统的结构图如图1所示。其中图2为有重置的联邦卡尔曼滤波器结构和图3为无重置的联邦卡尔曼滤波器结构。本发明是在无重置的联邦卡尔曼滤波器结构上进行优化设计，针对其中的一个子滤波器增加了一个容错卡尔曼滤波器和一个故障检测函数。
本发明采用的技术方案包括下列步骤：
步骤1、选择
Figure BDA0001838408340000051
作为状态变量，
其中：
Figure BDA0001838408340000052
δλ：惯性导航系统维度和经度误差；
δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；
εxεyεz：陀螺一阶马尔可夫过程随机误差；
αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；
然后根据系统特性，列写出系统的状态方程：
Figure BDA0001838408340000053
SINS/GPS子系统选择位置和速度作为量测量，
即
Figure BDA0001838408340000054
式中：
Figure BDA0001838408340000055
λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；
Figure BDA0001838408340000056
λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度。
SINS/Doppler子系统选择选择速度作为量测量，
即
Figure BDA0001838408340000057
式中：VIX，VIY分别为惯导解算的东向速度，北向速度；
VdX，VdY，分别为多普勒测得的东向速度，北向速度。
分别进行离散化处理得到
Figure BDA0001838408340000058
式中：Φi k/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k为第i个子系统的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵。
步骤2、构建常规卡尔曼滤波器进行状态估计。常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测
Figure BDA0001838408340000061
和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值
Figure BDA0001838408340000062
和方差的估计值Pk。具体过程如下：
状态一步预测方程：
Figure BDA0001838408340000063
状态估值计算方程：
Figure BDA0001838408340000064
滤波增益方程：
Figure BDA0001838408340000065
一步预测均方差方程：
Figure BDA0001838408340000066
估计均方差方程：
Figure BDA0001838408340000067
步骤3、构建构建故障检测函数进行检测：
采用残差χ2检验法，定义滤波器的残差为：
Figure BDA0001838408340000068
式中的预报值为
Figure BDA0001838408340000069
通过对残差rk的均值的检验就可以确定系统是否发生了故障。
对rk作以下二元假设：
H0，无故障时：
Figure BDA00018384083400000610
H0，有故障时：E{rk}＝μ,E{[rk-μ][rk-μ]T}＝Ak
定义故障检测函数：
Figure BDA00018384083400000611
式中，λk是服从自由度为m的χ2分布，即λk～χ2(m)。m为量测Zk的维数。
故障判定准则为：
Figure BDA00018384083400000612
其中，TD为预先设置的门限值，可由误警率Pf确定(当限定误警率Pf＝α时，由Pf＝P[λk＞TD/H0]＝α即可求出TD)
步骤4、中构建容错卡尔曼滤波器进行状态估计。通过步骤3对常规卡尔曼滤波的SINS/Doppler的估计进行检测，若出现故障则回到故障点，进入容错卡尔曼滤波器进行状态估计。下面构建容错卡尔曼滤波器，根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。
对于离散系统模型：
Figure BDA0001838408340000071
其中，Tk为满秩阵，Yk是状态变量Xk的一种线性组合。
设Tk(Ff)表示将输入序列
Figure BDA0001838408340000072
映射至滤波误差序列{e(k)}的传递函数。定义Tk(Ff)的H∞范数为：
Figure BDA0001838408340000073
其中e(k)为滤波误差，即
Figure BDA0001838408340000074
给定实数γ＞0，使得||Tk(Ff)||∞＜γ。并将γ代入以下递推公式进行状态估计：
Figure BDA0001838408340000075
Figure BDA0001838408340000076
Figure BDA0001838408340000077
Figure BDA0001838408340000078
其中：
Figure BDA0001838408340000079
式中，γ根据实际情况取值。
步骤5、在各个子系统的状态估计确认无故障后，将子滤波器状态的局部估计值及其协方差阵送入主滤波器进行信息融合，得到全局状态估计。如图2所示，是有重置联邦卡尔曼的一般结构。联邦卡尔曼滤波器由六种结构，不同的结构以及信息分配系数的确定对联邦滤波器的特性(容错性，最优性，计算量等)会产生不同的影响。本发明采用一种没有重置且βm＝0,βi＝1/N的结构如图3所示，这种结构不需将主滤波器的全局估计值
Figure BDA00018384083400000710
及其协方差Pg反馈到子滤波器，由于各子滤波器不会互相影响，可以提高系统的容错性，
Figure BDA00018384083400000711
Figure BDA00018384083400000712
其中：
Figure BDA00018384083400000713
分别为子滤波器1、子滤波器2的状态估计；
P1，P2分别为子滤波器1、子滤波器2的方差估计；
Figure BDA0001838408340000081
为全局状态估计，Pg为全局状态估计协方差阵。
步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼滤波系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。
具体实施例二：
本发明的目的在于提供一种基于故障容错卡尔曼滤波的组合导航方法。针对的是SINS/GPS/Doppler多组合导航系统，本发明采用的技术方案包括下列步骤：
步骤1、选择
Figure BDA0001838408340000082
作为状态变量，
其中：
Figure BDA0001838408340000083
δλ：惯性导航系统纬度和经度误差；
δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；
εxεyεz：陀螺一阶马尔可夫过程随机误差；
αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；
然后根据系统特性，列写出系统的状态方程：
Figure BDA0001838408340000084
SINS/GPS子系统选择位置和速度作为量测量，
即
Figure BDA0001838408340000085
式中：
Figure BDA0001838408340000086
λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；
Figure BDA0001838408340000087
λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度。
SINS/Doppler子系统选择选择速度作为量测量，
即
Figure BDA0001838408340000088
式中：VIX，VIY分别为惯导解算的东向速度，北向速度；
VdX，VdY，分别为多普勒测得的东向速度，北向速度。
分别进行离散化处理得到：
Figure BDA0001838408340000091
式中：Φi k/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵。
步骤2、构建常规卡尔曼滤波器进行状态估计。常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测
Figure BDA0001838408340000092
和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值
Figure BDA0001838408340000093
和方差的估计值Pk。具体过程如下：
状态一步预测方程：
Figure BDA0001838408340000094
状态估值计算方程：
Figure BDA0001838408340000095
滤波增益方程：
Figure BDA0001838408340000096
一步预测均方差方程：
Figure BDA0001838408340000097
估计均方差方程：
Figure BDA0001838408340000098
步骤3、中构建构建故障检测函数进行检测：
定义故障检测函数
Figure BDA0001838408340000099
其中：rk为残差，即
Figure BDA00018384083400000910
E{rk}＝μ,E{[rk-μ][rk-μ]T}＝Ak。
λk是服从自由度为m的χ2分布，即λk～χ2(m)。m为量测Zk的维数。
故障判定准则为：
Figure BDA00018384083400000911
其中，TD为预先设置的门限值，在给定误警率Pf时，可由χn分布来确定。
步骤4、构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数，可以取值为2.5)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。
对于
Figure BDA0001838408340000101
其中，Tk为满秩阵，Yk是状态变量Xk的一种线性组合。
进行如下递推估计：
Figure BDA0001838408340000102
Figure BDA0001838408340000103
Figure BDA0001838408340000104
Figure BDA0001838408340000105
其中：
Figure BDA0001838408340000106
步骤5、将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：
Figure BDA0001838408340000107
Figure BDA0001838408340000108
其中：
Figure BDA0001838408340000109
分别为子滤波器1、子滤波器2的状态估计；
P1，P2分别为子滤波器1、子滤波器2的方差估计；
Figure BDA00018384083400001010
为全局状态估计，Pg为全局状态估计协方差阵。
步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。
具体实施例三：
本发明公开了一种基于故障容错卡尔曼滤波的组合导航方法。该方法针对的是针对的是惯性/卫星/多普勒组合导航系统，该系统由两个子滤波器和一个主滤波器组成。本发明对其中的惯性/卫星导航子系统进行容错设计，增加了一个容错卡尔曼滤波器，以提高其抗干扰能力。在系统状态模型，先验信息均已知的情况下，优先采用常规卡尔曼滤波器进行状态估计，然后进行一次故障检测，无故障时，直接送往主滤波器进行数据融合；若出现故障，则采用容错卡尔曼滤波器对该子滤波器进行替换，回到故障点重新进行状态估计。输出的数据再进行一次故障检测，无故障时，再送往主滤波器进行数据融合。有故障时，直接将该子系统隔离掉，从而降低了系统误判的风险，提高了对导航子系统数据的利用程度，从而提高了组合导航系统的鲁棒性。
基于故障容错卡尔曼滤波的组合导航方法，其特征在于：
步骤1、选择组合导航状态量列写状态方程，选择量测量列写量测方程，并进行离散化处理；
步骤2、构建常规卡尔曼滤波器进行状态估计；
步骤3、构建故障检测函数进行故障检测；
步骤4、构建容错卡尔曼滤波器进行状态估计；
步骤5、将各子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计；
步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数。
步骤1中选择
Figure BDA0001838408340000111
作为参考系统的状态变量，
其中：
Figure BDA0001838408340000112
δλ：惯性导航系统纬度和经度误差；
δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；
εxεyεz：陀螺一阶马尔可夫过程随机误差；
αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；
然后根据系统特性，列写出导航系统的状态方程。
SINS/GPS子滤波器选择位置和速度作为量测量，
即
Figure BDA0001838408340000113
式中：
Figure BDA0001838408340000114
λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；
Figure BDA0001838408340000115
λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度。
SINS/Doppler子滤波器选择选择速度作为量测量，
即
Figure BDA0001838408340000116
式中：VIX，VIY分别为惯导解算的东向速度，北向速度；
VdX，VdY，分别为多普勒测得的东向速度，北向速度。
然后把系统的状态方程和量测方程进行离散化处理，得到系统的状态空间模型。
步骤2中构建常规卡尔曼滤波器进行状态估计：卡尔曼滤波分为两个过程，时间更新和测量更新。利用时间更新求出状态的一步预测
Figure BDA0001838408340000121
和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值
Figure BDA0001838408340000122
和方差的估计值Pk。
步骤3中构建构建故障检测函数进行检测：
定义故障检测函数：
Figure BDA0001838408340000123
其中：rk为残差，Ak为残差rk的方差。
λk是服从自由度为m的χ2分布，即λk～χ2(m)。m为量测Zk的维数。
故障判定准则为：
Figure BDA0001838408340000124
其中，TD为预先设置的门限值，在给定误警率Pf时，可由χn分布来确定。
步骤4中构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数，可以取值为2.5)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。
步骤5中将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：
Figure BDA0001838408340000125
Figure BDA0001838408340000126
其中：
Figure BDA0001838408340000127
分别为子滤波器1、子滤波器2的状态估计；
P1，P2分别为子滤波器1、子滤波器2的方差估计；
Figure BDA0001838408340000128
为全局状态估计，Pg为全局状态估计协方差阵。
步骤6中将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。

Claims (1)

1.基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：
步骤1：选择
Figure FDA0001838408330000019
作为状态变量，
其中：
Figure FDA00018384083300000110
δλ：惯性导航系统纬度和经度误差；
δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；
εx εy εz：陀螺一阶马尔可夫过程随机误差；
α β λ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；
然后根据系统特性，列写出系统的状态方程：
Figure FDA0001838408330000011
SINS/GPS子系统选择位置和速度作为量测量，
即
Figure FDA0001838408330000012
式中：
Figure FDA0001838408330000013
λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；
Figure FDA0001838408330000014
λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度；
SINS/Doppler子系统选择选择速度作为量测量，
即
Figure FDA0001838408330000015
式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度；
分别进行离散化处理得到：
Figure FDA0001838408330000016
式中：Φi k/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；
步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测
Figure FDA0001838408330000017
和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值
Figure FDA0001838408330000018
和方差的估计值Pk；具体过程如下：
状态一步预测方程：
Figure FDA0001838408330000021
状态估值计算方程：
Figure FDA0001838408330000022
滤波增益方程：
Figure FDA0001838408330000023
一步预测均方差方程：
Figure FDA0001838408330000024
估计均方差方程：
Figure FDA0001838408330000025
步骤3：构建故障检测函数进行检测：
定义故障检测函数
Figure FDA0001838408330000026
其中：rk为残差，即
Figure FDA0001838408330000027
E{rk}＝μ,E{[rk-μ][rk-μ]T}＝Ak；
λk是服从自由度为m的χ2分布，即λk～χ2(m)；m为量测Zk的维数；
故障判定准则为：
Figure FDA0001838408330000028
其中，TD为预先设置的门限值，在给定误警率Pf时，由χn分布来确定；
步骤4：构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ，γ是反映滤波稳健程度的参数，取值为2.5，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计；
对于
Figure FDA0001838408330000029
其中，Tk为满秩阵，Yk是状态变量Xk的一种线性组合；
进行如下递推估计：
Figure FDA00018384083300000210
Figure FDA00018384083300000211
Figure FDA00018384083300000212
Figure FDA00018384083300000213
其中：
Figure FDA0001838408330000031
步骤5：将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：
Figure FDA0001838408330000032
Figure FDA0001838408330000033
其中：
Figure FDA0001838408330000034
分别为子滤波器1、子滤波器2的状态估计；
P1，P2分别为子滤波器1、子滤波器2的方差估计；
Figure FDA0001838408330000035
为全局状态估计，Pg为全局状态估计协方差阵；
步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。
CN201811236937.3A 2018-10-23 2018-10-23 基于故障容错卡尔曼滤波的组合导航方法 Active CN109373999B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811236937.3A CN109373999B (zh) 2018-10-23 2018-10-23 基于故障容错卡尔曼滤波的组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811236937.3A CN109373999B (zh) 2018-10-23 2018-10-23 基于故障容错卡尔曼滤波的组合导航方法

Publications (2)

Publication Number Publication Date
CN109373999A CN109373999A (zh) 2019-02-22
CN109373999B true CN109373999B (zh) 2020-11-03

Family

ID=65400789

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811236937.3A Active CN109373999B (zh) 2018-10-23 2018-10-23 基于故障容错卡尔曼滤波的组合导航方法

Country Status (1)

Country Link
CN (1) CN109373999B (zh)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109813342B (zh) * 2019-02-28 2020-02-21 北京讯腾智慧科技股份有限公司 一种惯导-卫星组合导航系统的故障检测方法和系统
CN109813299A (zh) * 2019-03-06 2019-05-28 南京理工大学 一种基于交互多模型的组合导航信息融合方法
CN109813309A (zh) * 2019-03-08 2019-05-28 哈尔滨工程大学 一种六陀螺冗余式捷联惯导系统双故障隔离方法
CN110196067B (zh) * 2019-05-13 2023-07-07 南京航空航天大学 一种基于无参考系统的交互式故障检测方法
CN110154669B (zh) * 2019-05-19 2022-01-25 浙江大学 一种基于多源信息融合的ecas车架高度扰动消除方法
CN110208843B (zh) * 2019-05-21 2022-07-22 南京航空航天大学 一种基于增广伪距信息辅助的容错导航方法
CN110196049A (zh) * 2019-05-28 2019-09-03 哈尔滨工程大学 一种动态环境下四陀螺冗余式捷联惯导系统硬故障检测与隔离方法
CN110296701B (zh) * 2019-07-09 2022-12-13 哈尔滨工程大学 惯性与卫星组合导航系统渐变型故障回溯容错方法
CN110579740B (zh) * 2019-09-17 2023-03-31 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110823217B (zh) * 2019-11-21 2023-08-22 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111142143A (zh) * 2019-12-23 2020-05-12 西北工业大学 一种基于多源信息融合的进场段飞行技术误差估计方法
CN111024124B (zh) * 2019-12-25 2023-11-07 南京航空航天大学 一种多传感器信息融合的组合导航故障诊断方法
CN111044051B (zh) * 2019-12-30 2023-11-03 星际智航(太仓)航空科技有限公司 一种复合翼无人机容错组合导航方法
CN111829508B (zh) * 2020-07-24 2022-02-08 中国人民解放军火箭军工程大学 一种基于新息的容错联邦滤波方法及系统
CN112212860B (zh) * 2020-08-28 2023-03-03 山东航天电子技术研究所 具有故障容错的分布式滤波微纳卫星姿态确定方法
CN111999747B (zh) * 2020-08-28 2023-06-20 大连海事大学 一种惯导-卫星组合导航系统的鲁棒故障检测方法
CN113326616A (zh) * 2021-05-31 2021-08-31 上海航天测控通信研究所 抗慢变量测粗差的容错卡尔曼滤波方法
CN113932803B (zh) * 2021-08-31 2023-10-20 惠州学院 适用于高动态飞行器的惯性/地磁/卫星组合导航系统
CN116243610B (zh) * 2023-05-12 2023-08-01 青岛大学 一种数据驱动车辆队列容错跟踪控制追踪方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101865693A (zh) * 2010-06-03 2010-10-20 天津职业技术师范大学 航空用多传感器组合导航系统
CN105758401A (zh) * 2016-05-14 2016-07-13 中卫物联成都科技有限公司 一种基于多源信息融合的组合导航方法和设备

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101865693A (zh) * 2010-06-03 2010-10-20 天津职业技术师范大学 航空用多传感器组合导航系统
CN105758401A (zh) * 2016-05-14 2016-07-13 中卫物联成都科技有限公司 一种基于多源信息融合的组合导航方法和设备

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Fault-Tolerant Integrated Navigation Algorithm of the Federal Kalman Filter;Li Jing etal.;《Advances in Future Computer and Control Systems》;20121231;第160卷;第621-627页 *
一种容错联邦滤波算法在INS/GPS/Doppler组合导航系统中的应用;张崇猛等;《中国惯性技术学报》;19991231;第7卷(第4期);第1-5页 *
改进故障隔离的容错联邦滤波;熊智等;《航空学报》;20150325;第36卷(第3期);第929-938页 *

Also Published As

Publication number Publication date
CN109373999A (zh) 2019-02-22

Similar Documents

Publication Publication Date Title
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111780755B (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN110579740B (zh) 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN110095800B (zh) 一种多源融合的自适应容错联邦滤波组合导航方法
US7058505B1 (en) System for navigation redundancy
EP2927640B1 (en) Global positioning system (gps) self-calibrating lever arm function
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
US20140267686A1 (en) System and method for augmenting a gnss/ins navigation system of a low dynamic vessel using a vision system
CN113792488A (zh) 一种双阈值辅助故障容错kf的组合导航系统及方法
CN105547300A (zh) 用于auv的全源导航系统及方法
CN114252077B (zh) 基于联邦滤波器的双gps/sins的组合导航方法及系统
CN109974695B (zh) 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
CN117606491B (zh) 一种自主式水下航行器的组合定位导航方法及装置
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
Romanovas et al. A method for IMU/GNSS/Doppler Velocity Log integration in marine applications
US8566055B1 (en) Gyro indexing compensation method and system
US20230341563A1 (en) System and method for computing positioning protection levels
Lou et al. An approach to improving attitude estimation using sensor fusion for robot navigation
US11821733B2 (en) Terrain referenced navigation system with generic terrain sensors for correcting an inertial navigation solution
CN111649744A (zh) 一种基于动力学模型辅助的组合导航定位方法
RU2783480C1 (ru) Автоматизированная система навигации с контролем аномальных измерений координат от спутниковых радионавигационных систем
US20220358365A1 (en) Tightly coupled end-to-end multi-sensor fusion with integrated compensation
CN117739971A (zh) 一种鲁棒sinsusbl组合导航方法、装置及系统

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