CN109596018B - 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 - Google Patents

Abstract

本发明针对旋转弹发射高过载、高自旋转和高动态的“三高”限制了现有成熟的弹载姿态测量系统无法直接移植应用于旋转弹的问题。采用了基于三轴地磁传感器、两个单轴陀螺仪和卫星接收机组成旋转弹飞行姿态测量方案，针对该测量方案提出了一种适用于旋转弹飞行姿态高精度滤波估计方法，包括如下步骤：（1）、旋转弹飞行姿态组合测量，包括1.1、弹载传感器及安装方式，1.2、弹体飞行姿态高精度组合滤波结构；（2）、旋转弹高精度误差模型，包括2.1、旋转弹姿态误差模型，2.2、旋转弹速度误差模型，2.3、位置误差方程；（3）、弹体姿态高阶非线性滤波模型；（4）、弹体姿态高精度实时滤波算法，进行滤波更新过程。

Description

基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法

技术领域

本发明涉及飞行器或者弹体空间三维姿态的测量方法，具体为一种适用于的旋转弹飞行姿态高精度测量方法。

背景技术

由于旋转弹发射高过载、高自旋转和高动态的“三高”特殊的弹载应用环境，现有成熟的弹载姿态测量系统无法直接移植应用于旋转弹药，存在可靠性差、飞行三维姿态参数测试不全或测量精度较低等问题。为解决上述问题，目前通常采用INS+GPS组合、地磁+GPS或地磁+INS+GPS等组合测量方案，并采用相应滤波算法完成弹体飞行姿态参数的估计，但所采用滤波器结构、滤波模型或滤波算法等存在一定的局限性，所设计滤波器的收敛性、实时性以及估计精度不理想。比如采用现有基于小失准误差角假设的姿态滤波模型，其忽略了高阶项的滤波模型并不能精确地描述旋转弹系统的强非线性特性，这些将直接影响到所设计弹体姿态滤波器的收敛速度与滤波精度，甚至滤波发散等问题，也会造成滤波器大误差参数估计结果。

因此，寻求一种适用于旋转弹飞行姿态高精度测量方法，对解决旋转弹制导化改造中飞行姿态测量难题具有重要的理论价值与现实意义。

发明内容

本发明针对旋转弹发射高过载、高自旋转和高动态的“三高”限制了现有成熟的弹载姿态测量系统无法直接移植应用于旋转弹的问题，采用了基于三轴地磁传感器、两个单轴陀螺仪和卫星接收机组成旋转弹飞行姿态测量方案，针对该测量方案提出了一种适用于旋转弹飞行姿态高精度滤波估计方法。

本发明是采用如下技术方案实现的：

一种基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法，包括如下步骤：

(1)、旋转弹飞行姿态组合测量

1.1、弹载传感器及安装方式

弹载传感器采用三轴地磁传感器、两个单轴陀螺仪和卫星接收机，其中，三轴地磁传感器用于测量弹体内的地磁场矢量信息，其各敏感轴方向Mx、My和Mz与弹体坐标系OX_bY_bZ_b各轴方向完全一致；两个单轴陀螺仪Gy和Gz分别捷联安装于Y_b和Z_b轴上，陀螺仪用于测量弹体Y轴和Z轴角速率信息，X轴向角速率信息由地磁传感器测量信息滤波估算得到；卫星接收机用于测量弹体速度和位置信息，接收机天线环形安装于弹体表面。

1.2、弹体飞行姿态高精度组合滤波结构

组合滤波器由滚转角速率估计滤波器和弹体飞行姿态高精度滤波器两个滤波器组成，分别用于完成弹体滚转速率的估计和弹体飞行姿态参数的高精度估计。

(2)、旋转弹高精度误差模型

2.1、旋转弹姿态误差模型

根据捷联惯性导航理论，弹体坐标系到导航坐标系的姿态四元数

的微分方程表示为：

上式中，

为四元数矩阵，是反对阵矩阵形式：

