CN102937449A - 惯性导航系统中跨音速段气压高度计和gps信息两步融合方法 - Google Patents

Abstract

本发明公开了一种惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法，包括互补滤波和卡尔曼滤波，其中互补滤波采用大气/GPS高度通道互补融合滤波，以消除气压高度值剧烈变化；卡尔曼滤波通过建立气压高度计/GPS/惯性导航系统组合导航系统状态方程和量测方程，通过两步融合最终得到惯性高度通道的状态最优估计。本发明中气压高度计和GPS组成高度冗余和故障检测隔离系统，可以有效提高飞信过程中特别是跨音速阶段高度值得精度，为精确导航和飞行控制提供有力条件。

Description

惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法

技术领域

本发明涉及一种用于跨音速段气压高度计和GPS两步融合方法，能够通过融合气压高度计和GPS的测量值，用于飞行器高度导航参数的确定，属于导航、制导与控制领域。

背景技术

飞行高度信息的准确与否对飞行器能否安全飞行起至关重要的作用。由于惯性系统的高度通道是不稳定的，除了采取措施补偿引力(或重力)加速度外,还要引入外部高度信息与惯性垂直通道信息进行综合,目前一般采用引入气压高度表或大气数据中心等外部高度信息进行阻尼，修正惯性高度。

基于经典控制理论设计的惯性/大气阻尼系统，其本质是惯导高度向大气高度逼近。在飞行器进行跨音速和俯冲段大机动任务飞行时，大气测量误差变化剧烈，严重影响惯性/大气阻尼性能。高度通道性能严重滞后于飞行特性和任务特点日益多样化的航空平台及武器对惯导系统高度通道性能的需求，无法满足飞行器在精密导航飞行阶段的自主控制精确、安全可靠的高精度要求。

传统飞行器高度测量多采用气压高度计或者GPS分别测量飞行高度。气压高度计的短时精度较高，但会随着大气压与温度的变化而发生变化，随时间的变长会产生较大的高度测量误差。当高度很高时，气压高度表的精度也会下降。GPS易受周围建筑物和树丛的遮挡而失效，在特殊时期由于受某些因素影响亦无法使用。

因此，无论是GPS还是气压高度计，单独进行导航时所提供的标准定位服务都无法满足飞行器在精密导航飞行阶段的自主控制精确、安全可靠的高精度要求。

发明内容

本发明所要解决的技术问题在于：针对进行跨音速大机动任务飞行时，大气测量误差变化剧烈、严重影响传统惯性/大气阻尼性能的问题，提出一种跨音速段气压高度计和GPS信息两步融合方法。

本发明为解决上述技术问题，采用如下技术方案：

本发明一种惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法，包括以下步骤：

第一步：互补滤波融合；

步骤（1），航空机载气压高度计测量的高度h_ADS和GPS测量的高度信息h_GPS，经过互补滤波器环节处理后进行融合，得到融合后的高度h_p为：

$h_p=\frac{{\mathrm{sh}}_{\mathrm{ADS}}+{\mathrm{kh}}_{\mathrm{GPS}}}{s+k}---\left(1\right)$

其中，k为截止频率，s为拉普拉斯变换；

步骤（2），对步骤（1）模型离散化，可得到如下滤波方程

其中，T为采样周期，

为互补滤波融合高度；

第二步：卡尔曼滤波融合；

步骤（3），通过对惯性导航系统的性能及误差源的分析，建立基于大气辅助的航空机载惯性导航系统的误差状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

其中，X(t)为连续系统在t时刻的状态矢量，F(t)为连续系统在t时刻的状态系数矩阵，G(t)为连续系统在t时刻的误差系数矩阵，W(t)为连续系统在t时刻的白噪声随机误差矢量；

航空机载惯性导航系统误差状态量X定义为：

$X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\δh}}_p]}^T,$

其中φ_E,φ_N,φ_U为平台误差角；δv_E,δv_N,δv_U为速度误差；δL,δλ，δh为纬度、经度和高度误差；ε_bx，ε_by,ε_bz为陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为一阶马尔可夫漂移误差；

为加速度计零偏，δhp为气压高度计经步骤(2)得到的互补滤波融合高度，其模型方程表达式为：

$\mathrm{\δ}{\stackrel{\·}{h}}_p=-\frac{v_d}{T_p}{\mathrm{\δh}}_p+{\mathrm{\ω}}_p---\left(4\right)$

