发明内容
本发明的目的在于提供一种一种分布式结构的个人导航定位***,将MEMS惯性传感器安装于人体足部,通过MEMS微惯性导航***与其他多种量测方法进行信息融合来提高导航定位性能。
实现本发明目的的技术解决方案为:一种微小型个人组合导航***,该***由安装在人体足部的分***与安装在躯干部位的分***构成,足部分***包括微机电MEMS IMU(Micro Electro Mechanical System,简称为MEMS)惯性测量单元(Inertial Measurement Unit,简称为IMU)、磁强计与第一通讯模块,其中磁强计的方位轴与MEMS IMU的方位轴重合安装,其输出端并分别与第一通讯模块相连接,躯干部位的分***包括卫星导航接收机、多普勒测速仪、气压高度计、第二通讯模块以及微型导航计算机,其中卫星导航接收机、多普勒测速仪、气压高度计分别与微型导航计算机相连接,微型导航计算机与第二通讯模块相连接,第二通讯模块通过无线通讯的方式与第一通讯模块连接并接收其发送的数据。
一种基于微小型个人组合导航***的导航定位方法,由采集MEMS IMU信号、陀螺仪与加速度计的随机误差建模与校正、惯性导航***初始对准、捷联惯性导航解算、惯性导航***零速修正、MEMS惯性导航***/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航五部分组成,导航定位过程中首先通过导航计算机采集MEMS IMU中陀螺仪与加速度计信号,对陀螺仪与加速度计进行随机误差建模与校正、并采用校正后的数据完成惯性导航***初始对准,在此基础上进行捷联惯性导航解算,并在足部着地时间段进行惯性导航***零速修正,将经过修正的捷联惯性导航结果与卫星导航接收机、多普勒测速仪、磁强计、气压高度计完成多信息融合组合导航,具体步骤如下:
(1)MEMS IMU信号采集步骤:采集MEMS IMU中三轴MEMS陀螺仪与三轴MEMS加速度计的输出信号,得到捷联惯性导航解算所需的角速度和比力,并将角速度和比力信号通过安装于足部与躯干的通讯模块传输给微型导航计算机;
(2)MEMS惯性器件的随机误差的建模与误差校正步骤:在微型导航计算机中对步骤(1)采集的MEMS陀螺仪与MEMS加速度计数据分别建立求和自回归滑动平均模型即ARIMA模型,将MEMS陀螺仪与MEMS加速度计实际输出作为观测量,将MEMS陀螺仪与MEMS加速度计的理论输出作为状态量,建立基于ARIMA模型的卡尔曼滤波器,实时估计MEMS陀螺仪与MEMS加速度计的理论输出,并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航的数据源;
(3)惯性导航***初始对准步骤:该步骤在微型导航计算机中进行,微小型个人组合导航***启动后在静态条件下,利用步骤(2)中MEMS加速度计的理论输出估计值,在微型导航计算机中通过水平自对准得到MEMSIMU的初始横滚角与俯仰角,并将磁强计的信号输出通过通讯模块传送至微型导航计算机以得到MEMS IMU的初始航向角;
(4)捷联惯性导航解算步骤:该步骤在微型导航计算机中进行,由姿态、速度、位置解算三步组成,惯导***姿态解算首先利用公式
计算机体相对导航坐标系的角速率
,其中
为MEMS陀螺仪的理论输出估计值,
采用多普勒测速仪输出、地球自转角速度
以及上个周期的姿态转移矩阵
求得,然后采用四元数微分方程求解姿态转移矩阵
对应的四元数,并将得到的四元数规范化后生成姿态转移矩阵
,最后从
中提取横滚角
、俯仰角
、航向角
,MEMS惯性导航***的速度、位置解算首先将加速度计理论输出估计值通过
的转置矩阵
从载体坐标系转换到导航坐标系,并通过解算惯性比力微分方程得到MEMS惯性导航***的速度信息
,再将
进行一次积分得到***的位置信息;
(5)惯性导航***零速修正步骤:该步骤在微型导航计算机中进行,求解MEMS加速度计输出经步骤(2)误差建模与校正后的估计值的三轴矢量和,通过判断该值是否接近重力来检测人体足部是否处于着地时间段,并利用着地时间段MEMSIMU相对地面静止的特点进行间断式的零速修正,即建立惯性导航***误差状态方程,以足部着地时间段MEMS惯性***的水平速度输出为量测量建立量测方程,运用卡尔曼滤波器估计惯性导航***的平台误差角
、
、
,MEMS陀螺仪随机常值误差
、
、
,以及MEMS加速度计随机常值误差
、
,并以反馈校正的方式提高惯性导航***的水平姿态精度与惯性器件精度;
(6)MEMS惯性导航***/卫星导航***多普勒测速仪/磁强计/气压高度计组合导航步骤:该步骤在导航计算机中进行,1)构建MEMS惯性/卫星组合导航子***即子***Ⅰ,将MEMS惯性导航***与卫星导航***输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,以惯性导航***与卫星导航***所测量的位置、速度信息之间的差值为量测量建立量测方程,采用卡尔曼滤波实时估计惯性导航***与惯性器件的误差;2)构建MEMS惯性/多普勒组合导航子***即子***Ⅱ,将MEMS惯性导航***与多普勒测速***输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,以惯性导航***与多普勒测速***所测量的速度信息之间的差值为量测量建立量测方程,采用卡尔曼滤波实时估计惯性导航***与惯性器件的误差;3)构建惯性/气压高度计组合导航子***Ⅲ:将MEMS惯性导航***与气压高度计输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,以惯性导航***与气压高度计所测量的高度信息之间的差值为量测量建立量测方程,采用卡尔曼滤波实时估计惯性导航***与惯性器件的误差;4)利用组合导航子***Ⅰ、Ⅱ、Ⅲ建立联邦滤波器,进一步估计惯性导航***与惯性器件误差,从而提高惯性导航***与惯性器件误差的估计精度,并通过闭环反馈校正提高惯性导航***的位置、速度与姿态精度。
本发明与现有技术相比,其显著优点:(1)本发明将个人导航***设计为由足部分***与躯干分***构成的分布式结构,两个分***之间通过无线通讯模块进行通讯,有效减轻了导航***中各类传感器之间的电磁干扰,并使人体躯干与足部分担***的重量,更适宜于个人便携;
(2)本发明将MEMS IMU与磁强计安装于人体足部,在进行捷联惯性导航解算的同时,可利用足部着地时间段对惯性导航***进行零速修正,提高了惯性导航***的性能;
(3)本发明采用了MEMS惯性导航***/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航方案,可在复杂电磁环境中实现连续、实时、全参数的导航定位功能。
具体实施方式
下面结合附图对本发明作进一步详细描述。
本发明旨在便携式的个人装备中,安装MEMSIMU、卫星导航接收机、多普勒测速仪、气压高度计等设备,探索个人便携式导航定位的新途径,为行人、作业人员、作战士兵、救援人员等提供实时、精确、完整的导航定位信息。
本发明一种微小型个人组合导航***,该组合导航***由安装在人体足部的分***与安装在躯干部位的分***构成,足部分***包括MEMS IMU1、磁强计2与第一通讯模块3,其中磁强计的方位轴与MEMS IMU的方位轴重合安装,其输出端并分别与第一通讯模块相连接,躯干部位的分***包括卫星导航接收机4、多普勒测速仪5、气压高度计6、第二通讯模块7以及微型导航计算机8,其中卫星导航接收机、多普勒测速仪、气压高度计分别与微型导航计算机相连接,微型导航计算机与第二通讯模块相连接,第二通讯模块通过无线通讯的方式与第一通讯模块连接并接收其发送的数据。组合导航***的结构如图1与图2所示。
本发明导航定位方法由采集MEMS IMU信号、陀螺仪与加速度计的随机误差建模与校正、惯性导航***初始对准、捷联惯性导航解算、惯性导航***零速修正、MEMS惯性导航***/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航五部分组成,导航定位过程中首先通过导航计算机采集MEMS IMU中陀螺仪与加速度计信号,对陀螺仪与加速度计进行随机误差建模与校正、并采用校正后的数据完成惯性导航***初始对准,在此基础上进行捷联惯性导航解算,并在足部着地时间段进行惯性导航***零速修正,将经过修正的捷联惯性导航结果与卫星导航接收机、多普勒测速仪、磁强计、气压高度计完成多信息融合组合导航,组合导航方法流程如图3所示,通过以下步骤实现:
(1)MEMS IMU信号采集步骤:采集MEMS IMU中MEMS陀螺与MEMS加速度计的输出信号,得到惯导***的角速度和比力,并将信号通过安装于足部与躯干的通讯模块传输给微型导航计算机;
(2)MEMS惯性器件的随机误差的建模与误差校正步骤:在微型导航计算机中对步骤(1)采集的MEMS陀螺仪与MEMS加速度计数据分别建立求和自回归滑动平均模型即ARIMA模型,将MEMS陀螺仪与MEMS加速度计实际输出作为观测量,将MEMS陀螺仪与MEMS加速度计的理论输出作为状态量,建立基于ARIMA模型的卡尔曼滤波器,实时估计MEMS陀螺仪与MEMS加速度计的理论输出,并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航的数据源;
(3)惯性导航***初始对准步骤:该步骤在微型导航计算机中进行,***启动后在静态条件下,利用步骤(2)中MEMS加速度计的理论输出估计值,在微型导航计算机中通过水平自对准得到MEMS IMU初始横滚角与俯仰角,并将磁强计的信号输出通过通讯模块1与2传送至微型导航计算机以得到MEMS IMU的初始航向角,水平自对准的公式为:
,
,其中
为俯仰角,
为横滚角,
为重力加速度,
与
分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量;
(4)捷联惯性导航解算步骤:该步骤在导航计算机中进行,由姿态解算与速度、位置解算组成,惯导***姿态解算首先利用公式
计算机体相对导航坐标系角速率
,式中
为MEMS陀螺仪的理论输出估计值,
可采用行进中的速度在导航坐标系中的投影、地球自转角速度
以及上个周期的姿态转移矩阵
求得,然后采用四元数微分方程
求解姿态转移矩阵
对应的四元数
,其中
、
、
为相互正交的单位矢量,
为四元数算法计算的单位时间间隔,
,
,将得到的四元数采用公式
进行规范化,式中
为直接由四元数微分方程计算得到的四元数,
为规范化后的四元数,再将规范化后的四元数通过公式
生成姿态转移矩阵,
的定义同上,最后利用公式
、
、
从
中提取横滚角
、俯仰角
、航向角
,其中
、
、
、
、
、
、
、
、
同样分别代表姿态转移矩阵
对应行列位置的各元素,MEMS惯性导航***速度、位置的解算方法为:首先将加速度计理论输出估计值通过平台
的转置矩阵
从载体坐标系转换到导航坐标系,并通过解算比力微分方程
得到MEMS惯性导航***速度在导航坐标系中的投影
,再将
经过一次积分得到其位置,式中
为MEMS加速度计理论输出估计值在导航坐标系中的投影,
为地球自转角速度在导航坐标系中的投影,
为导航坐标系相对于地球坐标系的角速度在导航坐标系中的投影,可通过行进中的速度在导航坐标系中的投影求得;
(5)惯性导航***零速修正步骤:该步骤在微型导航计算机中进行,求解MEMS加速度计输出经步骤(2)误差建模与校正后的估计值的三轴矢量和,通过判断该值是否接近重力来检测人体足部是否处于着地时间段,并利用着地时刻MEMSIMU相对地面静止的特点进行间断式的零速修正,即根据惯性导航***误差与惯性器件误差特性误差建立状态方程,以足部着地时间段MEMS惯性导航***的水平速度输出作为观测量,建立***的观测方程,运用最优滤波方法实时估计MEMS惯性导航***的初始平台误差角,MEMS陀螺仪随机常值误差,以及MEMS加速度计随机常值误差,并以反馈校正的方式提高惯性导航***的水平姿态精度与惯性器件精度;
(6)MEMS惯性导航***/卫星导航***/多普勒测速仪/磁强计/气压高度计组合导航步骤:该步骤在微型导航计算机中进行,1)构建MEMS惯性/卫星组合导航子***Ⅰ,将MEMS惯性导航***与卫星导航***的输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,以惯性导航***与卫星导航***所输出的位置、速度信息之间的差值为量测量建立量测方程,采用最优滤波方法实时估计惯性导航***与惯性器件的误差;2)构建MEMS惯性/多普勒组合导航子***Ⅱ,将MEMS惯性导航***与多普勒测速***输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,以惯性导航***与多普勒测速***所输出的速度信息之间的差值为量测量建立量测方程,采用最优滤波方法实时估计惯性导航***误差与惯性器件的误差;3)构建惯性/气压高度计组合导航子***Ⅲ,将MEMS惯性导航***与气压高度计输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差与惯性器件误差特性建立状态方程,以惯性导航***与气压高度计所输出的高度信息之间的差值为量测量建立量测方程,采用最优滤波方法实时估计惯性导航***与惯性器件的误差;4)基于子***Ⅰ、Ⅱ、Ⅲ构建联邦滤波器,即利用组合导航子***Ⅰ、Ⅱ、Ⅲ作为子滤波器,并以惯性导航***与惯性器件误差特性建立主滤波器,通过全局滤波进一步提高惯性导航***与惯性器件误差的估计精度,并通过闭环反馈校正方式提高惯性导航***的位置、速度、姿态以及惯性器件精度。
下面结合实施例对本发明作进一步详细说明。
本发明个人组合导航***的结构由安装在人体足部的分***与安装在躯干部位的分***构成。
足部分***包括MEMS IMU,磁强计与第一通讯模块,其中MEMS IMU可由三个MEMS陀螺仪与三个MEMS加速度计组成,三个陀螺仪与三个加速度计分别相互正交,磁强计的方位轴与MEMS IMU的方位轴重合安装,通讯模块可采用基于ZigBee、WIFI、Bluetooth、UWB等无线传感技术研制的无线通讯模块。
躯干部位的分***包括便携式的卫星导航接收机、多普勒测速仪、气压高度计、第二通讯模块与微型导航计算机,其中第二通讯模块采用与足部分***相同的无线通讯模块,微型导航计算机可采用基于DSP等处理器的微型计算机,或采用PDA、掌上电脑等便携式移动设备。
为了完成个人导航定位、测速与定姿,需要完成的步骤如以下实例所示:
5.1I MEMS IMU信号采集步骤
采集MEMS IMU中MEMS陀螺与MEMS加速度计的输出信号,得到惯导***的角速度和比力,并将信号通过安装于足部与躯干的通讯模块传输给微型导航计算机。
5.2 MEMS惯性器件的随机误差的建模与校正步骤
在MEMS陀螺仪与MEMS加速度计的零位偏置随时间变化较明显的情况下,首先需对零位偏置的特性进行统计分析,分别建立符合MEMS陀螺仪与MEMS加速度计的多阶多项式零位偏置趋势项,如某型MEMS陀螺仪零偏趋势项适用模型为四阶多项式模型,某型MEMS加速度计零偏趋势项适用模型为二阶多项式模型,其形式分别如公式与所示:
式中,
与
分别为陀螺仪与加速度计零偏的趋势项,
为时间,
、
、
、
、
分别为四阶多项式模型的各阶参数,
、
、
分别为二阶多项式模型的各阶参数。确定MEMS陀螺仪与MEMS加速度计的零偏趋势项模型后,在MEMS陀螺仪与MEMS加速度计的信号输出端加入零偏趋势项校正,对数据的零偏趋势项进行实时校正。
对校正趋势项后的MEMS陀螺仪与MEMS加速度计分别建立求和自回归滑动平均模型即ARIMA模型,如某型MEMS陀螺仪随机误差的适用模型为ARMA(2,1)模型,其中2为自回归模型阶数,1为滑动平均模型阶数,其模型结构如公式所示:
式中
为t时刻的信号,
为t时刻噪声,
、
为自回归模型参数,
为滑动平均模型参数。如某型MEMS加速度计随机误差的适用模型为AR(1)模型,其中2为自回归模型阶数,其模型结构如公式所示:
式中
为t时刻的信号,
为t时刻噪声,
为自回归模型参数。将经过5.1中趋势项建模与校正步骤后的MEMS陀螺仪与MEMS加速度计输出作为观测量,将MEMS陀螺仪与MEMS加速度计的理想输出估计值作为状态量,建立基于ARIMA模型的卡尔曼滤波器,分别实时估计MEMS陀螺仪与MEMS加速度计的理想输出,卡尔曼滤波的状态方程如公式(5)所示:
其中
,为相邻两个时刻的MEMS惯性传感器(MEMS陀螺仪或MEMS加速度计)的输出,
,为相邻两个时刻的***过程噪声,
为离散***的状态转移矩阵,
为***过程噪声系数矩阵,卡尔曼滤波的量测方程如公式(6)所示:
将MEMS惯性传感器的理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航步骤的数据源。
5.3 MEMS惯性***初始对准步骤
惯性导航***初始对准步骤:该步骤在微型导航计算机中进行,***启动后在静态条件下,利用步骤(2)中MEMS加速度计的理论输出估计值,在微型导航计算机中通过水平自对准得到MEMS IMU初始横滚角与俯仰角,并将磁强计的信号输出通过通讯模块1与2传送至微型导航计算机以得到MEMS IMU的初始航向角,水平自对准的公式为:
其中
为俯仰角,
为横滚角,
为重力加速度,
与
分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量;
5.4 捷联惯性导航解算步骤
MEMS惯性导航***的捷联惯性导航解算步骤分为姿态解算与速度、位置解算。
(1) 姿态解算
捷联惯性导航***的姿态解算有欧拉角法、方向余弦法和四元数法。由于四元数法具有可以全姿态工作、计算工作量小等特点,故本方法采用该方法进行姿态解算。四元数法解算姿态主要有以下几个步骤:
其中
为MEMS陀螺仪输出的理论估计值,
可通过行进速度在导航坐标系中的投影、地球自转角速度
以及上个周期的姿态转移矩阵
求得。
3) 四元数微分方程的求解
四元数微分方程的离散化形式,即四元数迭代计算公式如下:
上式中
为规范化后的四元数,其中
、
、
为相互正交的单位矢量,
为四元数算法计算的单位时间间隔;
,
、
、
分别为在一次姿态解算周期内对MEMS陀螺仪输出角速度的3次采样值,即采用3子样采样方法补偿陀螺非定轴转动时角速度矢量积分产生的计算误差,MEMS陀螺仪采样周期为
。
4) 四元数规范化
姿态解算中计算误差的存在使得计算的变换四元数的范数不再等于1,即计算的四元数失去规范性,因此对计算的四元数必须周期性地进行规范化处理。设
为直接由四元数微分方程计算得到的四元数,而设规范化后的四元数为
,其中
、
、
为相互正交的单位矢量,利用最小二乘判据可以求得最优规范化的变换四元数。计算公式如下:
依据四元数表示的固定矢量之间的变换关系,可以获得四元数与方向余弦矩阵的关系为:
其中
、
、
分别为捷联惯导***的俯仰角、横滚角以及航向角,
、
、
、
、
、
、
、
、
分别为姿态转移矩阵
对应行列位置的各元素,即
、
、
、
、
、
、
、
、
。
(2) 速度、位置解算
捷联惯性导航速度解算中,由于加速度计固连在MEMS惯性测量组件上,它的输出是载体相对于惯性空间的比力在载体坐标系中的投影,因此需要把加速度计原始输出
通过姿态转移矩阵
转换到导航坐标系中,记为
,即
从而可以解算出载体在地理系中的速度,其微分方程为:
(17)
式中
为MEMS加速度计理论输出估计值在导航坐标系中的投影,
为地球自转角速度在导航坐标系中的投影,
为导航坐标系相对于地球坐标系的角速度在导航坐标系中的投影,可通过行进中的速度在导航坐标系中的投影求得。将上式移项并展开得:
式中,
、
、
分别是
在东向、北向和地向的投影分量,
、
、
分别是东向、北向和地向的速度分量,
、
分别为当地的经度与纬度,
为当地重力加速度。
利用
、
、
并结合上个时刻载体的纬度、经度、高度与速度,求解当前时刻导航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影
、以及当前时刻载体的纬度
、经度
、高度
,完成整个MEMS捷联惯性导航的算法流程。
5.4惯性导航***零速修正步骤
该步骤在微型导航计算机中进行,求解MEMS加速度计输出经步骤(2)误差建模与校正后的估计值的三轴矢量和,通过判断该值是否接近重力来检测人体足部是否处于着地时间段,并利用着地时间段MEMS IMU相对地面静止的特点进行间断式的零速修正,即根据惯性导航***误差与惯性器件误差特性误差建立状态方程:
式中,误差状态量
,其中
、
为水平速度误差沿北、东方向的分量,
、
、
为平台误差角在北、东、地方向的分量;
、
分别为加速度计的常值零偏在载体坐标系中x、y方向的分量;
、
、
分别为陀螺零位常值漂移在载体坐标系中x、y、z方向的分量;状态转移矩阵
,其中
,
,
为地球自转角速度在地向与北向的分量,
~
分别为惯性导航***姿态转移矩阵
中的对应行列位置上的各元素,定义同上;
为当地重力加速度;
,式中
、
为等效北向、东向的加速度计输出白噪声分量,
、
、
为等效北向、东向、地向的陀螺仪输出白噪声分量。以足部着地点MEMS惯性导航***的水平速度输出作为观测量,建立***的观测方程:
(20)
式中,
、
分别为水平速度误差沿北、东方向的分量,
定义同上;
为观测噪声矢量。运用最优滤波方法实时估计MEMS惯性导航***的初始平台误差角,MEMS陀螺仪随机常值误差,以及MEMS加速度计随机常值误差,并以反馈校正的方式提高惯性导航***的水平姿态精度与惯性器件精度。
5.5 MEMS惯性导航***/磁强计/卫星导航***/多普勒测速仪/气压高度计组合导航步骤
MEMS惯性导航***/磁强计/卫星导航***/多普勒测速仪/气压高度计组合导航步骤由以下步骤(1)、(2)、(3)组成:
(1)构建MEMS惯性/卫星组合导航子***Ⅰ,将MEMS惯性导航***与卫星导航***构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程:
式中:
=[
]
为***状态量,分别代表惯性导航***的3维平台误差角(其中航向误差角已经磁强计修正)、3维速度误差、3维位置误差、陀螺的3轴随机常值和3轴一阶马尔可夫误差以及加速度计的3轴一阶马尔可夫误差,共18阶。
其中
表示3阶单位矩阵,
表示
阶的0矩阵(下同),
表示对应平台误差角、速度误差、位置误差9个基本导航参数误差的系数矩阵,其非零元素为
其中
为地球自转角速率,
为载体所在纬度,
为地球赤道平面半径,
为地球子午圈半径,
为地球卯酉圈半径,
、
、
为载体北向、东向和天向速度,
、
、
分别为北向、东向和天向的比力,
和
分别为:
其中
、
、
分别为MEMS陀螺仪一阶马尔科夫过程误差的相关时间,
、
、
分别为MEMS加速度计一阶马尔科夫过程误差的相关时间,以惯性导航***与卫星导航***所输出的位置、速度信息之间的差值为量测量建立量测方程,位置差值的观测方程分别为:
式中:
表示位置差值,
;
为量测噪声,可视为白噪声,其方差分别为
,
,
。
速度差值的观测方程为
式中:
,
为量测噪声,可视为白噪声,其方差分别为
,
,
。
采用线性卡尔曼滤波方法实时估计惯性导航***与惯性器件的误差;
(2)构建MEMS惯性/多普勒组合导航子***Ⅱ,将MEMS惯性导航***与多普勒测速***输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差方程与惯性器件误差特性建立状态方程,同公式(21)。以惯性导航***与多普勒测速***所输出的速度信息之间的差值为量测量建立量测方程,同公式(23)。采用线性卡尔曼滤波方法实时估计惯性导航***误差与惯性器件的误差;
(3)构建惯性/气压高度计组合导航子***Ⅲ,将MEMS惯性导航***与气压高度计输出传送至微型导航计算机构建组合导航子***,根据惯性导航***误差与惯性器件误差特性建立状态方程,同公式(21)。以惯性导航***与气压高度计所输出的高度信息之间的差值为量测量建立量测方程,同公式(22)中的高度差值观测量以及对应的观测系数矩阵与观测噪声,采用最优滤波方法实时估计惯性导航***与惯性器件的误差;
4)基于子***Ⅰ、Ⅱ、Ⅲ的构建联邦滤波器,利用组合导航子***Ⅰ、Ⅱ、Ⅲ作为子滤波器,并以惯性导航***与惯性器件误差特性建立主滤波器,通过全局滤波进一步提高惯性导航***与惯性器件误差的估计精度,并通过闭环反馈校正方式提高惯性导航***的位置、速度、姿态以及惯性器件精度。