其中，

为弹体坐标系相对于惯性坐标系的角速度在弹体坐标系上的投影分量

即为导航坐标系相对与惯性坐标系的角速度在导航坐标系上的投影分量

为导航坐标系相对与惯性坐标系的角速度在载体坐标系上的投影分量。

和

分别表示为：

由于角速率分量

是通过传感器测量所得，其测量值

必然存在测量误差

其表示为：

捷联解算所得到的角速度

也存在解算误差，其表示为：

把上式(3)和(4)代入姿态四元数微分方程(1)，得到实际解算时的姿态四元数更微分方程为：

因此，把式(5)减去式(1)得到如下形式：

令

则得到姿态四元数误差方程为：

上式(7)中最后两项表达式满足如下关系：

其中，

和

分别定义为：

上式

和

中，满足如下关系：

把上式关系代入(7)，得姿态四元数误差方程式：

2.2、旋转弹速度误差模型

由比力方程推导出弹体速度误差方程为：

上式中，

为姿态变换矩阵的计算误差，由于

是关于四元数误差

的非线性函数，所以

也是关于

的非线性函数；

为加速度计在导航系下的等效零偏，满足

δK_A和δA分别为加速度计的刻度系数误差矩阵和安装误差矩阵；Vⁿ为导航系下速度分量；δVⁿ为速度误差；

地球自转角速度；

地球自转角速度误差；

为导航坐标系相对地球的角速；

为导航坐标系相对地球的角速误差；f^b为加速度计比力。

若加速度计事前经校准后，可不考虑δK_A和δA，则弹体速度误差方程为：

根据上述式(11)所述转换关系，得弹体姿态变换矩阵有：

因此，姿态变换矩阵的计算误差

为：

如前所述，

代入上式(16)得到

又由于

则

简化为：

当姿态角误差较大时，

对

是非线性的，其中

的计算公式为：

由上述的推导关系可知，速度误差方程体现出是一个非线性的误差方程，其中

项即为其非线性部分，若假设速度误差方程(14)中非线性部分为f₁(x,t)：

在对非线性的速度误差方程进行线性化处理时，f₁(x,t)对

的雅克比矩阵公式为：

上式δu_i(i＝1,2,3,4)表示

的第i行，其中，

和

分别定义为：

因此，综合方程(14)和(18)用于准确的描述弹体在大失准误差角情况下的弹体速度误差传规律，是一种适用于大误差角状态下的弹体速度误差模型。

2.3、位置误差方程

弹体位置误差方程为：

式(24)中，V_E、V_N、V_U分别为弹体沿东、北、天方向的速度；R_M和R_N分别为弹体所在点子午圈和卯酉圈的曲率半径；L为弹体所在点纬度，λ为弹体所在点经度，h为弹体所在点的高度；δV_E,δV_N,δV_U为旋转弹速度误差参数，δh，δλ和δL为位置误差参数。

(3)、弹体姿态高阶非线性滤波模型

选取旋转弹姿态四元数误差δQ、速度误差δv和位置误差δp以及陀螺漂移ε^b、加速度计零偏作为系统的状态变量

其可表示为

选取上述推导所得弹体飞行姿态误差方程(12)、速度误差方程(14)和位置误差方程(24)共同构成了滤波器状态方程组，将其简化为如下一般形式：

上式中f[X(t),t]是关于自变量X(t)的非线性函数，系统过程白噪声w(t)满足E[w(t)]＝0和E[w(t),w^T(τ)]＝Q(t)δ(t-τ)；

选取卫星导航系统测量输出的位置和速度与惯导相应解算的位置和速度相减结果作为组合滤波系统的观测变量Z(t)，其表示为：

上式中L_GPS,λ_GPS,h_GPS和v_i,GPS(i＝N,E,U)分别表示为卫星导航系统测量输出的位置和速度信息，下标为INS变量表示为惯导解算的位置和速度信息；量测噪声v(t)满足E[v(t)]＝0和E[v(t),v^T(τ)]＝R(t)δ(t-τ)；

