CN104655152B - 一种基于联邦滤波的机载分布式pos实时传递对准方法 - Google Patents

Abstract

一种基于联邦滤波的机载分布式POS实时传递对准方法。机载分布式POS由一个高精度主POS和多个子IMU组成。针对机载分布式POS实时传递对准对实时性要求高的问题，采用基于分散结构的联邦滤波进行传递对准。首先，进行联邦滤波器结构设计，包括两个子滤波器和一个主滤波器；其次，建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；再次，由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置；最后，利用重置后的误差状态量计算出更加准确的子IMU的位置、速度和姿态。

Description

一种基于联邦滤波的机载分布式POS实时传递对准方法

技术领域

本发明涉及一种基于联邦滤波的机载分布式POS实时传递对准方法，可用于机载或舰载分布式POS系统中子IMU的位置、速度和姿态解算，特别适合于对实时性要求较高的情况。

背景技术

目前，机载多任务遥感载荷高精度对地观测已经成为空基对地观测的重要发展方向，如集成高分辨率测绘相机、成像光谱仪、大视场红外扫描仪、合成孔径雷达(SyntheticAperture Radar，SAR)于同一载机的多任务载荷，机载分布式阵列天线SAR和柔性多基线干涉SAR等。机载多任务遥感载荷高精度对地观测系统要实现高精度成像，需要获取各载荷安置点的高精度运动参数。

位置姿态测量系统(Position and Orientation System，POS)包括惯性测量单元(Inertial Measurement Unit，IMU)、全球定位系统(Global Position System，GPS)接收机、POS计算机(POS Computer System，PCS)和后处理软件。POS系统能够为载机及其遥感设备提供高精度的位置、速度、姿态等信息。但是对于机载多任务遥感载荷对地观测系统，传统的单一POS已经无法满足不同载荷安置点的高精度运动参数测量需求。因此，必须建立起高精度机载分布式POS系统，从而为机载对地观测系统中遥感载荷各安置点提供高精度的运动参数。

机载分布式POS系统一般由一个高精度主POS和多个分布在机体(包括机翼)上的低精度子IMU组成。主POS通过传递对准将高精度位置、速度、姿态等运动参数传递给子IMU，从而获得各子节点的高精度运动参数信息。机载对地观测成像有实时成像和离线成像两种工作模式，在实时成像的时候要求POS做实时传递对准，且对实时性的要求较高。但是在实际飞行中机体存在弹性变形，且主、子系统之间存在安装误差角，因此采用传统的基于集中式滤波结构的传递对准数学模型，需要将弹性变形角和安装误差角均扩充为状态变量进行估计，子IMU的误差包括速度误差、姿态误差、位置误差和惯性仪表误差，这样完整的传递对准状态变量维数通常高达24维甚至更高，而传递对准滤波算法计算量与状态变量维数成正比(p³+pq²，其中，p为系统模型的状态变量维数，q为观测量维数)，从而导致实时导航解算计算量很大，难以满足实时传递对准要求。因此，提高机载分布式POS实时传递对准的实时性有着十分重要的意义。

联邦滤波是一种并行的分散化滤波，将高维状态变量分散到各子滤波器中，降低了滤波器的维数，从而提高运算速度。但是目前基于联邦滤波的传递对准方法大多应用于舰载和机载战术武器的初始对准中，即只需通过传递对准获得舰载和机载武器的初始位置、姿态和速度等初始信息，因此对准时间比较短。子系统的姿态误差方程和速度误差方程中和的值比较小，因此可以将其忽略以简化误差方程，从而使状态变量维数得到降低(王勇军,徐景硕,高扬.舰载机惯导快速传递对准的联邦滤波器设计[J].压电与声光.2014,36(1):42-46.；宋丽君,秦永元.联邦模糊自适应卡尔曼滤波在速度+姿态传递对准中的应用[J].测控技术.2013,32(5):130-133.)。但是在机载多任务遥感载荷实时成像中，整个作业过程都需要利用主POS系统的高精度运动参数对子IMU进行传递对准，来获取子IMU的速度、位置和姿态信息，且要求传递对准算法既要满足实时性要求又要满足运动参数的高精度测量要求。此时，和对传递对准的精度影响就不能忽略，而和中含有速度误差和位置误差项，考虑这些因素又会增加状态维数，增加计算量。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提出一种基于联邦滤波的机载分布式POS实时传递对准方法，该方法可降低滤波器维数、减小计算量、提高运算速度，从而满足机载多任务遥感载荷对地观测用分布式POS对实时性的要求。

本发明的技术解决方案为：一种基于联邦滤波的机载分布式POS实时传递对准方法。其具体步骤如下：

(1)进行联邦滤波器结构设计，包括两个子滤波器和一个主滤波器；

在子滤波器1中子IMU和主POS采用速度匹配方式进行局部滤波得到局部状态估计值和协方差阵P_v；在子滤波器2中子IMU与主POS采用姿态匹配方式进行局部滤波得到局部状态估计值和协方差阵P_a；最后将各子滤波器公共状态的估计值和协方差阵P_cv和P_ca送入主滤波器并与主滤波器的状态估计值和协方差阵P_m进行融合，得到全局最优估计值和协方差阵P_g；

(2)建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；

1)选取子滤波器的状态变量

相关参考坐标系的定义包括：记i为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，分别用n和n₁表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用b_m和b分别代表主POS和子IMU的载体坐标系；

①子滤波器1的状态变量

其中，和分别为子IMU在子滤波器1中的东向、北向和天向失准角，下标E、N和U分别表示东向、北向和天向；δV_E、δV_N和δV_U分别为子IMU的东向、北向和天向速度误差；和分别为子IMU在子滤波器1中载体坐标系x轴、y轴和z轴的陀螺常值漂移；和分别为子IMU载体坐标系x轴、y轴和z轴的加速度计常值偏置；

②子滤波器2的状态变量

由于安装误差角和弹性变形角对子IMU的姿态角精度影响较大，所以在子滤波器2中将安装误差角和弹性变形角以及弹性变形角速率均扩充为状态变量，子滤波器2的状态变量X_a共15维，具体为；

