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

权利要求书

1.基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：

步骤1：选择作为状态变量，

其中：δλ：惯性导航系统纬度和经度误差；

δV_x,δV_y：惯性导航系在地理坐标系东向和北向的速度误差；

ε_x ε_y ε_z：陀螺一阶马尔可夫过程随机误差；

α β λ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；

然后根据系统特性，列写出系统的状态方程：

SINS/GPS子系统选择位置和速度作为量测量，

即

式中：λ_I，V_IX，V_IY分别为惯导解算的纬度，经度，东向速度，北向速度；λ_G，V_GX，V_GY分别为GPS测得纬度，经度，东向速度，北向速度；

SINS/Doppler子系统选择选择速度作为量测量，

即

式中：V_IX，V_IY分别为惯导解算的东向速度，北向速度；V_dX，V_dY，分别为多普勒测得的东向速度，北向速度；

分别进行离散化处理得到：

式中：Φⁱ_k/k-1，H_ik，Γ_i,k-1，W_i,k-1，V_i,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；

步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测P_k/k-1；利用测量更新求出状态的估计值和方差的估计值P_k；具体过程如下：

状态一步预测方程：

状态估值计算方程：

滤波增益方程：

一步预测均方差方程：

估计均方差方程：

步骤3：构建故障检测函数进行检测：

定义故障检测函数

其中：r_k为残差，即

E{r_k}＝μ,E{[r_k-μ][r_k-μ]^T}＝A_k；

λ_k是服从自由度为m的χ²分布，即λ_k～χ²(m)；m为量测Z_k的维数；

故障判定准则为：

其中，T_D为预先设置的门限值，在给定误警率P_f时，由χ_n分布来确定；

步骤4：构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ，γ是反映滤波稳健程度的参数，取值为2.5，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计；

对于

其中，T_k为满秩阵，Y_k是状态变量X_k的一种线性组合；

进行如下递推估计：

其中：

步骤5：将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：

其中：分别为子滤波器1、子滤波器2的状态估计；

P₁，P₂分别为子滤波器1、子滤波器2的方差估计；

为全局状态估计，P_g为全局状态估计协方差阵；

步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。