因此，综合上述状态方程(25)和量测方程(26)共同构成了适用于大误差角状态下的弹体姿态高阶非线性滤波模型。

(4)、弹体姿态高精度实时滤波算法

基于修正系数的SRUKF滤波算法流程如下：

4.1、滤波初始化：

对于任一时段UKF滤波算法计算一个循环的具体步骤：

4.2、对于给定的

和P_k-1(k＝0,2,...n)，首先计算状态一步预测

及一步预报误差协方差阵P_k,k-1，包括sigma点计算及其传播过程：

4.2.1、计算sigma点

i＝0,2,...2n，即为：

4.2.2、计算

i＝0,2,...2n通过状态方程f(·)传播的sigma点，即为：

上式中qr{·}表示对其矩阵进行矩阵的正交三角分解，而

表为其QR分解后的返回值R；公式S′＝cholupdate{S,u,±v}是等价于SS^T＝P，S′＝chol{P±vuu^T}；

4.3、利用UT变换，求sigma点

和P_k,k-1通过量测方程的传播，包括如下两更新计算过程：

4.3.1、首先，计算sigma点

和P_k,k-1通过量测方程h(·)的对X_k的传播：

4.3.2、然后，计算输出的一步提前预测值：

4.4、最后，利用得到的新的量测值，进行滤波更新过程：

本发明方案具有如下优点：

1、本发明采用了适用于该姿态测量方案的基于磁测滚转角信息的弹体飞行姿态高精度组合滤波结构，该组合滤波器由滚转角速率估计滤波器(EKF)和弹体飞行姿态高精度滤波器(SRUKF)组成，分别用于完成弹体滚转速率的估计和弹体飞行姿态高精度估计。

2、本发明采用了适用于大误差角状态的旋转弹用高精度弹体姿态高阶非线性滤波模型。滤波状态方程主要包括采用加性四元数误差模型来描述大误差角状态下旋转弹姿态误差模型、采用基于大误差角来描述旋转弹速度误差模型和位置误差模型。选取卫星导航系统测量输出的位置和速度与惯导解算的位置和速度相减结果作为组合滤波系统的观测量，由上述状态方程和观测方程共同构成高精度弹体姿态高阶滤波模型。本发明采用弹体姿态高阶滤波模型可以更准确地描述其非线性系统的特性，可以提高弹体三维姿态参数估计的精度。

3、本发明采用了基于修正系数的SRUKF弹体姿态滤波估计算法，用于完成旋转弹飞行姿态高精度滤波估计。基于修正系数的SRUKF滤波估计算法与常规SRUKF滤波方法不同之处在于sigma点计算中多了一项修正系数μ，其大小必须通过先验知识来加于选择确定，改进后的修正系数SRUKF滤波算法的收敛发散的问题将得到较好的抑制，同时也兼顾估计算法的实时性与滤波估计精度。

附图说明

图1表示弹载传感器安装方式示意图。

图2表示磁测滚转角信息的姿态组合滤波结构示意图。

具体实施方式

下面结合附图对本发明的具体实施例进行详细说明。

一种基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法，包括如下步骤：

1、旋转弹飞行姿态组合测量方案

1.1、弹载传感器及安装方式

旋转弹姿态测量系统采用地磁/惯性/卫星组合方案，弹载传感器主要有三轴地磁传感器、两个单轴陀螺仪和卫星接收机，各传感器捷联安装方式如图1所示。地磁传感器的用于测量弹体内的地磁场矢量信息，其各敏感轴方向(Mx、My和Mz)与弹体坐标系(OX_bY_bZ_b)各轴方向完全一致；两个单轴陀螺仪(Gy和Gz)分别捷联安装于Y_b和Z_b轴上，陀螺仪用于测量弹体Y轴和Z轴角速率信息，X轴向角速率信息需要由地磁传感器测量信息滤波估算得到；卫星接收机用于测量弹体速度和位置信息，接收机天线环形安装于弹体表面。

