CN113916222A - 基于卡尔曼滤波估计方差约束的组合导航方法 - Google Patents

Abstract

本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。应用本发明的技术方案，能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。

Description

基于卡尔曼滤波估计方差约束的组合导航方法

技术领域

本发明涉及惯性导航技术领域，尤其涉及一种基于卡尔曼滤波估计方差约束的组合导航方法。

背景技术

在组合导航系统的使用过程中，在天向通道上由于存在重力，进行卡尔曼滤波器估计时，在系统进行长时间组合导航经常会出现高度通道与卫星高度之间存在一个缓慢增大的偏差，这对高精度定点打击或者精密无人机悬停的影响很大。

针对卡尔曼滤波过程进行分析总结，发现系统的某一个状态量观测性极强的情况下，卡尔曼滤波器的方差会快速的收敛，而系统在后续的组合导航过程中，加速度计零位或者标定因素发生变化时，滤波器无法再进行跟踪，导致组合导航的天向通道出现偏差，具体表现为长时间组合导航过程中高度与卫星高度慢慢出现偏差值；当系统长时间不进行卡尔曼滤波的量测更新时，由于天向通道及其容易发散，卡尔曼滤波过程中对应高度与天速状态量的方差变得极大，比如高度发散到10000000m，而陀螺漂移设定值为0.01°/h，在初期滤波过程中收敛至0.001°/h，转换为标准单位4.85e-9rad/s，当系统再次具备量测更新的条件时，在解算运算过程中，由于计算机的精度有限，容易出现“大数吃小数”的现象，导致滤波出现异常。

发明内容

本发明旨在至少解决现有技术中存在的技术问题之一。

本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。

进一步地，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值包括：根据惯性器件误差设定卡尔曼滤波器的误差协方差P_k+1/k+1的对角线元素的最大值P_Max和最小值P_min，其中，最大值P_Max和最小值P_min均为一维数组，维数与误差协方差P_k+1/k+1的对角线元素个数以及状态量

的维数相同，最大值P_Max和最小值P_min中的各元素与状态量

中的各变量一一对应，最大值P_Max中的各元素均为元素最大值，最小值P_min中的各元素均为元素最小值；在进行卡尔曼滤波计算的量测更新过程中，若误差协方差P_k+1/k+1的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k+1的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；在进行卡尔曼滤波计算的时间更新过程中，若误差协方差P_k+1/k的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；其中，P_k+1/k和P_k+1/k+1分别为k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，

为k时刻系统状态量估计值，k为计算时刻，k＝1,2,...n，n为整数。

进一步地，基于卡尔曼滤波估计方差约束的组合导航方法可根据

构建系统误差模型，其中，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Q_k，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为R_k，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量

δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δV_N、δV_U、δV_E分别为惯导北、天、东速度误差，φ_N、φ_U、φ_E分别为惯导系统地理坐标系内北、天、东三个方向的失准角，

和

分别为x加速度计、y加速度计和z加速度计的加速度计零偏，ε_x、ε_y和ε_z分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。

进一步地，设定卡尔曼滤波器初值包括：设定卡尔曼滤波的初始协方差矩阵P₀、系统噪声方差阵、系统误差状态初值、量测噪声方差阵和卡尔曼滤波计算周期。

进一步地，卡尔曼滤波计算中时间更新公式为

P_k+1/k＝Φ_{k+1, k}P_k/kΦ^T _k+1,k+Q_k，新息计算公式为

量测更新公式为K_k+1＝P_k+1/kH^T _k+1[H_{k+ 1}P_k+1/kH^T _k+1+R_k+1]^-1，P_k+1/k+1＝[I-K_k+1H_k+1]P_k+1/k，

不进行量测更新时K_k+1＝0，其中，Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵，

分别为k时刻系统状态量估计值、k到k+1时刻系统状态量一步预测估计值、k+1时刻系统状态量量测更新后估计值，P_k/k为k时刻的误差协方差值，Z_k+1表示k+1时刻系统的量测量，γ_k表示新息值，K_k+1表示量测更新过程中的增益值，Q_k、R_k+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。