其中，和分别为子IMU在子滤波器2中的东向、北向和天向失准角；和分别为子IMU在子滤波器2中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；ρ_x、ρ_y和ρ_z分别为子IMU相对于主POS的安装误差角在子IMU载体坐标系x轴、y轴和z轴上的分量；θ_x、θ_y和θ_z分别为子IMU相对于主POS的弹性变形角在子IMU载体坐标系x轴、y轴和z轴上的分量，和分别为对应的弹性变形角速率；

2)建立子滤波器的数学模型

①建立子滤波器1的数学模型

a)状态方程

子滤波器1的系统状态方程如下：

其中，F_v、G_v和ω_v分别为子滤波器1的状态转移矩阵、系统噪声驱动阵和系统噪声向量；和分别为子IMU载体坐标系x轴、y轴、z轴陀螺和加速度计的随机误差，不包括随机常值误差，其方差阵Q_v包含Q_av和Q_cv两部分，其中Q_cv＝1/β₁Q，β₁＝1/3，Q为陀螺随机误差噪声方差阵，根据陀螺的噪声水平进行选取，Q_av为加速度计随机误差噪声方差阵，根据加速度计的噪声水平进行选取；

根据对子滤波器1中状态变量的选取可知，子滤波器1中的误差方程包括姿态误差方程、速度误差方程和惯性仪表误差方程，分别为：

其中，为子IMU导航坐标系相对于惯性坐标系的旋转角速度在其导航坐标系中的投影；为的计算误差；为子IMU载体坐标系到其导航坐标系的转换矩阵；为陀螺随机误差，包括随机常值漂移和白噪声漂移；V_E、V_N和V_U分别为子IMU导航坐标系的东向、北向和天向速度；f_E、f_N和f_U分别为子IMU在其导航坐标系中东向、北向和天向加速度；为地球坐标系相对惯性坐标系的旋转角速度在子IMU导航坐标系的投影；为子IMU导航坐标系相对地球坐标系的旋转角速度在其导航坐标系的投影；和分别为和的计算误差；为加速度计随机误差，包括随机常值偏置和白噪声偏置；

根据上述误差方程确定的F_v和G_v分别为：

其中，ω_ie为地球自转角速度；R_M和R_N分别为地球沿子午圈和卯酉圈的主曲率半径；L和H分别为子IMU的纬度和高度；T为滤波周期；式中， A₅＝-2ω_ieV_EsinL以及

b)量测方程

子滤波器1的量测方程为：

Z_v＝H_vX_v+v_v

其中，系统观测量为：Z_v＝[δV_E′δV_N′δV_U′]^T，δV_E′、δV_N′和δV_U′分别为主POS经杆臂速度补偿后与子IMU之间的东向、北向和天向速度之差，杆臂速度V_r ⁿ的计算公式为：

其中，为主POS载体坐标系到其导航坐标系的转换矩阵；为主POS载体坐标系相对于惯性坐标系的旋转角速度在其载体坐标系中的投影；为主、子系统之间的杆臂在主POS载体坐标系上的投影；系统量测噪声和分别为主POS东向、北向和天向速度的量测噪声，v_v为零均值高斯白噪声，其方差阵R_v由主POS的速度精度决定，量测矩阵H_v为：

H_v＝[0_3×3 I_3×3 0_3×6]

②建立子滤波器2的数学模型

a)状态方程

子滤波器2的系统状态方程如下：

其中，F_a、G_a、ω_a和B_a分别为子滤波器2的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值漂移，η_x、η_y和η_z分别为子IMU载体坐标系x轴、y轴和z轴弹性变形角二阶Markov过程白噪声，ω_a对应的噪声方差阵Q_a包括Q_ca和Q_η两部分，其中Q_ca＝1/β₂Q，β₂＝1/3，Q_η为二阶Markov过程白噪声的方差；U_a为已知的非随机函数，U_a＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对子滤波器2中状态变量的选取可知，子滤波器2中的误差方程包括姿态误差方程、陀螺误差方程、安装误差角方程和弹性变形角方程，其中姿态误差方程和陀螺误差方程表达式与子滤波器1中的姿态误差方程和陀螺误差方程相同，安装误差角方程和弹性变形角方程分别为：

其中，ρ＝[ρ_x ρ_y ρ_z]^T；θ_J用二阶Markov过程模型来描述；β_J＝2.146/τ_J，τ_J为二阶Markov过程相关时间；η_J为零均值白噪声，其方差满足：

其中，σ_J ²为弹性变形角θ_J的方差，

根据上述误差方程确定的F_a、G_a和B_a分别为：

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

b)量测方程

子滤波器2的量测方程为：

Z_a＝H_aX_a+v_a

其中，系统观测量为：Z_a＝[δψ δθ δγ]^T，δψ、δθ和δγ分别为子IMU与主POS之间的航向角、俯仰角和横滚角之差；系统量测噪声为v_a＝[v_δψ v_δθ v_δγ]^T，v_δψ、v_δθ、v_δγ分别为主POS航向角、俯仰角和横滚角的测量噪声，v_a取为零均值高斯白噪声，其方差阵R_a由主POS的姿态精度决定，量测矩阵H_a为：

H_a＝[H₁ 0_3×3 H₂ H₃ 0_3×3]

其中，为主POS载体坐标系到其导航坐标系的转换矩阵，j＝1,2,3，l＝1,2,3，表示T_a的第j行、第l列元素；

3)计算局部估计值

根据子滤波器1和子滤波器2的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，并将子滤波器1和子滤波器2中的公共状态变量，即子IMU的失准角和陀螺的常值漂移，估计值分别记为和对应协方差记为P_cv和P_ca；

(3)由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置；

1)主滤波器进行卡尔曼滤波

①选取主滤波器状态变量

将子IMU的失准角和陀螺常值漂移作为主滤波器状态变量：

②主滤波器数学模型

由于主滤波器没有观测信息，所以没有观测方程只有状态方程，其状态方程为：

其中，F_m、G_m、ω_m和B_m分别为主滤波器的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值误差，ω_m取为零均值白噪声，其方差阵Q_m＝1/β_mQ，β_m＝1/3；U_m为已知的非随机函数，U_m＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对主滤波器状态变量的选取可知，主滤波器中的误差方程包括姿态误差方程和陀螺误差方程，二者表达式均与子滤波器1的姿态误差方程和陀螺误差方程相同，因此F_m、G_m和B_m的表达式分别为：

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

