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

本发明专利技术公开了一种基于故障容错卡尔曼滤波的组合导航方法，属于组合导航技术领域。本发明专利技术包括：选择组合导航状态量列写状态方程，选择量测量列写量测方程，并进行离散化处理；构建常规卡尔曼滤波器进行状态估计；构建故障检测函数进行故障检测；构建容错卡尔曼滤波器进行状态估计；将各子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计；将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数。本发明专利技术针对因出现故障而隔离的SINS/GPS子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。

【技术实现步骤摘要】
基于故障容错卡尔曼滤波的组合导航方法
本专利技术属于组合导航
，具体涉及一种基于故障容错卡尔曼滤波的组合导航方法。
技术介绍
现代科学技术已经能够给航行体提供了多种导航设备，如惯性导航系统(INS)，多普勒(Doppler)雷达导航系统，全球定位系统(GPS)。这些导航设备各有优缺点，精度和成本也大不相同。其中惯性导航系统是一种不依赖外部信息，不向外辐射能量的自主式导航系统，而且能提供多种导航参数(位置，速度，姿态)。作为惯性导航系统的一种——捷联式惯性导航系统(SINS)正在得到越来越广泛地应用。将惯性导航系统与其他导航系统适当地组合起来，可取长补短，大大提高导航的精度，也可以大大降低惯导系统的成本。所以，组合导航系统已经成为一种趋势。利用卡尔曼滤波技术对组合导航系统进行最优组合，一种是集中式卡尔曼滤波，另一种是分散化卡尔曼滤波(即联邦卡尔曼滤波)，由于集中式卡尔曼滤波器的状态维数较高，计算负担重，不利于滤波的实时运行，而且容错性能很差。联邦卡尔曼滤波由于设计的灵活性，计算量小，容错性能好，而受到广泛的应用。传统的联邦卡尔曼容错系统，是在子滤波器往主滤波器输出信息时进行一次故障检测，一旦有故障则对其进行隔离，无故障时再送往主滤波器进行信息融合，得到最佳的导航估计。随着多种导航系统的出现，以及人们对导航精度要求不断提高，提高卡尔曼组合导航系统的容错性能已成为组合导航
主要的研究课题之一。关于故障容错与联邦卡尔曼的文献有很多，其中张海龙等的《联邦卡尔曼滤波在捷联惯导/全球定位/里程仪组合导航系统中的优化设计》(探测与控制学报，2009)，王鹏等的《惯导系统/全球定位系统/多普勒计程仪的船舶组合导航滤波算法研究》(科学技术与工程，2014)，上述两篇文献讲述了联邦卡尔曼滤波的原理及其特点，并进行了仿真设计，证明了联邦卡尔曼滤波器在不组合导航系统应用中的可行性和优越性，但是系统采用的是联邦卡尔曼的一般结构，其抗干扰能力较弱，容错性能有待改善。本专利技术是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的SINS/GPS子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。
技术实现思路
本专利技术的目的在于提供一种基于故障容错卡尔曼滤波的组合导航方法。本专利技术的目的是这样实现的：基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：步骤1：选择作为状态变量，其中：δλ：惯性导航系统纬度和经度误差；δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；εxεyεz：陀螺一阶马尔可夫过程随机误差；αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；然后根据系统特性，列写出系统的状态方程：SINS/GPS子系统选择位置和速度作为量测量，即式中：λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度；SINS/Doppler子系统选择选择速度作为量测量，即式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度；分别进行离散化处理得到：式中：Φik/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值和方差的估计值Pk；具体过程如下：状态一步预测方程：状态估值计算方程：滤波增益方程：一步预测均方差方程：估计均方差方程：步骤3：构建故障检测函数进行检测：定义故障检测函数其中：rk为残差，即E{rk}＝μ,E{[rk-μ][rk-μ]T}＝Ak；λk是服从自由度为m的χ2分布，即λk～χ2(m)；m为量测Zk的维数；故障判定准则为：其中，TD为预先设置的门限值，在给定误警率Pf时，由χn分布来确定；步骤4：构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ，γ是反映滤波稳健程度的参数，取值为2.5，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计；对于其中，Tk为满秩阵，Yk是状态变量Xk的一种线性组合；进行如下递推估计：其中：步骤5：将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：其中：分别为子滤波器1、子滤波器2的状态估计；P1，P2分别为子滤波器1、子滤波器2的方差估计；为全局状态估计，Pg为全局状态估计协方差阵；步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。与现有技术相比，本专利技术的有益效果在于：本专利技术是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的SINS/GPS子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。附图说明图1为本专利技术的系统结构图；图2为有重置的联邦卡尔曼滤波器的一般结构；图3为无重置的联邦卡尔曼滤波器的一般结构。具体实施方式下面结合附图对本专利技术作出详细说明：具体实施例一：本专利技术的目的在于提供基于故障容错卡尔曼滤波的组合导航方法。针对的是SINS/GPS/Doppler多组合导航系统，该系统的结构图如图1所示。其中图2为有重置的联邦卡尔曼滤波器结构和图3为无重置的联邦卡尔曼滤波器结构。本专利技术是在无重置的联邦卡尔曼滤波器结构上进行优化设计，针对其中的一个子滤波器增加了一个容错卡尔曼滤波器和一个故障检测函数。本专利技术采用的技术方案包括下列步骤：步骤1、选择作为状态变量，其中：δλ：惯性导航系统维度和经度误差；δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；εxεyεz：陀螺一阶马尔可夫过程随机误差；αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；然后根据系统特性，列写出系统的状态方程：SINS/GPS子系统选择位置和速度作为量测量，即式中：λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度。SINS/Doppler子系统选择选择速度作为量测量，即式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度。分别进行离散化处理得到式中：Φik/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k为第i个子系统的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵。步骤2、构建常规卡尔曼滤波器进行状态估计。常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测Pk/k-1；利用测量更新本文档来自技高网...

【技术保护点】
1.基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：步骤1：选择

【技术特征摘要】
1.基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：步骤1：选择作为状态变量，其中：δλ：惯性导航系统纬度和经度误差；δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；εxεyεz：陀螺一阶马尔可夫过程随机误差；αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；然后根据系统特性，列写出系统的状态方程：SINS/GPS子系统选择位置和速度作为量测量，即式中：λI，VIX，VIY分别为惯导解算的纬度，经度，东向速度，北向速度；λG，VGX，VGY分别为GPS测得纬度，经度，东向速度，北向速度；SINS/Doppler子系统选择选择速度作为量测量，即式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度；分别进行离散化处理得到：式中：Φik/k-1，Hik，Γi,k-1，Wi,k-1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测Pk/k-1；利用测量更新求出状态的估计值和方差的估计值Pk；具体过程如下：...

【专利技术属性】
技术研发人员：吴磊，姜南，奔粤阳，王坤，王禹，王贵创，胡泽生，展浩，刘明旭，陈海南，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23