应用本发明的技术方案，提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明的具体实施例提供的基于卡尔曼滤波估计方差约束的组合导航方法的流程示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

如图1所示，根据本发明的具体实施例提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。

应用此种配置方式，提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。

进一步地，在本发明中，为了实现基于卡尔曼滤波估计方差约束的组合导航，首先对惯导系统进行粗对准。作为本发明的一个具体实施例，在惯导系统开机后，利用陀螺角速率

和加速度计比力

进行粗对准，确定初始姿态。

此外，在本发明中，在粗对准结束后，根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度。

进一步地，在本发明中，在完成惯性导航解算后，建立系统误差模型。作为本发明的一个具体实施例，基于卡尔曼滤波估计方差约束的组合导航方法可根据

构建系统误差模型，其中，k为计算时刻，k＝1,2,...n，n为整数，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Q_k，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为R_k，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量

δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δV_N、δV_U、δV_E分别为惯导北、天、东速度误差，φ_N、φ_U、φ_E分别为惯导系统地理坐标系内北、天、东三个方向的失准角。

和

分别为x加速度计、y加速度计和z加速度计的加速度计零偏，ε_x、ε_y和ε_z分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。在该实施例中，可根据实际情况设定计算时刻k的周期，例如可设定为1.0s。在本发明中，根据导航计算结果和系统误差模型解算系统状态转移矩阵。

此外，在本发明中，在建立系统误差模型后，选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值。作为本发明的一个具体实施例，在本发明中，选取惯导位置和速度为卡尔曼滤波的量测量，设定卡尔曼滤波的初始协方差矩阵P₀，其参数可根据初始误差的量级进行设置；设定系统噪声方差阵Q，可根据实际系统噪声大小进行设置；设定系统误差状态初值X₀，维数为15×1，一般设为零矩阵；设定量测噪声方差阵R；设定卡尔曼滤波计算周期T_k。

进一步地，在本发明中，在设定卡尔曼滤波器初值后，进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。

作为本发明的一个具体实施例，卡尔曼滤波计算中时间更新公式为

P_k+1/k＝Φ_k+1,kP_k/kΦ^T _k+1,k+Q_k，新息计算公式为

量测更新公式为K_k+1＝P_k+1/kH^T _k+1[H_k+1P_k+1/kH^T _k+1+R_k+1]^-1，P_k+1/k+1＝[I-K_k+1H_k+1]P_k+1/k，

不进行量测更新时K_k+1＝0，其中，Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵，

分别为k时刻系统状态量估计值、k到k+1时刻系统状态量一步预测估计值、k+1时刻系统状态量量测更新后估计值，P_k/k、P_k+1/k、P_k+1/k+1分别为k时刻、k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，Z_k+1表示k+1时刻系统的量测量，γ_k表示新息值，K_k+1表示量测更新过程中的增益值，Q_k、R_k+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。通过上述卡尔曼滤波计算能够获取组合导航数据直至组合导航任务结束。

此外，在本发明中，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值具体包括：

根据惯性器件误差设定卡尔曼滤波器的误差协方差P_k+1/k+1的对角线元素的最大值P_Max和最小值P_min，其中，最大值P_Max和最小值P_min均为一维数组，维数与误差协方差P_k+1/k+1的对角线元素个数以及状态量

的维数相同，最大值P_Max和最小值P_min中的各元素与状态量

中的各变量一一对应，最大值P_Max中的各元素均为元素最大值，最小值P_min中的各元素均为元素最小值；

在进行卡尔曼滤波计算的量测更新过程中，若误差协方差P_k+1/k+1的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k+1的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

在进行卡尔曼滤波计算的时间更新过程中，若误差协方差P_k+1/k的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值。

通过上述约束方法可将卡尔曼滤波器的误差协方差的取值限定在一定范围内，能够明显提高长时间组合导航的精度，尤其是可观测性极强的高度通道，组合导航的高度不会与基准高度存在固定偏差。