③主滤波器根据其数学模型进行卡尔曼滤波，由于主滤波器中只有状态方程，因此只进行时间更新，并将状态变量估计值和协方差阵分别记为和P_m；

2)计算全局滤波解

将主、子滤波器中的公共状态变量估计值和协方差阵按照下式进行融合，即可得到全局最优估计：

其中，和P_g分别表示全局状态变量估计值和协方差阵，是由子滤波器1和子滤波器2中局部状态变量估计值和协方差阵在主滤波器中进行融合后得到的；

3)对主、子滤波器公共状态变量估计值和协方差阵进行重置

利用全局滤波解对主、子滤波器中的失准角和陀螺常值漂移的估计值和协方差阵进行重置，如下式所示：

其中，和分别表示经过重置后子滤波器1、子滤波器2和主滤波器中的公共状态变量估计值；P_cv′、P_ca′和P_m′分别表示经过重置后子滤波器1、子滤波器2和主滤波器中公共状态变量的协方差阵；

(4)利用步骤(3)中重置后的误差状态量计算出更加准确的子IMU的位置、速度和姿态，不断重复步骤(2)和步骤(3)直至传递对准结束；

1)速度修正

其中，V_E′、V_N′和V_U′分别为修正后子IMU的东向、北向和天向速度；

2)位置修正

其中，λ为捷联解算得到的子IMU的经度；L′、λ′和H′分别为修正后子IMU的纬度、经度和高度；δL、δλ和δH分别为子IMU的纬度、经度和高度误差，由子滤波器1估计出的速度误差通过积分得到，表达式如下所示：

3)姿态修正

利用重置后中的失准角来计算子IMU导航坐标系n₁与计算导航坐标系n₁′间的转换矩阵

修正后的转换矩阵为：

其中，为子IMU进行捷联解算后得到的转换矩阵；

利用修正后的转换矩阵计算子IMU的姿态角，包括航向角、俯仰角和横滚角；

4)将子滤波器1和子滤波器2状态变量中的失准角和速度误差变量置零，并将修正后子IMU的速度、位置和姿态作为下一次解算初始值，重复步骤(2)和步骤(3)，直至循环结束完成传递对准。

本发明与现有技术相比的优点在于：

针对机载分布式POS实时传递对准存在滤波器状态变量维数高导致运算速度低的问题，利用基于联邦滤波结构的传递对准方法，将高维状态变量分配到两个子滤波器中，降低了滤波器状态变量维数，并利用速度误差积分的方式来表示位置误差，克服了传统方法中将含有速度误差和位置误差项的和直接忽略导致对准精度下降的不足，从而在保证了传递对准精度的同时，满足了机载分布式POS实时传递对准对高实时性的要求。

附图说明

图1为本发明的流程图；

图2为基于联邦滤波的机载分布式POS实时传递对准结构图。

具体实施方式

如图1所示，本发明的具体方法实施如下：

1、进行基于联邦滤波的传递对准结构设计

将子IMU看作公共参考系统，分别设计两个子滤波器和一个主滤波器，基于联邦滤波的机载分布式POS实时传递对准结构图如图2所示。其中，在子滤波器1中子IMU和主POS采用速度匹配方式进行局部滤波得到局部状态估计值和协方差阵P_v；在子滤波器2中子IMU与主POS采用姿态匹配方式进行局部滤波得到局部状态估计值和协方差阵P_a；最后将各子滤波器公共状态的估计值和协方差阵P_cv和P_ca送入主滤波器并与主滤波器的状态估计值和协方差阵P_m进行融合，得到全局最优估计值和协方差阵P_g。

2、建立两个子滤波器的数学模型并分别进行卡尔曼滤波

(1)选取子滤波器的状态变量

相关参考坐标系的定义包括：记i为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，分别用n和n₁表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用b_m和b分别代表主POS和子IMU的载体坐标系；

1)子滤波器1的状态变量

其中，和分别为子IMU在子滤波器1中的东向、北向和天向失准角，下标E、N和U分别表示东向、北向和天向；δV_E、δV_N和δV_U分别为子IMU的东向、北向和天向速度误差；和分别为子IMU在子滤波器1中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；和分别为子IMU载体坐标系x轴、y轴和z轴的加速度计常值偏置；

2)子滤波器2的状态变量

由于安装误差角和弹性变形角对子IMU的姿态角精度影响较大，所以在子滤波器2中将安装误差角和弹性变形角以及弹性变形角速率均扩充为状态变量，子滤波器2的状态变量X_a共15维，具体为；

其中，和分别为子IMU在子滤波器2中的东向、北向和天向失准角；和分别为子IMU在子滤波器2中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；ρ_x、ρ_y和ρ_z分别为子IMU相对于主POS的安装误差角在子IMU载体坐标系x轴、y轴和z轴上的分量；θ_x、θ_y和θ_z分别为子IMU相对于主POS的弹性变形角在子IMU载体坐标系x轴、y轴和z轴上的分量，和分别为对应的弹性变形角速率；

(2)建立子滤波器的数学模型

1)建立子滤波器1的数学模型

①状态方程

子滤波器1的系统状态方程如下：

其中，F_v、G_v和ω_v分别为子滤波器1的状态转移矩阵、系统噪声驱动阵和系统噪声向量；和分别为子IMU载体坐标系x轴、y轴、z轴陀螺和加速度计的随机误差，不包括随机常值误差，其方差阵Q_v包含Q_av和Q_cv两部分，其中Q_cv＝1/β₁Q，β₁＝1/3，Q为陀螺随机误差噪声方差阵，根据陀螺的噪声水平进行选取，Q_av为加速度计随机误差噪声方差阵，根据加速度计的噪声水平进行选取；

根据对子滤波器1中状态变量的选取可知，子滤波器1中的误差方程包括姿态误差方程、速度误差方程和惯性仪表误差方程，分别为：