1.2、弹体飞行姿态高精度组合滤波结构

基于磁测滚转角信息的弹体飞行姿态高精度组合滤波结构如图2所示，组合滤波器由滚转角速率估计滤波器(EKF)和弹体飞行姿态高精度滤波器(SRUKF)两个滤波器组成，分别用于完成弹体滚转速率的估计和弹体飞行姿态参数的高精度估计。弹体飞行姿态高精度滤波器设计：1)选取弹体姿态角误差、速度误差和位置误差模型以及惯性器件误差模型构建组合滤波系统的状态方程，其中，利用加性四元数误差模型来描述旋转弹姿态误差模型；采用基于大误差角模型来描述旋转弹速度误差模型和位置误差模型。2)选取卫星导航系统测量输出的速度和位置与相对应的捷联惯性解算结果相减作为组合滤波系统的观测量。3)弹体飞行姿态高精度滤波器采用了一种基于修正系数SRUKF滤波算法完成弹体飞行姿态参数的实时高精度估计，该算法兼顾状态参数收敛速度和估计精度问题。

2、旋转弹高精度误差模型

2.1、旋转弹姿态误差模型

考虑到惯性导航系统本身是非线性的，特别是弹轴X轴上没有安装陀螺仪，X轴向角速率是采用地磁信息估算所得，但角速率估计精度并不高，若再采用的较低精度的惯性器件的组合测量方案时，系统的非线性程度更为明显，若采用现有的基于小失准角假设的误差模型，忽略了高阶项的滤波模型并不能精确地描述旋转弹系统的非线性特性，这些将直接影响到所设计弹体姿态滤波器的收敛速度与滤波精度，甚至滤波发散。因此，本发明采用了基于加性四元数误差模型来描述旋转弹飞行姿态误差模型，可以更准确地描述其非线性系统的特性，以提高弹体三维姿态参数估计的精度。根据捷联惯性导航理论，弹体坐标系到导航坐标系的姿态四元数

的微分方程可以表示为：

上式中，

为四元数矩阵，是反对阵矩阵形式：

其中，

为弹体坐标系(b系)相对于惯性坐标系(i系)的角速度在b系上的投影分量

即为导航坐标系(n系)相对与惯性坐标系(i系)的角速度在n系上的投影分量

为导航坐标系相对与惯性坐标系的角速度在载体坐标系上的投影分量；

和

分别表示为：

上式(1)即为适用于任何角度情况下的姿态四元数更新的微分方程。

由于角速率分量

是通过传感器测量所得，其测量值

必然存在测量误差

其可表示为：

捷联解算所得到的角速度

也存在解算误差，其也可以表示为：

把上式(3)和(4)代入姿态四元数微分方程(1)，可得到实际解算时的姿态四元数更微分方程为：

因此，把式(5)减去式(1)可得到如下形式：

令

则可以得到姿态四元数误差方程为：

上式(7)中最后两项表达式满足如下关系：

其中，

和

分别定义为：

上式

和

中，满足如下关系：

把上式关系代入(7)，可得姿态四元数误差方程式：

需要说明的是，本发明所述姿态四元数误差方程式(12)的推导过程，并没有假定是小失准误差角的情况，因此其可用于描述在大失准角误差下的弹体飞行姿态误差传播特性，是一种适用于大误差角状态下的弹体姿态误差模型。

2.2、旋转弹速度误差模型

由比力方程可推导出弹体速度误差方程为：

上式中，

为姿态变换矩阵的计算误差，由于

是关于四元数误差

的非线性函数，所以

也是关于

的非线性函数；

为加速度计在导航系下的等效零偏，满足

δK_A和δA分别为加速度计的刻度系数误差矩阵和安装误差矩阵；Vⁿ为导航系下速度分量。δVⁿ为速度误差；

地球自转角速度；

地球自转角速度误差；