式中，v_d为飞行地速，T_p为气压相关系数，ω_p为测量误差高斯白噪声；

步骤（4），采用航空机载地理系下位置线性化观测原理，建立航空机载地理系下位置观测量和步骤(3)所述的误差状态量中的高度误差状态量之间线性化误差量测方程，其表达式如下：

$Z\left(t\right)=\left[\begin{array}{c}{h}_I-h_G\\ h_I-h_p\end{array}\right]=\left[\begin{array}{c}\mathrm{\δh}+v_1\\ \mathrm{\δh}+{\mathrm{\δh}}_p\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(5\right)$

其中，Z(t)为连续系统在t时刻的量测矢量，H(t)为连续系统在t时刻的量测矩阵，V(t)为连续系统在t时刻的量测噪声。

h_I表示惯性导航系统的位置信息：h_I=h_t+δh；

h_G表示GPS接收机给出的位置信息：h_G=h_t-v₁；

h_p表示气压高度的位置信息为：h_p=h_t-δh_p；

其中，h_t为高度的真值，δh为高度误差，v₁考虑为高斯白噪声；

步骤（5），进行误差状态方程和误差量测方程的离散化，获得惯性导航系统的线性化卡尔曼滤波器，实现基于大气高度辅助的INS/GPS高精度组合导航，具体如下：

a，进行误差状态方程和误差量测方程的离散化，其离散化形式如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(6\right)$

式中 ${\mathrm{\Φ}}_{k,k-1}=\underset{m=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F\left(t_k\right)T\right]}^m/m!,$ ${\mathrm{\Γ}}_{k-1}=\left\{\underset{m=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{m!}{\left(F\left(t_k\right)T\right)}^{m-1}\left]\right\}G\left(t_k\right)T,$ 时刻k、变量m为自然数，T为迭代周期；

b，获得系统的线性化卡尔曼滤波器方程如下：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}],$ 其中： ${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1};$

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$ 其中：

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1},$ $P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{{\mathrm{\Φ}}_{k,k-1}}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{{\mathrm{\Γ}}_{k-1}}^T;$

上式中P_k为系统k时刻的最优滤波误差协方差阵，K_k为系统k时刻的滤波增益，P_k/k-1为系统k时刻的一步预测均方误差，Q_k,R_k分别为系统k时刻的噪声方差矩阵和量测方差矩阵。

进一步的，本发明的一种惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法，在进行第一步互补滤波融合之前，还包括采用故障检测与隔离算法判断GPS传感器是否有故障的步骤，具体如下：

①计算故障检测函数λ(k)：