其中，为子IMU导航坐标系相对于惯性坐标系的旋转角速度在其导航坐标系中的投影；为的计算误差；为子IMU载体坐标系到其导航坐标系的转换矩阵；为陀螺随机误差，包括随机常值漂移和白噪声漂移；V_E、V_N和V_U分别为子IMU导航坐标系的东向、北向和天向速度；f_E、f_N和f_U分别为子IMU在其导航坐标系中东向、北向和天向加速度；为地球坐标系相对惯性坐标系的旋转角速度在子IMU导航坐标系的投影；为子IMU导航坐标系相对地球坐标系的旋转角速度在其导航坐标系的投影；和分别为和的计算误差；为加速度计随机误差，包括随机常值偏置和白噪声偏置；

根据上述误差方程确定的F_v和G_v分别为：

其中，ω_ie为地球自转角速度；R_M和R_N分别为地球沿子午圈和卯酉圈的主曲率半径；L和H分别为子IMU的纬度和高度；T为滤波周期；式中，

A₅＝-2ω_ieV_E sinL (18)

②量测方程

子滤波器1的量测方程为：

Z_v＝H_vX_v+v_v (20)

其中，系统观测量为：Z_v＝[δV_E′δV_N′δV_U′]^T，δV_E′、δV_N′和δV_U′分别为主POS经杆臂速度补偿后与子IMU之间的东向、北向和天向速度之差，杆臂速度V_r ⁿ的计算公式为：

其中，为主POS载体坐标系到其导航坐标系的转换矩阵；为主POS载体坐标系相对于惯性坐标系的旋转角速度在其载体坐标系中的投影；

为主、子系统之间的杆臂在主POS载体坐标系上的投影；系统量测噪声和分别为主POS东向、北向和天向速度的量测噪声，v_v为零均值高斯白噪声，其方差阵R_v由主POS的速度精度决定，量测矩阵H_v为：

H_v＝[0_3×3 I_3×3 0_3×6] (22)

2)建立子滤波器2的数学模型

①状态方程

子滤波器2的系统状态方程如下：

其中，F_a、G_a、ω_a和B_a分别为子滤波器2的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值漂移，η_x、η_y和η_z分别为子IMU载体坐标系x轴、y轴和z轴弹性变形角二阶Markov过程白噪声，ω_a对应的噪声方差阵Q_a包括Q_ca和Q_η两部分，其中Q_ca＝1/β₂Q，β₂＝1/3，Q_η为二阶Markov过程白噪声的方差；U_a为已知的非随机函数，U_a＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对子滤波器2中状态变量的选取可知，子滤波器2中的误差方程包括姿态误差方程、陀螺误差方程、安装误差角方程和弹性变形角方程，其中姿态误差方程和陀螺误差方程表达式与子滤波器1中的姿态误差方程和陀螺误差方程相同，安装误差角方程和弹性变形角方程分别为：

其中，ρ＝[ρ_x ρ_y ρ_z]^T；θ_J用二阶Markov过程模型来描述；β_J＝2.146/τ_J，τ_J为二阶Markov过程相关时间；η_J为零均值白噪声，其方差Q_ηJ满足：

其中，σ_J ²为弹性变形角θ_J的方差，

根据上述误差方程确定的F_a、G_a和B_a分别为：

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

②量测方程

子滤波器2的量测方程为：

Z_a＝H_aX_a+v_a (32)

其中，系统观测量为：Z_a＝[δψ δθ δγ]^T，δψ、δθ和δγ分别为子IMU与主POS之间的航向角、俯仰角和横滚角之差；系统量测噪声为v_a＝[v_δψ v_δθ v_δγ]^T，v_δψ、v_δθ、v_δγ分别为主POS航向角、俯仰角和横滚角的测量噪声，v_a取为零均值高斯白噪声，其方差阵R_a由主POS的姿态精度决定，量测矩阵H_a为：

H_a＝[H₁ 0_3×3 H₂ H₃ 0_3×3] (33)

其中，为主POS载体坐标系到其导航坐标系的转换矩阵，j＝1,2,3，l＝1,2,3，表示T_a的第j行、第l列元素；

(3)计算局部估计值

根据子滤波器1和子滤波器2的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，将子滤波器1和子滤波器2中的公共状态变量，即子IMU的失准角和陀螺的常值漂移，估计值分别记为和对应协方差记为P_cv和P_ca。

3、由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置

由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置的步骤为：

(1)主滤波器进行卡尔曼滤波

1)选取主滤波器状态变量

将子IMU的失准角和陀螺常值漂移作为主滤波器状态变量：

2)主滤波器数学模型

由于主滤波器没有观测信息，所以没有观测方程只有状态方程，其状态方程为：

其中，F_m、G_m、ω_m和B_m分别为主滤波器的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值误差，ω_m取为零均值白噪声，其方差阵Q_m＝1/β_mQ，β_m＝1/3；U_m为已知的非随机函数，U_m＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对主滤波器状态变量的选取可知，主滤波器中的误差方程包括姿态误差方程和陀螺误差方程，二者表达式均与子滤波器1的姿态误差方程和陀螺误差方程相同，因此F_m、G_m和B_m的表达式分别为：

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

3)主滤波器根据其数学模型进行卡尔曼滤波，由于主滤波器中只有状态方程，因此只进行时间更新，并将状态变量估计值和协方差阵分别记为和P_m；

(2)计算全局滤波解

将主、子滤波器中的公共状态变量估计值和协方差阵按照下式进行融合，即可得到全局最优估计：

其中，和P_g分别表示全局状态变量估计值和协方差阵，是由子滤波器1和子滤波器2中局部状态变量估计值和协方差阵在主滤波器中进行融合后得到的；

(3)对主、子滤波器公共状态变量估计值和协方差阵进行重置

利用全局滤波解对主、子滤波器中的失准角和陀螺常值漂移的估计值和协方差阵进行重置，如下式所示：

其中，和分别表示经过重置后子滤波器1、子滤波器2和主滤波器中的公共状态变量估计值；P_cv′、P_ca′和P_m′分别表示经过重置后子滤波器1、子滤波器2和主滤波器中公共状态变量的协方差阵。

4、修正子IMU捷联解算结果得到子IMU的位置、速度和姿态

利用重置的误差状态量修正子IMU捷联解算结果，从而计算出更加准确的子IMU位置、速度和姿态，具体步骤如下：

(1)速度修正

其中，V_E′、V_N′和V_U′分别为修正后子IMU的东向、北向和天向速度；

(2)位置修正