为导航坐标系相对地球的角速；

为导航坐标系相对地球的角速误差；f^b为加速度计比力。

若加速度计事前经通校准后，可不考虑δK_A和δA，则弹体速度误差方程为：

根据上述式(11)所述转换关系，可得弹体姿态变换矩阵有：

因此，姿态变换矩阵的计算误差

可写为：

如前所述，

代入上式(16)可整理得到

又由于

则

可简写为：

当姿态角误差较大时，

对

是非线性的，其中

的计算公式为：

由上述的推导关系可知，速度误差方程体现出是一个非线性的误差方程，其中

项即为其非线性部分，若假设速度误差方程(14)中非线性部分为f₁(x,t)：

在对非线性的速度误差方程进行线性化处理时，f₁(x,t)对

的雅克比矩阵公式为：

上式δu_i(i＝1,2,3,4)表示

的第i行，其中，

和

分别定义为：

因此，综合方程(14)和(18)可以用于准确的描述弹体在大失准误差角情况下的弹体速度误差传规律，是一种适用于大误差角状态下的弹体速度误差模型。

2.3、位置误差方程

弹体位置误差方程为：

式(24)中，V_E、V_N、V_U分别为弹体沿东、北、天方向的速度；R_M和R_N分别为弹体所在点子午圈和卯酉圈的曲率半径；L为弹体所在点纬度，λ为弹体所在点经度，h为弹体所在点的高度；δV_E,δV_N,δV_U为旋转弹速度误差参数，δh，δλ和δL为位置误差参数。

3、弹体姿态高阶非线性滤波模型

选取旋转弹姿态四元数误差δQ、速度误差δv和位置误差δp以及陀螺漂移ε^b、加速度计零偏作为系统的状态变量

其可表示为

选取上述推导所得弹体飞行姿态误差方程(12)、速度误差方程(14)和位置误差方程(24)共同构成了滤波器状态方程组，将其简化为如下一般形式：

上式中f[X(t),t]是关于自变量X(t)的非线性函数，系统过程白噪声w(t)满足E[w(t)]＝0和E[w(t),w^T(τ)]＝Q(t)δ(t-τ)。

选取卫星导航系统测量输出的位置和速度与惯导相应解算的位置和速度相减结果作为组合滤波系统的观测变量Z(t)，其可表示为：

上式中L_GPS,λ_GPS,h_GPS和v_i,GPS(i＝N,E,U)分别表示为卫星导航系统测量输出的位置和速度信息，下标为INS变量表示为惯导解算的位置和速度信息。量测噪声v(t)满足E[v(t)]＝0和E[v(t),v^T(τ)]＝R(t)δ(t-τ)。因此，综合上述状态方程(25)和量测方程(26)共同构成了适用于大误差角状态下的弹体姿态高阶非线性滤波模型。

4、弹体姿态高精度实时滤波算法

上述旋转弹姿态高精度滤波模型是强非线性方程组，基于非线性函数概率密度分布进行近似方法的UKF和SRUKF等滤波方法可以有效避免了对非线性方程近似所产生的模型误差过大而影响滤波精度的问题。但SRUKF与传统的UKF滤波方法在一些情况下容易出现状态协方差矩阵P失去正定性而导致滤波发散现象。针对上述问题，本发明采用基于修正系数的SRUKF弹体姿态滤波算法，以提高滤波算法的收敛速度与参数估计精度。

基于修正系数的SRUKF滤波算法流程如下：

4.1、滤波初始化：

对于任一时段UKF滤波算法计算一个循环的具体步骤：

4.2、对于给定的

和P_k-1(k＝0,2,...n)，首先计算状态一步预测

及一步预报误差协方差阵P_k,k-1，包括sigma点计算及其传播过程：

4.2.1、计算sigma点

i＝0,2,...2n，即为：

4.2.2、计算

i＝0,2,...2n通过状态方程f(·)传播的sigma点，即为：

