CN104866705B - 机载导航数据的截断式卡尔曼滤波方法 - Google Patents

Abstract

本发明是有关于一种机载导航数据的截断式卡尔曼滤波方法，其包括以下步骤：步骤1设置滤波初始条件，选择初值X₀和P₀；步骤2自适应更新，包括中间矩阵及卡尔曼滤波增益矩阵的计算；步骤3根据增益矩阵进行递推输出及协方差矩阵的更新；步骤4滤波过程循环执行条件判断，若是飞机颠簸所造成数据突跳，则立即终止原滤波过程，重新选择滤波初值进入新的滤波过程，否则继续原滤波。模拟计算表明，截断式卡尔曼滤波可以更准确的描述实际飞行轨迹，既保证了对测量误差的抑制，又可以快速反映飞机位置的突变。

Description

机载导航数据的截断式卡尔曼滤波方法

技术领域

本发明涉及一种机载导航数据的离散式卡尔曼滤波处理，特别是涉及一种机载导航数据的截断式卡尔曼滤波方法。其可用于机载卫星通信、飞机遥感、航空摄影、无人机等应用领域。

背景技术

飞机在空中飞行时，为了确定其自身位置，需要利用GPS或者北斗卫星获得实时的经度、纬度、高度、航向角、速度等各种导航数据。为了消除误差，需要对获得的导航数据进行预处理。

对观测数据预处理方法有很多，包括最小二乘法、样条函授、卡尔曼滤波等等。其中卡尔曼滤波方法广泛应用于航空航天领域。

卡尔曼滤波的显著特点是对状态空间进行动态估计。它采用递推算法，只需要利用上一个参数的验前估计以及新的观测数据进行状态参数的更新，所以它只需要存储前一个历元时刻的状态估算数据，无需存储所有的历史观测数据。卡尔曼滤波具有极高的计算效率，并且可以进行实时估算。

在实际工程应用卡尔曼滤波进行导航数据处理时，通常有一个重要的前提，要求被测量物体的动力学模型具有“一致性”，或者说是“可预测的”。飞机从地面起飞，爬升，平稳飞行，最后降落，其空间位置应当是连续变化的，从当前位置可以预测出下一位置。在这个前提下，卡尔曼滤波算法根据当前和历史的数据，剔除测量误差，推算航空器当前正确的位置。

但是实际上我们都有体会，受气流的影响，飞机高度会突然大幅下降。由于这种下降来自外力(而非来自飞机动力)，所以在这瞬间飞机的动力学模型发生突变。传统的卡尔曼滤波假定飞机的动力学模型是稳定一致的，所以会将这种突跳作为不合理数据进行抑制。虽然最终卡尔曼滤波会逐渐趋向新的飞行轨迹，但是从出现突跳到趋向实际飞行轨迹之间的滤波输出是畸变的。请参阅图1，图中的跳动曲线是带有测量误差的数据，较细的光滑曲线是传统的卡尔曼滤波输出曲线。可以看到，当飞机由于颠簸出现数据突跳(图中原始数据大幅度突变处所示)后，传统的卡尔曼滤波输出数据需要大约100秒后才与实际导航数据重合。

由此可见，上述现有的机载导航数据的卡尔曼滤波方法在方法与使用上，显然仍存在有不便与缺陷，而亟待加以进一步改进。为了解决上述存在的问题，相关厂商莫不费尽心思来谋求解决之道，但长久以来一直未见适用的设计被发展完成，而一般卡尔曼滤波方法又没有适切的处理方法能够解决上述问题，此显然是相关业者急欲解决的问题。因此如何能创设一种新的机载导航数据的卡尔曼滤波方法，实属当前重要研发课题之一，亦成为当前业界极需改进的目标。

发明内容

本发明的目的在于，克服现有的机载导航数据的卡尔曼滤波方法存在的缺陷，而提供一种新的机载导航数据的截断式卡尔曼滤波方法，所要解决的技术问题是当出现由于飞机颠簸造成的数据突跳时，卡尔曼滤波输出数据可以迅速的跟踪实际飞行轨迹，同时保持对测量误差的抑制，从而避免飞机颠簸发生后一段时间内滤波数据的严重失真，非常适于实用。

本发明的目的及解决其技术问题是采用以下技术方案来实现的。依据本发明提出的一种机载导航数据的截断式卡尔曼滤波方法，其包括以下步骤：

步骤1设置滤波初始条件，选择初值X₀和P₀；

步骤2自适应更新，具体包括：

(1)根据建立的观测模型计算中间矩阵；

(2)根据中间矩阵计算卡尔曼滤波增益矩阵G_k；

步骤3根据增益矩阵G_k进行测量值更新，具体包括：

(1)计算k时刻的递推输出X_k；