其中，λ为捷联解算得到的子IMU的经度；L′、λ′和H′分别为修正后子IMU的纬度、经度和高度；δL、δλ和δH分别为子IMU的纬度、经度和高度误差，由子滤波器1估计出的速度误差通过积分得到，表达式如下所示：

(3)姿态修正

利用重置后中的失准角来计算子IMU导航坐标系n₁与计算导航坐标系n₁′间的转换矩阵

修正后的转换矩阵为：

其中，为子IMU进行捷联解算后得到的转换矩阵；

利用修正后的转换矩阵计算子IMU的姿态角，包括航向角ψ、俯仰角θ和横滚角γ；

将记为

其中，T_rs，r＝1,2,3，s＝1,2,3，为矩阵中第r行、第s列的元素；则子IMU航向角ψ、俯仰角θ和横滚角γ的主值，即ψ_主、θ_主和γ_主分别为：

由于航向角ψ、俯仰角θ和横滚角γ的取值范围分别定义为[0，2π]、 和[-π，+π]，那么，ψ、θ和γ的真值可由下式确定：

θ＝θ_主 (53)

通过上述对子IMU的速度、位置和姿态进行修正，能够得到更加准确的子IMU的速度、位置和姿态信息；

(4)将子滤波器1和子滤波器2状态变量中的失准角和速度误差变量置零，并将修正后子IMU的速度、位置和姿态作为下一次解算初始值，重复步骤2和步骤3，直至循环结束完成传递对准。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于联邦滤波的机载分布式POS实时传递对准方法，其特征在于具体步骤为：

(1)进行联邦滤波器结构设计，包括两个子滤波器和一个主滤波器；

在子滤波器1中子IMU和主POS采用速度匹配方式进行局部滤波得到局部状态估计值和协方差阵P_v；在子滤波器2中子IMU与主POS采用姿态匹配方式进行局部滤波得到局部状态估计值和协方差阵P_a；最后将各子滤波器公共状态的估计值和协方差阵P_cv和P_ca送入主滤波器并与主滤波器的状态估计值和协方差阵P_m进行融合，得到全局最优估计值和协方差阵P_g；

(2)建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；

1)选取子滤波器的状态变量

相关参考坐标系的定义包括：记i为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，分别用n和n₁表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用b_m和b分别代表主POS和子IMU的载体坐标系；

①子滤波器1的状态变量

$X_v={\[{\mathrm{\φ}}_E^v,{\mathrm{\φ}}_N^v,{\mathrm{\φ}}_U^v,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\ϵ}}_x^v,{\mathrm{\ϵ}}_y^v,{\mathrm{\ϵ}}_z^v,{\▿}_x,{\▿}_y,{\▿}_z\]}^T$

其中，和分别为子IMU在子滤波器1中的东向、北向和天向失准角，下标E、N和U分别表示东向、北向和天向；δV_E、δV_N和δV_U分别为子IMU的东向、北向和天向速度误差；和分别为子IMU在子滤波器1中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；和分别为子IMU载体坐标系x轴、y轴和z轴的加速度计常值偏置；

②子滤波器2的状态变量

由于安装误差角和弹性变形角对子IMU的姿态角精度影响较大，所以在子滤波器2中将安装误差角和弹性变形角以及弹性变形角速率均扩充为状态变量，子滤波器2的状态变量X_a共15维，具体为；

$X_a=\[{({\mathrm{\φ}}_E^a,{\mathrm{\φ}}_N^a,{\mathrm{\φ}}_U^a,{\mathrm{\ϵ}}_x^a,{\mathrm{\ϵ}}_y^a,{\mathrm{\ϵ}}_z^a,{\mathrm{\ρ}}_x,{\mathrm{\ρ}}_y,{\mathrm{\ρ}}_z,{\mathrm{\θ}}_x,{\mathrm{\θ}}_y,{\mathrm{\θ}}_z,{\stackrel{\·}{\mathrm{\θ}}}_x,{\stackrel{\·}{\mathrm{\θ}}}_y,{\stackrel{\·}{\mathrm{\θ}}}_z\]}^T$

其中，和分别为子IMU在子滤波器2中的东向、北向和天向失准角；和分别为子IMU在子滤波器2中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；ρ_x、ρ_y和ρ_z分别为子IMU相对于主POS的安装误差角在子IMU载体坐标系x轴、y轴和z轴上的分量；θ_x、θ_y和θ_z分别为子IMU相对于主POS的弹性变形角在子IMU载体坐标系x轴、y轴和z轴上的分量，和分别为对应的弹性变形角速率；

2)建立子滤波器的数学模型

①建立子滤波器1的数学模型

a)状态方程

子滤波器1的系统状态方程如下：

${\stackrel{\·}{X}}_v=F_v{X}_v+G_v{\mathrm{\ω}}_v$

其中，F_v、G_v和ω_v分别为子滤波器1的状态转移矩阵、系统噪声驱动阵和系统噪声向量； 和分别为子IMU载体坐标系x轴、y轴、z轴陀螺和加速度计的随机误差，不包括随机常值误差，其方差阵Q_v包含Q_av和Q_cv两部分，其中Q_cv＝1/β₁Q，β₁＝1/3，Q为陀螺随机误差噪声方差阵，根据陀螺的噪声水平进行选取，Q_av为加速度计随机误差噪声方差阵，根据加速度计的噪声水平进行选取；

根据对子滤波器1中状态变量的选取可知，子滤波器1中的误差方程包括姿态误差方程、速度误差方程和惯性仪表误差方程，分别为：

${\stackrel{\·}{\mathrm{\φ}}}_v^{n_1}=-{\mathrm{\ω}}_{{\mathrm{in}}_1}^{n_1}\×{\mathrm{\φ}}_v^{n_1}+{\mathrm{\δ\ω}}_{{\mathrm{in}}_1}^{n_1}+C_b^{n_1}{\mathrm{\ϵ}}_v^b$

$\mathrm{\δ}{\stackrel{\·}{V}}^{n_1}=f^{n_1}\×{\mathrm{\φ}}_v^{n_1}-(2{\mathrm{\δ\ω}}_{ie}^{n_1}+{\mathrm{\δ\ω}}_{{\mathrm{en}}_1}^{n_1})\×U^{n_1}-(2{\mathrm{\ω}}_{ie}^{n_1}+{\mathrm{\ω}}_{{\mathrm{en}}_1}^{n_1})\×{\mathrm{\δV}}^{n_1}+C_b^{n_1}{\▿}^b$