上式中qr{·}表示对其矩阵进行矩阵的正交三角分解(QR分解)，而

表为其QR分解后的返回值R；公式S′＝cholupdate{S,u,±v}是等价于SS^T＝P，S′＝chol{P±vuu^T}。

4.3、利用UT变换，求sigma点

和P_k,k-1通过量测方程的传播，包括如下两更新计算过程：

4.3.1、首先，计算sigma点

和P_k,k-1通过量测方程h(·)的对X_k的传播：

4.3.2、然后，计算输出的一步提前预测值：

4.4、最后，利用得到的新的量测值，进行滤波更新过程：

上述经修正系数的SRUKF滤波估计算法与常规SRUKF滤波方法不同之处在于sigma点计算，在sigma点计算公式(28)中多了一项修正系数μ，其大小必须通过先验知识来加于选择。由于修正系数满足μ＞1，所以在时间更新后sigma点

的误差方差S_k,k-1和提前预测值

的误差方差S_zk都必将会增大，但是其观测噪声方差阵R_k不变，因此，量测修正的增益K_k也将随之增大，这意味着利用新量测值的权重加大，也即降低了陈旧量测信息对估计值的影响，所以，改进后的修正系数SRUKF滤波算法的收敛发散的问题将得到较好的抑制。通过上述基于修正系数的SRUKF滤波算法流程，最终实现旋转弹飞行参数的高精度估计。

应当指出，对于本技术领域的一般技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和应用，这些改进和应用也视为本发明的保护范围。

Claims (1)

1.一种基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法，其特征在于：包括如下步骤：

(1)、旋转弹飞行姿态组合测量

1.1、弹载传感器及安装方式

弹载传感器采用三轴地磁传感器、两个单轴陀螺仪和卫星接收机，其中，三轴地磁传感器用于测量弹体内的地磁场矢量信息，其各敏感轴方向Mx、My和Mz与弹体坐标系OX_bY_bZ_b各轴方向完全一致；两个单轴陀螺仪Gy和Gz分别捷联安装于Y_b和Z_b轴上，陀螺仪用于测量弹体Y轴和Z轴角速率信息，X轴向角速率信息由地磁传感器测量信息滤波估算得到；卫星接收机用于测量弹体速度和位置信息，接收机天线环形安装于弹体表面；

1.2、弹体飞行姿态高精度组合滤波结构

组合滤波器由滚转角速率估计滤波器和弹体飞行姿态高精度滤波器两个滤波器组成，分别用于完成弹体滚转速率的估计和弹体飞行姿态参数的高精度估计；

(2)、旋转弹高精度误差模型

2.1、旋转弹姿态误差模型

根据捷联惯性导航理论，弹体坐标系到导航坐标系的姿态四元数

的微分方程表示为：

上式中，

为四元数矩阵，是反对阵矩阵形式：

其中，

为弹体坐标系相对于惯性坐标系的角速度在弹体坐标系上的投影分量

即为导航坐标系相对与惯性坐标系的角速度在导航坐标系上的投影分量

和

分别表示为：

由于角速率分量

是通过传感器测量所得，其测量值

必然存在测量误差

其表示为：

捷联解算所得到的角速度

也存在解算误差，其表示为：

把上式(3)和(4)代入姿态四元数微分方程(1)，得到实际解算时的姿态四元数更微分方程为：

因此，把式(5)减去式(1)得到如下形式：

令

则得到姿态四元数误差方程为：

上式(7)中最后两项表达式满足如下关系：

其中，

和

分别定义为：

上式

和

中，满足如下关系：

把上式关系代入(7)，得姿态四元数误差方程式：

2.2、旋转弹速度误差模型

由比力方程推导出弹体速度误差方程为：

上式中，

为姿态变换矩阵的计算误差，由于

是关于四元数误差

的非线性函数，所以

也是关于

的非线性函数；

为加速度计在导航系下的等效零偏，满足