本发明针对我国组合导航系统的发展需求，在现阶段对初始对准方法认识的基础上，分析系统卡尔曼滤波模型中不同观测量对滤波估计的影响程度，对惯组误差参数的种类及特性进行更加准确的分析与建模，同时针对卡尔曼滤波过程进行分析总结，提出了一种基于卡尔曼滤波估计方差约束的组合导航方法。

本发明的基于卡尔曼滤波估计方差约束的组合导航方法适用于长时间工作的组合导航系统，能够在消除在长时间组合导航过程中由于前期可观测性强的状态量在后期的组合导航过程中，状态量因环境等原因发生变化时滤波器无法跟踪的情况，提升组合导航性能，同时能够避免在受外界量测信息的影响而一直工作在时间更新状态一段时间后再次进行量测更新时出现滤波发散的问题，进一步提升了滤波器的稳定性；解决了传统对准方案的长时间组合导航精度受限及可能出现滤波发散的问题。

本发明的基于卡尔曼滤波估计方差约束的组合导航方法通过对组合导航滤波过程中的误差协方差的上下限进行约束，在现有组合导航技术的基础上进一步提升了长时间组合导航的精度，能够防止长时间工作条件下的估计效果明显下降，提高系统模型的可观测性，能够对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高了导航系统的组合性能。

为了对本发明有进一步地了解，下面对本发明的一个具体实施例进行详细说明。

在该具体实施例中，以系统静基座对准为例，进行长时间组合导航，组合导航30min后，人为的增加天向加速度计的误差系统，模拟弹用或者机载惯导系统在一段时间组合导航后受环境温度或者飞行高度的影响而产生的加速度计零位、刻度系数差产生影响的情况，状态如下所示：系统北天东放置，即滚动角、方位角、俯仰角均为0°，设定粗对准后系统导航坐标系与地理坐标系之间失准角均为1°，惯导系统陀螺常值漂移0.01°/h,陀螺随机游走系数

天向加速度计常值零位100ug，工作0.5h后，后续的1.5h中天向加速度计零位由100ug变化到200ug,当地纬度39.8133°，系统在组合导航2h，卡尔曼滤波周期为1s。

基于卡尔曼滤波估计方差约束的组合导航方法包括以下步骤。

步骤一，惯导系统开机，利用陀螺角速率

和加速度计比力

进行粗对准，确定初始姿态。

步骤二，粗对准结束后，根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度。

步骤三，根据

构建系统误差模型。

步骤四，选取惯导位置和速度为卡尔曼滤波的量测量，设定卡尔曼滤波的初始协方差矩阵P₀、系统噪声方差阵Q、系统误差状态初值X₀、量测噪声方差阵R和卡尔曼滤波计算周期T_k。

其中，初始协方差矩阵P₀参考值可设定为

其中，diag表示误差协方差P_k+1/k+1的对角线元素合集，R_M和R_N分别为地球子午面和卯酉面半径，μg为0.00000980147m/s²。

初始协方差矩阵P₀最大值限幅设定为

最小值限幅设定为

上述参数设定仅供参考，不同系统需按照应用需求及产品精度进行综合设计。

步骤五，根据时间更新公式

P_k+1/k＝Φ_k+1,kP_k/kΦ^T _k+1,k+Q_k，新息计算公式

量测更新公式K_k+1＝P_k+1/kH^T _k+1[H_k+1P_k+1/kH^T _k+1+R_k+1]^-1，P_k+1/k+1＝[I-K_k+1H_k+1]P_k+1/k，

不进行量测更新时K_k+1＝0进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束。

在卡尔曼滤波计算过程中，根据惯性器件误差设定卡尔曼滤波器的误差协方差P_k+1/k+1的对角线元素的最大值P_Max和最小值P_min，其中，最大值P_Max和最小值P_min均为一维数组，维数与误差协方差P_k+1/k+1的对角线元素个数以及状态量

的维数相同，最大值P_Max和最小值P_min中的各元素与状态量