${\stackrel{\·}{\mathrm{\ϵ}}}^v=0$

$\stackrel{\·}{\▿}=0$

其中， 为子IMU导航坐标系相对于惯性坐标系的旋转角速度在其导航坐标系中的投影；为的计算误差；为子IMU载体坐标系到其导航坐标系的转换矩阵；为陀螺随机误差，包括随机常值漂移和白噪声漂移； V_E、V_N和V_U分别为子IMU导航坐标系的东向、北向和天向速度；f_E、f_N和f_U分别为子IMU在其导航坐标系中东向、北向和天向加速度；为地球坐标系相对惯性坐标系的旋转角速度在子IMU导航坐标系的投影；为子IMU导航坐标系相对地球坐标系的旋转角速度在其导航坐标系的投影；和分别为和的计算误差；为加速度计随机误差，包括随机常值偏置和白噪声偏置；

根据上述误差方程确定的F_v和G_v分别为：

$F_v=\left[\begin{array}{cccc}{F}_1& F_2& C_b^{n_1}& 0_{3\×3}\\ F_3& F_4& 0_{3\×3}& C_b^{n_1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_N+H}& -({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+H})\\ -({\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_N+H})& 0& -\frac{V_N}{R_M+H}\\ {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+H}& \frac{V_N}{R_M+H}& 0\end{array}\right]$

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+H}& \frac{{\mathrm{TV}}_N}{{(R_M+H)}^2}\\ \frac{1}{R_N+H}& \frac{{\mathrm{T\ω}}_{ie}\sin L}{R_M+H}& -(\frac{{\mathrm{TV}}_N{\mathrm{\ω}}_{ie}\sin L}{{(R_M+H)}^2}+\frac{{\mathrm{TV}}_E\tan L}{{(R_N+H)}^2})\\ \frac{\tan L}{R_N+H}& (\frac{V_E{\sec }^{2}L}{R_N+H}-{\mathrm{\ω}}_{ie}\cos L)\frac{T}{R_M+H}& -\left(\right(\frac{V_E{\sec }^{2}L}{R_N+H}-{\mathrm{\ω}}_{ie}\cos L)\frac{{\mathrm{TV}}_N}{{(R_M+H)}^2}+\frac{{\mathrm{TV}}_E\tan L}{{(R_N+N)}^2})\end{array}\right]$

$F_3=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]$

$F_4=\left[\begin{array}{ccc}\frac{V_N\tan L}{R_N+H}-\frac{V_U}{R_N+H}& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_N+H}+\frac{A_{1}T}{R_M+H}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R_N+H}-\frac{A_1{\mathrm{TV}}_N}{{(R_M+H)}^2}+A_{2}T\\ -2({\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_N+H})& -\frac{V_U}{R_M+H}+\frac{A_{3}T}{R_M+H}& -\frac{V_N}{R_M+H}-\frac{A_3{\mathrm{TV}}_N}{{(R_M+H)}^2}+A_{4}T\\ 2({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+H})& \frac{2V_N}{R_M+H}+\frac{A_{5}T}{R_M+H}& -\frac{A_5{\mathrm{TV}}_N}{{(R_M+H)}^2}+A_{6}T\end{array}\right]$

$G_v=\left[\begin{array}{cc}{C}_b^{n_1}& 0_{3\×3}\\ 0_{3\×3}& C_b^{n_1}\\ 0_{6\×3}& 0_{6\×3}\end{array}\right]$

其中，ω_ie为地球自转角速度；R_M和R_N分别为地球沿子午圈和卯酉圈的主曲率半径；L和H分别为子IMU的纬度和高度；T为滤波周期；式中，

$A_1=2{\mathrm{\ω}}_{ie}{V}_N\cos L+\frac{V_E{V}_N{\sec }^{2}L}{R_N+H}+2{\mathrm{\ω}}_{ie}{V}_U\sin L,A_2=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+H)}^2},$

A₅＝-2ω_ieV_E sin L以及

$A_6=-\frac{V_E^2+V_N^2}{{(R_N+H)}^2};$

b)量测方程

子滤波器1的量测方程为：

Z_v＝H_xX_v+v_v

其中，系统观测量为：Z_v＝[δV′_E δV′_N δV′_U]^T，δV′_E、δV′_N和δV′_U分别为主POS经杆臂速度补偿后与子IMU之间的东向、北向和天向速度之差，杆臂速度的计算公式为：

$V_r^n=C_{b_m}^n({\mathrm{\ω}}_{{\mathrm{ib}}_m}^{b_m}\×r^{b_m})+C_{b_m}^n{\stackrel{\·}{r}}^{b_m}$

其中，为主POS载体坐标系到其导航坐标系的转换矩阵；为主POS载体坐标系相对于惯性坐标系的旋转角速度在其载体坐标系中的投影；

为主、子系统之间的杆臂在主POS载体坐标系上的投影；系统量测噪声 和分别为主POS东向、北向和天向速度的量测噪声，v_v为零均值高斯白噪声，其方差阵R_v由主POS的速度精度决定，量测矩阵H_v为：

H_v＝[0_3×3 I_3×3 0_3×6]

②建立子滤波器2的数学模型

a)状态方程

子滤波器2的系统状态方程如下：

${\stackrel{\·}{X}}_a=F_a{X}_a+G_a{\mathrm{\ω}}_a+B_a{U}_a$

其中，F_a、G_a、ω_a和B_a分别为子滤波器2的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵； 和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值漂移，η_x、η_y和η_z分别为子IMU载体坐标系x轴、y轴和z轴弹性变形角二阶Markov过程白噪声，ω_a对应的噪声方差阵Q_a包括Q_ca和Q_η两部分，其中Q_ca＝1/β₂Q，β₂＝1/3，Q_η为二阶Markov过程白噪声的方差；U_a为已知的非随机函数，U_a＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对子滤波器2中状态变量的选取可知，子滤波器2中的误差方程包括姿态误差方程、陀螺误差方程、安装误差角方程和弹性变形角方程，其中姿态误差方程和陀螺误差方程表达式与子滤波器1中的姿态误差方程和陀螺误差方程相同，安装误差角方程和弹性变形角方程分别为：