(2)计算k时刻的协方差矩阵P_k；

步骤4滤波过程循环执行条件判断，具体包括：

(1)将计算出的k时刻的状态值X_k、P_k与前一时刻的状态值X_k-1、P_k-1进行比较，判断是否出现数据突跳；

(2)如果未出现数据突跳，则继续滤波循环，并计算G_k、X_k与P_k；

(3)如果出现数据突跳，判断该数据突跳是否是由于飞机颠簸所造成；

(4)若不是飞机颠簸所造成，则继续滤波循环，并计算G_k、X_k与P_k；

(5)若是飞机颠簸所造成，则立即终止原滤波过程，重新选择滤波初值X₀和P₀进入新的滤波过程。

本发明的目的及解决其技术问题还可采用以下技术措施进一步实现。

前述的一种机载导航数据的截断式卡尔曼滤波方法，其中所述的初值X₀为第一个实际测量值。

前述的一种机载导航数据的截断式卡尔曼滤波方法，其中所述的初值P₀＝aI,其中a为较大的数，I是单位矩阵。

前述的一种机载导航数据的截断式卡尔曼滤波方法，其中，当飞机导航数据出现突跳时，通过判断数据传输校验和是否正确、数据跳动是否在合理范围、数据跳动是否合乎动力学原理的方法来判断是否是飞机出现颠簸造成的。

本发明与现有技术相比具有明显的优点和有益效果。借由上述技术方案，本发明机载导航数据的截断式卡尔曼滤波方法可达到相当的技术进步性及实用性，并具有产业上的广泛利用价值，其至少具有下列优点：

本发明提出的一种新的卡尔曼滤波方案，其特征是，当颠簸发生时，就重新定义为一个新的航段开始，立即终止原来的滤波过程，更换滤波初值，从而进入新的滤波状态。从整个航次看，卡尔曼滤波被截为若干独立的过程，故称为“截断式卡尔曼滤波”。模拟计算表明，截断式卡尔曼滤波可以更准确的描述实际飞行轨迹，既保证了对测量误差的抑制，又可以快速反映飞机位置的突变。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。

附图说明

图1是传统卡尔曼滤波与截断式卡尔曼滤波的效果比较。

图2是本发明一种机载导航数据的截断式卡尔曼滤波方法的实现流程图。

具体实施方式

为更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对依据本发明提出的一种机载导航数据的截断式卡尔曼滤波方法其具体实施方式、方法、步骤、特征及其功效，详细说明如后。

如图2所示，为本发明一种机载导航数据的截断式卡尔曼滤波方法较佳实施例的实现流程图。

根据飞机的动力学模型，建立卡尔曼滤波观测模型：

其中k表示数据测量的时刻，且k＝1,2,3,4….，Y_k为k时刻的观测矢量，为观测矩阵(又称设计矩阵)，X_k为k时刻的状态估值矢量，为噪声矩阵。观测矩阵根据飞机的动力学模型来定，不同种类的飞机其观测矩阵也不同，且观测矩阵会随着滤波时刻的变化而自适应变化，以使滤波结果更为精确。

下面，基于观测模型，详述机载导航数据的截断式滤波方法的实施步骤：

步骤1设置滤波初始条件，选择初值X₀和P₀。

截断式卡尔曼滤波与传统的卡尔曼算法一样是递推算法，需要选取初值。如果能选取测量值的均值作为X₀的初值是最理想的，但是实际上很难做到。由于飞机起飞过程较为平稳，数据偏差比较小，因此我们不妨选择第一个实际测量值作为X₀的初值。设协方差矩阵P₀＝aI,其中a为较大的数，I是单位矩阵。理论上可以证明，对于飞机导航数据滤波，X₀和P₀选择任何初值都是可以收敛的，只是收敛的速度不同。由于飞机起飞速度快时间短，因此为了使滤波过程迅速收敛，需选取a为较大的数，例如100000。

步骤2自适应更新

(1)计算中间矩阵

B_k＝P_k-1H_k ①

②

其中P_k-1为协方差矩阵，H_k为观测矩阵，且与互为转置矩阵，λ为对角矩阵，且0≤λ≤1。当λ越接近0时，卡尔曼滤波结果越接近实测数据，相应地输出波形波动就越大；当λ越接近1时，卡尔曼滤波结果就越趋于一个中间值，所输出波形波动就越平缓。使用者应该根据对测量误差的估计，以及实际应用的需要，决定λ的选取。对于大型民航客机，由于其飞行比较平稳，，一般选取λ较接近于1。

(2)计算卡尔曼滤波增益矩阵

③

其中，B_k为相对应的中间矩阵，为R_k的逆矩阵。

步骤3根据增益矩阵G_k进行测量值更新

(1)计算k时刻的递推输出