δK_A和δA分别为加速度计的刻度系数误差矩阵和安装误差矩阵；Vⁿ为导航系下速度分量；δVⁿ为速度误差；

地球自转角速度；

地球自转角速度误差；

为导航坐标系相对地球的角速；

为导航坐标系相对地球的角速误差；f^b为加速度计比力；

若加速度计事前经校准后，则不考虑δK_A和δA，则弹体速度误差方程为：

根据上述式(11)所述转换关系，得弹体姿态变换矩阵有：

因此，姿态变换矩阵的计算误差

为：

如前所述，

代入上式(16)得到

又由于

则

简化为：

对

是非线性的，其中

的计算公式为：

由上述的推导关系可知，速度误差方程体现出是一个非线性的误差方程，其中

项即为其非线性部分，若假设速度误差方程(14)中非线性部分为f₁(x,t)：

在对非线性的速度误差方程进行线性化处理时，f₁(x,t)对

的雅克比矩阵公式为：

上式δu_i(i＝1,2,3,4)表示

的第i行，其中，

和

分别定义为：

综合方程(14)和(18)为适用于大误差角状态下的弹体速度误差模型；

2.3、位置误差方程

弹体位置误差方程为：

式(24)中，V_E、V_N、V_U分别为弹体沿东、北、天方向的速度；R_M和R_N分别为弹体所在点子午圈和卯酉圈的曲率半径；L为弹体所在点纬度，λ为弹体所在点经度，h为弹体所在点的高度；δV_E,δV_N,δV_U为旋转弹速度误差参数，δh，δλ和δL为位置误差参数；

(3)、弹体姿态高阶非线性滤波模型

选取旋转弹姿态四元数误差δQ、速度误差δv和位置误差δp以及陀螺漂移ε^b、加速度计零偏作为系统的状态变量

其表示为

选取上述推导所得弹体飞行姿态误差方程(12)、速度误差方程(14)和位置误差方程(24)共同构成了滤波器状态方程组，将其简化为如下一般形式：

上式中f[X(t),t]是关于自变量X(t)的非线性函数，系统过程白噪声w(t)满足E[w(t)]＝0和E[w(t),w^T(τ)]＝Q(t)δ(t-τ)；

选取卫星导航系统测量输出的位置和速度与惯导相应解算的位置和速度相减结果作为组合滤波系统的观测变量Z(t)，其表示为：

上式中L_GPS,λ_GPS,h_GPS和v_i,GPS(i＝N,E,U)分别表示为卫星导航系统测量输出的位置和速度信息，下标为INS变量表示为惯导解算的位置和速度信息；量测噪声v(t)满足E[v(t)]＝0和E[v(t),v^T(τ)]＝R(t)δ(t-τ)；

综合上述状态方程(25)和量测方程(26)共同构成适用于大误差角状态下的弹体姿态高阶非线性滤波模型；

(4)、弹体姿态高精度实时滤波算法

基于修正系数的SRUKF滤波算法流程如下：

4.1、滤波初始化：

对于任一时段UKF滤波算法计算一个循环的具体步骤：

4.2、对于给定的

和P_k-1(k＝0,2,...n)，首先计算状态一步预测

及一步预报误差协方差阵P_k,k-1，包括sigma点计算及其传播过程：

4.2.1、计算sigma点

即为：

4.2.2、计算

通过状态方程f(·)传播的sigma点，即为：

上式中qr{·}表示对其矩阵进行矩阵的正交三角分解，而

表为其QR分解后的返回值R；公式S′＝cholupdate{S,u,±v}是等价于SS^T＝P，S′＝chol{P±vuu^T}；

4.3、利用UT变换，求sigma点

和P_k,k-1通过量测方程的传播，包括如下两更新计算过程：

4.3.1、首先，计算sigma点

和P_k,k-1通过量测方程h(·)的对X_k的传播：

4.3.2、然后，计算输出的一步提前预测值：

4.4、最后，利用得到的新的量测值，进行滤波更新过程：

Images