中的各变量一一对应，最大值P_Max中的各元素均为元素最大值，最小值P_min中的各元素均为元素最小值；

在进行卡尔曼滤波计算的量测更新过程中，若误差协方差P_k+1/k+1的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k+1的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

在进行卡尔曼滤波计算的时间更新过程中，若误差协方差P_k+1/k的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值。

按以上步骤组合导航2h，其组合导航误差结果如下表1中所示。

表1组合导航过程中位置误差最大值

由结果可以看出，在未进行方差约束时，由于初始0.5h组合导航过程中，可观测性强的天向加速度计P已经快速收敛，在后续1.5h过程中随着加速度计零位的变化，卡尔曼滤波估计的效果变弱，导致天向通道(即高度方向)出现常值误差。采用本方法后，天向通道能够正常估计，高度跟踪良好。本发明能够消除在长时间组合导航过程中由于前期可观测性强的状态量在后期的组合导航过程中，状态量因环境等原因发生变化时滤波器无法跟踪的情况，提升组合导航性能，同时能够避免在受外界量测信息的影响而一直工作在时间更新状态一段时间后再次进行量测更新时出现滤波发散的问题，进一步提升了滤波器的稳定性；解决了传统对准方案的长时间组合导航精度受限及可能出现滤波发散的问题。

综上所述，本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，所述基于卡尔曼滤波估计方差约束的组合导航方法包括：

对惯导系统进行粗对准；

根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；

选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；

进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。

2.根据权利要求1所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值包括：

根据惯性器件误差设定卡尔曼滤波器的误差协方差P_k+1/k+1的对角线元素的最大值P_Max和最小值P_min，其中，最大值P_Max和最小值P_min均为一维数组，维数与误差协方差P_k+1/k+1的对角线元素个数以及状态量

的维数相同，最大值P_Max和最小值P_min中的各元素与状态量

中的各变量一一对应，最大值P_Max中的各元素均为元素最大值，最小值P_min中的各元素均为元素最小值；

在进行卡尔曼滤波计算的量测更新过程中，若误差协方差P_k+1/k+1的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k+1的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

在进行卡尔曼滤波计算的时间更新过程中，若误差协方差P_k+1/k的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

其中，P_k+1/k和P_k+1/k+1分别为k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，

为k时刻系统状态量估计值，k为计算时刻，k＝1,2,...n，n为整数。

3.根据权利要求1或2所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，基于卡尔曼滤波估计方差约束的组合导航方法可根据

构建系统误差模型，其中，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Q_k，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为R_k，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量

δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δV_N、δV_U、δV_E分别为惯导北、天、东速度误差，φ_N、φ_U、φ_E分别为惯导系统地理坐标系内北、天、东三个方向的失准角，

和

分别为x加速度计、y加速度计和z加速度计的加速度计零偏，ε_x、ε_y和ε_z分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。

4.根据权利要求1所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，设定卡尔曼滤波器初值包括：设定卡尔曼滤波的初始协方差矩阵P₀、系统噪声方差阵、系统误差状态初值、量测噪声方差阵和卡尔曼滤波计算周期。

5.根据权利要求1至4中任一项所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，卡尔曼滤波计算中时间更新公式为

P_k+1/k＝Φ_k+1,kP_k/kΦ^T _k+1,k+Q_k，新息计算公式为

量测更新公式为K_k+1＝P_k+1/kH^T _k+1[H_{k+ 1}P_k+1/kH^T _k+1+R_k+1]^-1，P_k+1/k+1＝[I-K_k+1H_k+1]P_k+1/k，

不进行量测更新时K_k+1＝0，其中，Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵，

分别为k时刻系统状态量估计值、k到k+1时刻系统状态量一步预测估计值、k+1时刻系统状态量量测更新后估计值，P_k/k为k时刻的误差协方差值，Z_k+1表示k+1时刻系统的量测量，γ_k表示新息值，K_k+1表示量测更新过程中的增益值，Q_k、R_k+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。

Images