$\left\{\begin{array}{c}\mathrm{\γ}\left(k\right)=Z\left(k\right)-Z_{\mathrm{gps}}\left(k\right)\\ \mathrm{\λ}\left(k\right)={\mathrm{\γ}}^T\left(k\right)\mathrm{\γ}\left(k\right)\end{array}\right.---\left(7\right)$

式中，Z(k)为捷联惯性导航系统高度信息最优估计值，Z_gps(k)为GPS高度输出值；

②故障判断的准则为：

式中，T_D1、T_D2为预先设定的门限；

③若判断出GPS系统无故障，则执行步骤（1）至步骤（5）进行气压高度计和GPS信息两步融合；

若判断出GPS系统有故障，则对GPS系统进行隔离，并通过系统重构不致因故障而失效，当隔离GPS后系统处于跨音速阶段，切断气压高度计实施纯捷联惯性导航；当隔离GPS后系统处于跨音速以外阶段，对气压高度计实施高度通道阻尼；

当检测到GPS系统恢复正常后，再执行步骤（1）至步骤（5），重新进行气压高度计和GPS信息两步融合。

本发明与现有技术相比克服了进行跨音速大机动任务飞行时，大气测量误差变化剧烈，严重影响传统惯性/大气阻尼性能，构建了一种跨音速段气压高度计和GPS两步融合方法。主要具有以下优点：

（1）通过互补滤波器抑制大气测量误差在跨音速阶段剧烈变化，使互补融合大气高度消除剧烈变化，变化平缓；

（2）采用气压高度计和GPS测量飞行高度，将气压高度计和GPS构成冗余和故障检测与隔离系统，GPS信息主要有两个用途，一个是用于和大气参数融合，另外一个就是用于卡尔曼滤波器中和捷联惯性导航输出进行组合修正。

附图说明

图1为本发明的气压高度计和GPS两步融合流程图；

图2为本发明的大气/GPS高度互补滤波器结构图；

图3为本发明的气压高度计和GPS两步融合高度误差仿真对比结果图；

图4为加入故障检测与隔离前后的气压高度计和GPS两步融合高度误差仿真结果图。

具体实施方式

下面结合附图对发明的技术方案进行进一步地详细说明：

本发明利用气压高度计信息和GPS信息融合获取高精度高度信息，融合算法主要分为如下两步：

1、互补滤波融合：基于互补滤波器的思想，若将无偏差但受噪声干扰大的GPS导航系统所测得的高度信息，与低噪声但存在偏差的大气数据系统所测得的高度信息融合。

2、卡尔曼滤波融合：构建气压高度计/GPS/惯性导航系统组合，实现基于大气高度辅助的INS/GPS高精度组合导航。

如图1所示，具体实施方法如下：

一、互补滤波融合：

步骤（1），建立气压高度计和GPS互补滤波器：

惯性高度通道一般采用基于经典控制理论设计的惯性/大气阻尼系统，在跨音速阶段，传统大气阻尼有较大的影响，导致其误差特性同样产生较大波动。基于互补滤波器的这种思想，若将无偏差但受噪声干扰大的GPS导航系统所测得的高度信息，与低噪声但存在偏差的大气数据系统所测得的高度信息融合，获取各自想要的分量，必定能够实现更为精确的飞行高度的获取。

参考图2，建立如图2所示的互补滤波结构，航空机载气压高度计测量的高度h_ADS和GPS高度信息h_GPS，经过互补滤波器环节处理后进行融合，得到融合后的高度h_p为：

$h_p=\frac{{\mathrm{sh}}_{\mathrm{ADS}}+{\mathrm{kh}}_{\mathrm{GPS}}}{s+k}---\left(1\right)$

其中，k为截止频率，s为拉普拉斯变换。

步骤（2），气压高度计和GPS互补滤波器离散化

对步骤（1）连续系统模型(1)离散化，可得到如下滤波方程

其中，T为采样周期，为互补滤波融合高度。

二、卡尔曼滤波融合：

步骤A，建立航空机载惯性导航系统的误差状态方程：

选择导航坐标系为东北天地理水平坐标系（O_nX_nY_nZ_n），采用线性卡尔曼滤波器进行组合，系统的状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能及误差源的分析，可以获得惯导系统的误差状态量方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

其中，X(t)为连续系统在t时刻的状态矢量，F(t)为连续系统在t时刻的状态系数矩阵，

G(t)为连续系统在t时刻的误差系数矩阵，W(t)为连续系统在t时刻的白噪声随机误差矢量。

式中， $X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\δh}}_p]}^T$

其中φ_E,φ_N,φ_U为平台误差角；δv_E,δv_N,δv_U为速度误差；δL,δλ，δh为纬度、经度和高度误差；ε_bx，ε_by,ε_bz为陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为一阶马尔可夫漂移误差；

为加速度计零偏，δh_p为气压高度计经步骤(2)得到的互补滤波融合高度，其方程表达式为：

$\mathrm{\δ}{\stackrel{\·}{h}}_p=-\frac{v_d}{T_p}{\mathrm{\δh}}_p+{\mathrm{\ω}}_p---\left(4\right)$

式中，v_d为飞行地速，T_p为气压相关系数，ω_p为测量误差高斯白噪声。

步骤B，建立航空机载惯性导航系统的误差量测方程：

采用航空机载地理系下位置线性化观测原理，建立航空机载地理系下位置观测量和被估计的步骤A所述的航空机载惯性导航系统误差状态量中的高度误差状态量之间的线性化量测方程，其观测量有两组：一组为位置观测量，即惯性导航系统给出的高度信息和GPS接收机给出相应信息的差值；惯导高度与互补滤波高度之差作为组合卡尔曼滤波器的另一组观测量。

表示惯性导航系统的位置信息为：h_I=h_t+δh；

表示GPS接收机给出的位置信息为：h_G=h_t-v₁；

表示气压高度的位置信息为：h_p=h_t-δh_p；

其中，h_t为高度的真值，δh为高度误差，v₁考虑为高斯白噪声。

其表达式如下：