$\stackrel{\·}{\mathrm{\ρ}}=0$

${\stackrel{\·\·}{\mathrm{\θ}}}_J+2{\mathrm{\β}}_J{\stackrel{\·}{\mathrm{\θ}}}_J+{\mathrm{\β}}_J^2{\mathrm{\θ}}_J={\mathrm{\η}}_J,J=x,y,z$

其中，ρ＝[ρ_x ρ_y ρ_z]^T；θ_J用二阶Markov过程模型来描述；β_J＝2.146/τ_J，τ_J为二阶Markov过程相关时间；η_J为零均值白噪声，其方差满足：

$Q_{{\mathrm{\η}}_J}=4{{\mathrm{\β}}_J}^3{{\mathrm{\σ}}_J}^2$

其中，σ_J ²为弹性变形角θ_J的方差，

根据上述误差方程确定的F_a、G_a和B_a分别为：

$F_a=\left[\begin{array}{ccccc}{F}_1& C_b^{n_1}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& B_1& B_2\end{array}\right]$

$B_1=\left[\begin{array}{ccc}-{\mathrm{\β}}_x^2& 0& 0\\ 0& -{\mathrm{\β}}_y^2& 0\\ 0& 0& -{\mathrm{\β}}_z^2\end{array}\right]$

$B_2=\left[\begin{array}{ccc}-2{\mathrm{\β}}_x& 0& 0\\ 0& -2{\mathrm{\β}}_y& 0\\ 0& 0& -2{\mathrm{\β}}_z\end{array}\right]$

$G_a=\left[\begin{array}{cc}{C}_b^{n_1}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]$

$B_a=\left[\begin{array}{c}{F}_2\\ 0_{12\×3}\end{array}\right]$

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

b)量测方程

子滤波器2的量测方程为：

Z_a＝H_aX_a+v_a

其中，系统观测量为：Z_a＝[δψ δθ δγ]^T，δψ、δθ和δγ分别为子IMU与主POS之间的航向角、俯仰角和横滚角之差；系统量测噪声为v_a＝[v_δψ v_δθ v_δγ]^T，v_δψ、v_δθ、v_δγ分别为主POS航向角、俯仰角和横滚角的测量噪声，v_a取为零均值高斯白噪声，其方差阵R_a由主POS的姿态精度决定，量测矩阵H_a为：

H_a＝[H₁ 0_3×3 H₂ H₃ 0_3×3]

$H_1=\left[\begin{array}{ccc}\frac{{T_a}^{\left(12\right)}{T_a}^{\left(32\right)}}{{\left({T_a}^{\left(12\right)}\right)}^2+{\left({T_a}^{\left(22\right)}\right)}^2}& 0& -1\\ -\frac{{T_a}^{\left(22\right)}}{\sqrt{1-{\left({T_a}^{\left(32\right)}\right)}^2}}& \frac{{T_a}^{\left(12\right)}}{\sqrt{1-{\left({T_a}^{\left(32\right)}\right)}^2}}& 0\\ \frac{{T_a}^{\left(21\right)}{T_a}^{\left(33\right)}-{T_a}^{\left(31\right)}{T_a}^{\left(23\right)}}{{\left({T_a}^{\left(33\right)}\right)}^2+{\left({T_a}^{\left(31\right)}\right)}^2}& \frac{{T_a}^{\left(31\right)}{T_a}^{\left(13\right)}-{T_a}^{\left(11\right)}{T_a}^{\left(33\right)}}{{\left({T_a}^{\left(33\right)}\right)}^2+{\left({T_a}^{\left(31\right)}\right)}^2}& 0\end{array}\right]$

$H_2=H_3=\left[\begin{array}{ccc}\frac{{T_a}^{\left(12\right)}{T_a}^{\left(23\right)}-{T_a}^{\left(13\right)}{T_a}^{\left(22\right)}}{{\left({T_a}^{\left(12\right)}\right)}^2+{\left({T_a}^{\left(22\right)}\right)}^2}& 0& \frac{{T_a}^{\left(11\right)}{T_a}^{\left(22\right)}-{T_a}^{\left(12\right)}{T_a}^{\left(21\right)}}{{\left({T_a}^{\left(12\right)}\right)}^2+{\left({T_a}^{\left(22\right)}\right)}^2}\\ \frac{{T_a}^{\left(33\right)}}{\sqrt{1-{\left({T_a}^{\left(32\right)}\right)}^2}}& 0& -\frac{{T_a}^{\left(31\right)}}{\sqrt{1-{\left({T_a}^{\left(32\right)}\right)}^2}}\\ -\frac{{T_a}^{\left(31\right)}{T_a}^{\left(32\right)}}{{\left({T_a}^{\left(33\right)}\right)}^2+{\left({T_a}^{\left(31\right)}\right)}^2}& 1& -\frac{{T_a}^{\left(32\right)}{T_a}^{\left(33\right)}}{{\left({T_a}^{\left(33\right)}\right)}^2+{\left({T_a}^{\left(31\right)}\right)}^2}\end{array}\right]$

其中，为主POS载体坐标系到其导航坐标系的转换矩阵， 表示T_a的第j行、第l列元素；

3)计算局部估计值

根据子滤波器1和子滤波器2的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，并将子滤波器1和子滤波器2中的公共状态变量，即子IMU的失准角和陀螺的常值漂移，估计值分别记为和对应协方差记为P_cv和P_ca；

(3)由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置；

1)主滤波器进行卡尔曼滤波

①选取主滤波器状态变量

将子IMU的失准角和陀螺常值漂移作为主滤波器状态变量：

$X_m={\[{\mathrm{\φ}}_E^m,{\mathrm{\φ}}_N^m,{\mathrm{\φ}}_U^m,{\mathrm{\ϵ}}_x^m,{\mathrm{\ϵ}}_y^m,{\mathrm{\ϵ}}_z^m\]}^T$

②主滤波器数学模型

由于主滤波器没有观测信息，所以没有观测方程只有状态方程，其状态方程为：

