[发明专利]基于故障容错卡尔曼滤波的组合导航方法

摘要

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

主权项

1.基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：步骤1：选择作为状态变量，其中：δλ：惯性导航系统纬度和经度误差；δVx,δVy：惯性导航系在地理坐标系东向和北向的速度误差；εx εy εz：陀螺一阶马尔可夫过程随机误差；α β λ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；然后根据系统特性，列写出系统的状态方程：SINS/GPS子系统选择位置和速度作为量测量，即式中：λ_I，V_IX，V_IY分别为惯导解算的纬度，经度，东向速度，北向速度；λ_G，V_GX，V_GY分别为GPS测得纬度，经度，东向速度，北向速度；SINS/Doppler子系统选择选择速度作为量测量，即式中：VIX，VIY分别为惯导解算的东向速度，北向速度；VdX，VdY，分别为多普勒测得的东向速度，北向速度；分别进行离散化处理得到：式中：Φik/k‑1，Hik，Γi,k‑1，Wi,k‑1，Vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测P_k/k‑1；利用测量更新求出状态的估计值和方差的估计值P_k；具体过程如下：状态一步预测方程：状态估值计算方程：滤波增益方程：一步预测均方差方程：估计均方差方程：步骤3：构建故障检测函数进行检测：定义故障检测函数其中：r_k为残差，即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的方差估计；为全局状态估计，P_g为全局状态估计协方差阵；步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。