$Z\left(t\right)=\left[\begin{array}{c}{h}_I-h_G\\ h_I-h_p\end{array}\right]=\left[\begin{array}{c}\mathrm{\δh}+v_1\\ \mathrm{\δh}+{\mathrm{\δh}}_p\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(5\right)$

其中，Z(t)为连续系统在t时刻的量测矢量，H(t)为连续系统在t时刻的量测矩阵，V(t)为连续系统在t时刻的量测噪声。

步骤C，状态方程和量测方程的离散化及卡尔曼滤波器：

当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(3)和量测方程(5)进行离散化，从而获得离散形式的系统方程。其离散化形式如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(6\right)$

式中 ${\mathrm{\Φ}}_{k,k-1}=\underset{m=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F\left(t_k\right)T\right]}^m/m!,$ ${\mathrm{\Γ}}_{k-1}=\left\{\underset{m=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{m!}{\left(F\left(t_k\right)T\right)}^{m-1}\left]\right\}G\left(t_k\right)T,$ k、m为自然数，T为迭代周期。

从而可以获得系统的线性化卡尔曼滤波器方程如下：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}],$ 其中： ${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1};$

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$ 其中：

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1},$ $P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{{\mathrm{\Φ}}_{k,k-1}}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{{\mathrm{\Γ}}_{k-1}}^T;$

上式中P_k为系统k时刻的最优滤波误差协方差阵，K_k为系统k时刻的滤波增益，P_k/k-1为系统k时刻的一步预测均方误差，Q_k,R_k分别为系统k时刻的噪声方差矩阵和量测方差矩阵。

步骤D，故障检测与隔离算法，判断GPS传感器是否有故障：

考虑到GPS易受周围建筑物和树丛的遮挡而失效，此外，GPS受某些发达国家控制，在特殊时期亦无法使用，必须实时地确定GPS系统处理的高度信息的有效性，所以应配备实时的故障检测和隔离算法。

①计算故障检测函数λ(k)：

$\left\{\begin{array}{c}\mathrm{\γ}\left(k\right)=Z\left(k\right)-Z_{\mathrm{gps}}\left(k\right)\\ \mathrm{\λ}\left(k\right)={\mathrm{\γ}}^T\left(k\right)\mathrm{\γ}\left(k\right)\end{array}\right.---\left(7\right)$

式中，Z(k)为捷联惯性导航系统高度信息最优估计值，Z_gps(k)为GPS高度输出值。

②故障判断的准则为：

式中，T_D1、T_D2为预先设定的门限。

若判断出GPS系统无故障，则进行气压高度计和GPS高度两步融合；若检测出GPS系统有故障，则对GPS系统进行隔离，并通过系统重构不致因故障而失效。考虑气压高度计在跨音速阶段误差较大，当隔离GPS后系统处于跨音速阶段，切断气压高度计实施纯捷联惯性导航；当隔离GPS后系统处于跨音速以外阶段，对气压高度计实施高度通道阻尼。当检测到GPS系统恢复正常后，再重新进行气压高度计和GPS高度两步融合。

图3的仿真结果表明，采用气压高度计和GPS两步融合，飞行器在高度方向上的运动轨迹更加接近于其真实运动轨迹，数据融合算法是有效与传统二阶阻尼高度误差相比大大提高，较气压高度计和GPS直接卡尔曼滤波定位精度明显改善。

图4的仿真结果表明，系统没有加入故障检测单元时，系统的恢复速度慢，定位精度受GPS故障影响较大；加入故障检测和隔离后，系统的定位精度和稳定性得到显著提高。

Claims (2)

1.一种惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法，其特征在于包括以下步骤：

第一步：互补滤波融合；

步骤（1），航空机载气压高度计测量的高度h_ADS和GPS测量的高度信息h_GPS，经过互补滤波器环节处理后进行融合，得到融合后的高度h_p为：

$h_p=\frac{{\mathrm{sh}}_{\mathrm{ADS}}+{\mathrm{kh}}_{\mathrm{GPS}}}{s+k}---\left(1\right)$

其中，k为截止频率，s为拉普拉斯变换；

步骤（2），对步骤（1）模型离散化，可得到如下滤波方程

其中，T为采样周期，

为互补滤波融合高度；

第二步：卡尔曼滤波融合；

步骤（3），通过对惯性导航系统的性能及误差源的分析，建立基于大气辅助的航空机载惯性导航系统的误差状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

其中，X(t)为连续系统在t时刻的状态矢量，F(t)为连续系统在t时刻的状态系数矩阵，G(t)为连续系统在t时刻的误差系数矩阵，W(t)为连续系统在t时刻的白噪声随机误差矢量；

航空机载惯性导航系统误差状态量X定义为：

$X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\δh}}_p]}^T,$