${\stackrel{\·}{X}}_m=F_m{X}_m+G_m{\mathrm{\ω}}_m+B_m{U}_m$

其中，F_m、G_m、ω_m和B_m分别为主滤波器的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵； 和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值误差，ω_m取为零均值白噪声，其方差阵Q_m＝1/β_mQ，β_m＝1/3；U_m为已知的非随机函数，U_m＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U为子滤波器1中估计出来的速度误差；

根据对主滤波器状态变量的选取可知，主滤波器中的误差方程包括姿态误差方程和陀螺误差方程，二者表达式均与子滤波器1的姿态误差方程和陀螺误差方程相同，因此F_m、G_m和B_m的表达式分别为：

$F_m=\left[\begin{array}{cc}{F}_1& C_b^{n_1}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$G_m=\left[\begin{array}{c}{C}_b^{n_1}\\ 0_{3\×3}\end{array}\right]$

$B_m=\left[\begin{array}{c}{F}_2\\ 0_{3\×3}\end{array}\right]$

其中，F₁和F₂的含义与子滤波器1状态转移矩阵中F₁和F₂相同；

③主滤波器根据其数学模型进行卡尔曼滤波，由于主滤波器中只有状态方程，因此只进行时间更新，并将状态变量估计值和协方差阵分别记为和P_m；

2)计算全局滤波解

将主、子滤波器中的公共状态变量估计值和协方差阵按照下式进行融合，即可得到全局最优估计：

$\left\{\begin{array}{c}{P}_g={({P_{cv}}^{-1}+{P_{ca}}^{-1}+{P_m}^{-1})}^{-1}\\ {\hat{X}}_g=P_g({P_{cv}}^{-1}{\hat{X}}_{cv}+{P_{ca}}^{-1}{\hat{X}}_{ca}+{P_m}^{-1}{\hat{X}}_m)\end{array}\right.$

其中，和P_g分别表示全局状态变量估计值和协方差阵，是由子滤波器1和子滤波器2中局部状态变量估计值和协方差阵在主滤波器中进行融合后得到的；

3)对主、子滤波器公共状态变量估计值和协方差阵进行重置

利用全局滤波解对主、子滤波器中的失准角和陀螺常值漂移的估计值和协方差阵进行重置，如下式所示：

$\left\{\begin{array}{c}{\hat{X}}_{cv}={\hat{X}}_g\\ {P_{cv}}^{\′-1}={\mathrm{\β}}_1{P_g}^{-1}\end{array}\right.$

$\left\{\begin{array}{c}{{\hat{X}}_{ca}}^{\′}={\hat{X}}_g\\ {P_{ca}}^{\′-1}={\mathrm{\β}}_2{P_g}^{-1}\end{array}\right.$

$\left\{\begin{array}{c}{{\hat{X}}_m}^{\′}={\hat{X}}_g\\ {P_m}^{\′-1}={\mathrm{\β}}_m{P_g}^{-1}\end{array}\right.$

其中，和分别表示经过重置后子滤波器1、子滤波器2和主滤波器中的公共状态变量估计值；P_cv′、P_ca′和P_m′分别表示经过重置后子滤波器1、子滤波器2和主滤波器中公共状态变量的协方差阵；

(4)利用步骤(3)中重置后的误差状态量计算出更加准确的子IMU的位置、速度和姿态，不断重复步骤(2)和步骤(3)直至传递对准结束；

1)速度修正

$\left\{\begin{array}{c}{V}_E^{\′}=V_E-{\mathrm{\δV}}_E\\ V_N^{\′}=V_N-{\mathrm{\δV}}_N\\ V_U^{\′}=V_U-{\mathrm{\δV}}_U\end{array}\right.$

其中，V_E′、V_N′和V_U′分别为修正后子IMU的东向、北向和天向速度；

2)位置修正

$\left\{\begin{array}{c}{L}^{\′}=L-\mathrm{\δ}L\\ {\mathrm{\λ}}^{\′}=\mathrm{\λ}-\mathrm{\δ}\mathrm{\λ}\\ H^{\′}=H-\mathrm{\δ}H\end{array}\right.$

其中，λ为捷联解算得到的子IMU的经度；L′、λ′和H′分别为修正后子IMU的纬度、经度和高度；δL、δλ和δH分别为子IMU的纬度、经度和高度误差，由子滤波器1估计出的速度误差通过积分得到，表达式如下所示：

$\left\{\begin{array}{c}\mathrm{\δ}L=\frac{{\mathrm{\δV}}_{N}T}{R_M+H}-\frac{V_{N}T}{{\left(R_M+H\right)}^2}\mathrm{\δ}H\\ \mathrm{\δ}\mathrm{\λ}=\frac{T\sec L}{R_N+H}{\mathrm{\δV}}_E+\frac{{\mathrm{TV}}_E\sec L\tan L}{R_N+H}\mathrm{\δ}L-\frac{{\mathrm{TV}}_E\sec L}{{\left(R_N+H\right)}^2}\mathrm{\δ}H\\ \mathrm{\δ}H={\mathrm{\δV}}_{U}T\end{array}\right.$

3)姿态修正

利用重置后中的失准角来计算子IMU导航坐标系n₁与计算导航坐标系n′₁间的转换矩阵

$C_{n_1^{\′}}^{n_1}=\left[\begin{array}{ccc}1& -{{\mathrm{\φ}}_U^{\′}}^a& {{\mathrm{\φ}}_N^{\′}}^a\\ {{\mathrm{\φ}}_U^{\′}}^a& 1& -{{\mathrm{\φ}}_E^{\′}}^a\\ -{{\mathrm{\φ}}_N^{\′}}^a& {{\mathrm{\φ}}_E^{\′}}^a& 1\end{array}\right]$

修正后的转换矩阵为：

$C_b^{n_1}=C_{n_1^{\′}}^{n_1}{C}_b^{n_1^{\′}}$

其中，为子IMU进行捷联解算后得到的转换矩阵；

利用修正后的转换矩阵计算子IMU的姿态角，包括航向角、俯仰角和横滚角；

4)将子滤波器1和子滤波器2状态变量中的失准角和速度误差变量置零，并将修正后子IMU的速度、位置和姿态作为下一次解算初始值，重复步骤(2)和步骤(3)，直至循环结束完成传递对准。