④

其中，X_k为k时刻欲获得的最优估算值矢量(卡尔曼滤波输出)，X_k-1是k-1时刻的最优估算值矢量，G_k为k时刻的卡尔曼滤波增益矩阵，为k时刻实测观测矢量，为k时刻的观测矩阵。

(2)计算k时刻的协方差矩阵

⑤

步骤4滤波过程循环执行条件判断

(1)将计算出的K时刻的状态值X_k、P_k与前一时刻的状态值X_k-1、P_k-1进行比较，判断是否出现数据突跳；

(2)如果未出现数据突跳，则继续滤波循环，并计算G_k、X_k与P_k；

(3)如果出现数据突跳，判断该数据突跳是否是由于飞机颠簸所造成；

(4)若不是飞机颠簸所造成，则继续滤波循环，并计算G_k、X_k与P_k；

(5)若是飞机颠簸所造成，则立即终止原滤波过程，重新选择滤波初值X₀和P₀进入新的滤波过程。

为了实现上述的截断式滤波，滤波软件需要判断导航数据跳变是由于数据野值造成，还是由于气流颠簸造成了飞机位移，我们称其为“突跳性质判断”。本发明提出的方法是，借助卡尔曼滤波之外的手段对飞机导航数据的跳变性质做出判断，综合利用数据的校验码、数据冗余性和物理常识，判断数据的合理性。比如，飞机在1秒钟内上升100米是极小概率事件，而突然下降20米是大概率事件。

一旦判断飞机出现了颠簸突跳，卡尔曼滤波软件立即终止原滤波过程，重新设定滤波初值，开始新的滤波过程。

本发明所述截断式卡尔曼滤波效果如图1所示。图1中，由于飞机颠簸造成了滤波数据的突跳，如图中原始数据出现的大幅度数据突变，传统卡尔曼滤波输出数据(图中较细的平滑曲线)需要大约100秒后才与实际导航数据重合，而本发明所述的截断式卡尔曼滤波输出波形(图中较粗的平滑曲线)迅速发生了变化，很快便与实测原始数据吻合，可以更准确的描述实际飞行轨迹，既保证了对测量误差的抑制，又可以快速反映飞机位置的突变。

在实际工程中，还需要判断导航数据对具体控制过程的敏感性和重要性。有些数据，例如磁偏角，对于导航计算无关紧要，即使发生突跳也无需特别处理。

以上所述，仅是本发明的较佳实施例而已，并非对本发明做任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (4)

1.一种机载导航数据的截断式卡尔曼滤波方法，其特征在于其包括以下步骤：

步骤1设置滤波初始条件，选择初值X₀和P₀；

步骤2自适应更新，具体包括：

(1)根据建立的观测模型计算中间矩阵，以及B_k＝P_k-1H_k；

其中k表示数据测量的时刻，且k＝1,2,3,4….，Y_k是k时刻的实测数据矢量，是观测矩阵，上标T表示矩阵转置，X_k是k时刻的状态估计矢量,通常选取飞机起飞的初始位置值作为X₀，P_k-1是k-1时刻的协方差矩阵，V_k是噪声矩阵，λ是对角矩阵，且0≤λ≤1；

(2)根据中间矩阵R_k和B_k计算卡尔曼滤波增益矩阵

步骤3根据增益矩阵G_k进行测量值更新，具体包括：

(1)计算k时刻的递推输出

(2)计算k时刻的协方差矩阵

步骤4滤波过程循环执行条件判断，具体包括：

(1)将计算出的k时刻的状态值X_k、P_k与前一时刻的状态值X_k-1、P_k-1进行比较，判断是否出现数据突跳；

(2)如果未出现数据突跳，则继续滤波循环，并计算G_k、X_k与P_k；

(3)如果出现数据突跳，判断该数据突跳是否是由于飞机颠簸所造成；

(4)若不是飞机颠簸所造成，则继续滤波循环，并计算G_k、X_k与P_k；

(5)若是飞机颠簸所造成，则立即终止原滤波过程，重新选择滤波初值X₀和P₀进入新的滤波过程。

2.根据权利要求1所述的一种机载导航数据的截断式卡尔曼滤波方法，其特征在于其中所述的初值X₀为飞机导航数据的第一个实际测量值。

3.根据权利要求1所述的一种机载导航数据的截断式卡尔曼滤波方法，其特征在于其中所述的初值P₀＝aI,其中a为较大的数，I是单位矩阵。

4.根据权利要求1所述的一种机载导航数据的截断式卡尔曼滤波方法，其特征在：当λ接近0时，卡尔曼滤波结果接近实测数据，但是输出结果波动较大，当λ接近1时，卡尔曼滤波结果趋向一个中间值，所输出波形趋于平缓。