其中φ_E,φ_N,φ_U为平台误差角；δv_E,δv_N,δv_U为速度误差；δL,δλ，δh为纬度、经度和高度误差；ε_bx，ε_by,ε_bz为陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为一阶马尔可夫漂移误差；

为加速度计零偏，δhp为气压高度计经步骤(2)得到的互补滤波融合高度，其模型方程表达式为：

$\mathrm{\δ}{\stackrel{\·}{h}}_p=-\frac{v_d}{T_p}{\mathrm{\δh}}_p+{\mathrm{\ω}}_p---\left(4\right)$

式中，v_d为飞行地速，T_p为气压相关系数，ω_p为测量误差高斯白噪声；

步骤（4），采用航空机载地理系下位置线性化观测原理，建立航空机载地理系下位置观测量和步骤(3)所述的误差状态量中的高度误差状态量之间线性化误差量测方程，其表达式如下：

$Z\left(t\right)=\left[\begin{array}{c}{h}_I-h_G\\ h_I-h_p\end{array}\right]=\left[\begin{array}{c}\mathrm{\δh}+v_1\\ \mathrm{\δh}+{\mathrm{\δh}}_p\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(5\right)$

其中，Z(t)为连续系统在t时刻的量测矢量，H(t)为连续系统在t时刻的量测矩阵，V(t)为连续系统在t时刻的量测噪声。

h_I表示惯性导航系统的位置信息：h_I=h_t+δh；

h_G表示GPS接收机给出的位置信息：h_G=h_t-v₁；

h_p表示气压高度的位置信息为：h_p=h_t-δh_p；

其中，h_t为高度的真值，δh为高度误差，v₁考虑为高斯白噪声；

步骤（5），进行误差状态方程和误差量测方程的离散化，获得惯性导航系统的线性化卡尔曼滤波器，实现基于大气高度辅助的INS/GPS高精度组合导航，具体如下：

a，进行误差状态方程和误差量测方程的离散化，其离散化形式如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(6\right)$

式中 ${\mathrm{\Φ}}_{k,k-1}=\underset{m=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F\left(t_k\right)T\right]}^m/m!,$ ${\mathrm{\Γ}}_{k-1}=\left\{\underset{m=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{m!}{\left(F\left(t_k\right)T\right)}^{m-1}\left]\right\}G\left(t_k\right)T,$ 时刻k、变量m为自然数，T为迭代周期；

b，获得系统的线性化卡尔曼滤波器方程如下：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}],$ 其中： ${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1};$

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$ 其中：

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1},$ $P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{{\mathrm{\Φ}}_{k,k-1}}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{{\mathrm{\Γ}}_{k-1}}^T;$

上式中P_k为系统k时刻的最优滤波误差协方差阵，K_k为系统k时刻的滤波增益，P_k/k-1为系统k时刻的一步预测均方误差，Q_k,R_k分别为系统k时刻的噪声方差矩阵和量测方差矩阵。

2.根据权利要求1所述的一种惯性导航系统中跨音速段气压高度计和GPS信息两步融合方法，在进行第一步互补滤波融合之前，还包括采用故障检测与隔离算法判断GPS传感器是否有故障的步骤，具体如下：

①计算故障检测函数λ(k)：

$\left\{\begin{array}{c}\mathrm{\γ}\left(k\right)=Z\left(k\right)-Z_{\mathrm{gps}}\left(k\right)\\ \mathrm{\λ}\left(k\right)={\mathrm{\γ}}^T\left(k\right)\mathrm{\γ}\left(k\right)\end{array}\right.---\left(7\right)$

式中，Z(k)为捷联惯性导航系统高度信息最优估计值，Z_gps(k)为GPS高度输出值；

②故障判断的准则为：

式中，T_D1、T_D2为预先设定的门限；

③若判断出GPS系统无故障，则执行步骤（1）至步骤（5）进行气压高度计和GPS信息两步融合；

若判断出GPS系统有故障，则对GPS系统进行隔离，并通过系统重构不致因故障而失效，当隔离GPS后系统处于跨音速阶段，切断气压高度计实施纯捷联惯性导航；当隔离GPS后系统处于跨音速以外阶段，对气压高度计实施高度通道阻尼；

当检测到GPS系统恢复正常后，再执行步骤（1）至步骤（5），重新进行气压高度计和GPS信息两步融合。

Images