CN113375663A - 一种基于性能预估的多源信息融合自适应导航方法 - Google Patents

Abstract

本发明公开了一种基于性能预估的多源信息融合自适应导航方法，首先，获取多源导航系统中惯性导航及其他辅助导航的测量数据；计算以惯性导航为基准的子滤波器；其次，计算子滤波器的性能指标并进行排序，形成状态序列；根据状态序列，计算组合导航，形成组合导航库；然后，计算组合导航的ANP值并进行对比，修正各惯性导航输出；最后，对比各惯导ANP值，并选取对应信息形成最优导航输出。本发明使多源信息组合导航方案更加灵活；使导航方案精度的预估成为可能；提高了导航系统整体精度；与未采用ANP预估的多源信息组合导航定位算法相比，本发明在导航传感器冗余配置的导航系统中也能选取最优的导航方案，适合实际应用。

Description

一种基于性能预估的多源信息融合自适应导航方法

技术领域

本发明属于定位与导航技术领域，具体涉及一种基于性能预估的多源信息融合自适应导航方法。

背景技术

近年来，随着各种辅助导航手段、现代估计技术和高性能计算机技术的快速发展，组合导航系统获得了广泛的应用。组合导航系统是利用计算机和数据处理技术把具有不同特点的导航设备组合在一起，以达到优化的目的，整个系统由输入装置、数据处理和控制部分、输出装置以及外围设备组成。输入装置能够实时、连续的接收各种测量信息，由计算机将接收的信息进行综合处理，从而得到最优的结果以便于确定航向、航速、天文以及地文测算等。组合导航系统最大的优势就是能够实现优势互补,提高导航系统的精度和可靠性。组合导航技术是目前导航技术发展的重要研究方向。

传统多源信息组合导航算法采用的是固定的组合导航框架，在所有导航系统稳定输出的情况下有较好的定位精度，而对于部分导航系统信息阶段性存在的情况下定位精度受到较大影响。此外，由于部分导航系统指输出姿态信息，使传统组合导航算法输出的导航信息使整体最优，其中包括位置和姿态信息，但对于位置信息而言未必是最优的组合方式。

发明内容

发明目的：本发明提供一种在导航传感器冗余配置的多源导航系统中也能选取最优的导航方案，切适合实际应用的，基于性能预估的多源信息融合自适应导航方法。

发明内容：本发明提出一种基于性能预估的多源信息融合自适应导航方法，具体包括以下步骤：

(1)获取多源导航系统中惯性导航及其他辅助导航的测量数据；

(2)计算以惯性导航为基准的子滤波器；

(3)计算子滤波器的性能指标并进行排序，形成状态序列；

(4)根据状态序列，计算组合导航，形成组合导航库；

(5)计算组合导航的ANP值并进行对比，修正各惯性导航输出；

(6)对比各惯导ANP值，并选取对应信息形成最优导航输出。

进一步地，所述步骤(1)包括以下步骤：

(11)惯性导航系统数量为n,其他辅助导航系统数量为m，当前时刻为t；

(12)设定t＝0，并根据n个惯性导航系统的基本参数确定其误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)；

(13)对冗余配置的n个惯性导航系统同时执行(14)-(16)；

(14)t＝t+1；

(15)采集捷联惯性导航系统的导航信息，其中第i个捷联惯性导航系统的导航信息，存储为该导航系统预输出：

Y_ipout(t)＝(γ_i θ_i ψ_i v_ei v_ni v_ui L_i λ_i h_i)

其中，γ_i、θ_i和ψ_i分别是第i个惯性导航系统输出的横滚角、俯仰角和航向角，v_ei、v_ni和v_ui分别是第i个惯性导航系统输出的东向速度、北向速度和天向速度；L_i、λ_i和h_i分别是第i个惯性导航系统输出的经度、纬度和高度；

(16)根据导航系统预输出信息Y_ipout(t)和前一时刻的导航信息Y_out(t-1)，结合飞行任务，确认当前时刻的性能评估指标；

(17)对m个辅助导航系统执行(18)；

(18)采集m个辅助导航系统信息，其中第j个辅助导航系统的信息存储为Y_j(t)：

Y_j(t)＝(γ_j θ_j ψ_j v_ej v_nj v_uj L_jλ_j h_j)

其中，γ_j、θ_j和ψ_j分别是第j个辅助导航系统输出的横滚角、俯仰角和航向角，v_ej、v_nj和v_uj分别是第j个辅助导航系统输出的东向速度、北向速度和天向速度；L_j、λ_j和h_j分别是第j个辅助导航系统输出的经度、纬度和高度。

进一步地，所述步骤(2)包括以下步骤：

(21)j为第j个辅助导航，设j＝1；

(22)提取步骤(1)中获得的第i个惯性导航系统的误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，通过标准卡尔曼滤波的方式将第i个惯性导航和第j个辅助导航进行信息融合，形成一个子滤波器，记为Sf_i,j，并保存其误差矩阵X_i,j和协方差矩阵P_i,j；

(23)若j≤m，则j＝j+1，并转至步骤(22)，否则，转至步骤(3)。

进一步地，所述步骤(3)包括以下步骤：

(31)利用卡尔曼滤波的误差矩阵X_i,j和协方差矩阵P_i,j，提取以经纬度为单位的位置的误差矩阵和协方差矩阵，分别记为E_pos和P_pos：

(32)将经纬度位置误差E_pos转化为水平面内的位置误差矩阵e_pos和协方差矩阵p_pos：

e_pos＝[δx δy]

(33)提取速度和姿态的误差矩阵和协方差矩阵，分别记为e_velo、e_atti、p_velo和p_atti：

e_velo＝[δv_e δv_n δv_u],e_atti＝[δγ δθ δψ]

p_velo＝cov[e_velo],p_atti＝cov[e_atti]；

(34)对位置、速度、姿态的协方差矩阵分别进行分解得：

p＝AΛA^-1

式中，A是p矩阵对应特征值λ_l的特征向量；

(35)根据特征向量λ_l可以获得95％误差椭圆的长短半径，其表达式为：

(36)根据95％误差椭圆的长短半径计算不确定度值，位置、速度和姿态评价指标分别记为ANP_i,j,posi、ANP_i,j,velo和ANP_i,j,atti：

ANP_i,j＝a·axis_major

(37)分别存储ANP_i,j及其对应的误差矩阵X_i,j和协方差矩阵P_i,j；

(38)分别对比位置、速度和姿态的ANP_i,j值，按ANP_i,j值从小到大进行存储，形成状态序列，其公式如下：

位置序列：

速度序列：

姿态序列：

其中，Db_sf,posi是形成的位置序列，Sf_k,posi是位置序列中排在位置k的子滤波器，ANP_k,posi是该自滤波器的位置ANP值，速度序列、姿态序列与位置序列类同。

进一步地，所述步骤(4)包括以下步骤：

(41)设定c＝1；

(42)根据Db_sf,posi的排序，提取排序中的前c个子滤波器；

(43)提取误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，对子滤波器的信息和全局滤波器的信息进行分配，第k个子滤波器的信息分配原则如下：

(44)根据(43)中的X_c,k，P_c,k和Q_c,k，采用标准卡尔曼滤波算法，求得子滤波器的误差矩阵X_i,c,k、协方差矩阵P_i,c,k和噪声方差矩阵Q_i,c,k；

(45)采用联邦滤波的方式对全局滤波器信息进行融合，获得全局滤波器的误差矩阵X_i,c、协方差矩阵P_i,c和噪声方差矩阵Q_i,c，其计算公式如下：

(46)提取组合导航误差矩阵X_i,c和协方差矩阵P_i,c，计算组合导航ANP值，并存储为ANP_i,c,posi；

(47)根据Db_sf,velo的排序，提取排序中的前c个子滤波器，并重复(43)-(46)，速度指标存储为ANP_i,c,velo，并跳转至步骤(47)；

(48)根据Db_sf,atti的排序，提取排序中的前c个子滤波器，并重复(43)-(46)，姿态指标存储为ANP_i,c,atti，并跳转至(49)；

(49)c<m，则c＝c+1，并重复(42)-(48)，否则，形成以惯性导航i为基准的组合导航库，记为Db_IN：

其中，IN_l,posi是由Db_sf,posi中前l个子滤波器进行组合形成的组合导航，Db_IN,posi是由IN_l,posi形成的组合导航库；IN_l,velo是由Db_sf,velo中前l个子滤波器进行组合形成的组合导航，Db_IN,velo是由IN_l,velo形成的组合导航库；IN_l,atti是由Db_sf,atti中前l个子滤波器进行组合形成的组合导航，Db_IN,atti是由IN_l,atti形成的组合导航库。

进一步地，所述步骤(5)包括以下步骤：

(51)对比Db_IN,posi中ANP_i,c,posi值最小的一组为IN_x,posi，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

其中，L_i，λ_i和h_i是第i个惯导输出的经度、纬度和高度；

和

是第x套组合导航的位置误差值；

(52)对比Db_IN,velo中ANP_i,c,velo值最小的一组为IN_y,velo，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

其中，v_ei，v_ni和v_ui是第i个惯导输出的东向、北向和天向速度；

和

是第y套组合导航的速度误差值；

(53)对比Db_IN,atti中ANP_i,c,atti值最小的一组为IN_z,atti，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

其中，C_bc是机体系到计算系的转换矩阵；其中，γ_i，θ_i和ψ_i是第i个惯导输出的横滚角、俯仰角和航向角；

和

是第z套组合导航的姿态误差值。

进一步地，所述步骤(6)包括以下步骤：

(61)对比ANP_i,posi、ANP_i,velo和ANP_i,atti，ANP_i,posi值最小的惯导编号为i_posi，ANP_i,velo值最小的惯导编号为i_velo，ANP_i,atti值最小的惯导编号为i_atti，则导航系统输出Y_out(t)为：

(62)对每一个惯性导航输出进行修正：

Y_ipout(t)＝Y_out(t)；

(63)判断导航是否结束，若是，则结束计算，否则跳转至步骤(1)。

有益效果：与现有技术相比，本发明的有益效果：本发明针对多源导航信息，形成组合导航方案库，使多源信息组合导航方案更加灵活；针对多源信息组合导航滤波算法，增加了性能指标计算和预估对比模块，使导航方案精度的预估成为可能；同时增加了精度对比和选择模块，通过在多源信息导航方案库中选择导航精度更高的导航方案，提高了导航系统整体精度；与未采用ANP预估的多源信息组合导航定位算法相比，本发明在导航传感器冗余配置的导航系统中也能选取最优的导航方案，适合实际应用。

附图说明

图1为本发明中多源导航系统结构示意图；

图2为本发明的流程图；

图3为任一惯性导航最优输出子算法流程图；

图4为多惯导的组合导航系统采用本发明后的位置、速度和姿态的输出误差曲线图；其中(a)为辅助导航系统的输出误差曲线图；(b)、(c)、(d)分别为惯性导航1采用本发明后的位置、速度和姿态的误差曲线图；(e)为惯导冗余配置的组合导航系统在采用本发明后的最优输出误差曲线图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明提供一种基于性能预估的多源信息融合自适应导航方法，通过对形成组合导航库，使多源信息组合导航方案更加灵活，并对组合导航方案的位置精度进行预估和筛选，从而提高了整个任务过程中组合导航的位置精度，同时多源信息组合导航定位算法在导航系统冗余配置情况下的适应性。具体包括以下步骤：

步骤(1)，构建如图1所示的多源导航系统，并获取多源导航系统中惯性导航及其他辅助导航的测量数据。

在多源导航系统中，包含惯性导航系统数量为n,其他导航系统数量为m，当前时刻为t。如图2所示，步骤(1)包括如下具体步骤：

步骤(1-1)，设定t＝0，并根据n个惯性导航系统的基本参数确定其误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)。

步骤(1-2)，对n个捷联惯导系统并行下列程序。

步骤(1-3)，t＝t+1。

步骤(1-4)，采集n个捷联惯性导航系统的导航信息，其中第i个捷联惯性导航系统的导航信息，存储为该导航系统预输出：

X_ipout(t)＝(γ_i θ_i ψ_i v_ei v_ni v_ui L_iλ_i h_i)

其中，γ_i、θ_i和ψ_i分别是第i个惯性导航系统输出的横滚角、俯仰角和航向角，v_ei、v_ni和v_ui分别是第i个惯性导航系统输出的东向速度、北向速度和天向速度；L_i、λ_i和h_i分别是第i个惯性导航系统输出的经度、纬度和高度。

步骤(1-5)，根据导航系统预输出信息X_ipout(t)和前一时刻的导航信息X_out(t-1)，结合飞行任务，确认当前时刻的性能评估指标，以下步骤中以位置精度作为飞行任务全程性能指标为例。

步骤(1-6)，对m个辅助导航系统并行下列程序。

步骤(1-7)，采集m个辅助导航系统信息，其中第j个辅助导航系统的信息存储为X_j(t)；

X_j(t)＝(γ_j θ_j ψ_j v_ej v_nj v_uj L_jλ_j h_j)

其中，γ_j、θ_j和ψ_j分别是第j个辅助导航系统输出的横滚角、俯仰角和航向角，v_ej、v_nj和v_uj分别是第j个辅助导航系统输出的东向速度、北向速度和天向速度；L_j、λ_j和h_j分别是第j个辅助导航系统输出的经度、纬度和高度。

步骤(2)，计算以惯性导航为基准的子滤波器。

步骤(2-1)j为第j个辅助导航，设j＝1。

步骤(2-2)提取步骤(1)中获得的第i个惯性导航系统的误差矩阵X_IMU,i(t-1)协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，通过标准卡尔曼滤波的方式将第i个惯性导航和第j个辅助导航进行信息融合，形成一个子滤波器，记为Sf_i,j并保存其误差矩阵X_i,j和协方差矩阵P_i,j。

步骤(2-3)若j≤m，则j＝j+1，并转至步骤(22)，否则，转至步骤(3)。

步骤(3)，计算子滤波器的ANP值并进行排序，形成子滤波器库。如图3所示，包括如下具体步骤：

步骤(3-1)，利用卡尔曼滤波的误差矩阵X_i,j和协方差矩阵P_i,j，提取以经纬度为单位的位置的误差矩阵和协方差矩阵，分别记为E_pos和P_pos，其计算公式如下：

步骤(3-2)，将经纬度位置误差E_pos转化为水平面内的位置误差矩阵e_pos和协方差矩阵p_pos，其表达式为：

e_pos＝[x y]

步骤(3-3)，提取速度和姿态的误差矩阵和协方差矩阵，分别记为e_velo、e_atti、p_velo和p_atti，其计算公式如下：

e_velo＝[v_e v_n v_u],e_atti＝[γ θ ψ]

p_velo＝cov[e_velo],p_atti＝cov[e_atti]。

步骤(3-4)，对位置、速度、姿态的协方差矩阵分别进行分解可得：

p＝AΛA^-1

式中A是p矩阵对应特征值λ_l的特征向量。

步骤(3-5)，根据特征向量λ_l可以获得95％误差椭圆的长短半径，其表达式为：

步骤(3-6)，根据95％误差椭圆的长短半径计算不确定度值，位置、速度和姿态评价指标分别记为ANP_i,j,posi、ANP_i,j,velo和ANP_i,j,atti，其计算公式如下为：

ANP_i,j＝a·axis_major

步骤(3-7)，分别存储ANP_i,j及其对应的误差矩阵X_i,j和协方差矩阵P_i,j。

步骤(3-8)，分别对比位置、速度和姿态的ANP_i,j值，按ANP_i,j值从小到大进行存储，形成状态序列，其公式如下：

位置序列：

速度序列：

姿态序列：

其中，Db_sf,posi是形成的位置序列，Sf_k,posi是位置序列中排在位置k的子滤波器，ANP_k,posi是该自滤波器的位置ANP值，速度序列、姿态序列与位置序列类同。

步骤(4)，根据子滤波器库，计算组合导航，形成组合导航库，包括如下具体步骤：

步骤(4-1)，设定c＝1。

步骤(4-2)，根据Db_sf,posi的排序，提取排序中的前c个子滤波器。

步骤(4-3)，提取误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，对子滤波器的信息和全局滤波器的信息进行分配，第k个子滤波器的信息分配原则如下：

步骤(4-4)，根据步骤(4-3)中的X_c,k，P_c,k和Q_c,k，采用标准卡尔曼滤波算法，求得子滤波器的误差矩阵X_i,c,k、协方差矩阵P_i,c,k和噪声方差矩阵Q_i,c,k。

步骤(4-5)，采用联邦滤波的方式对全局滤波器信息进行融合，获得全局滤波器的误差矩阵X_i,c、协方差矩阵P_i,c和噪声方差矩阵Q_i,c，其计算公式如下：

步骤(4-6)，提取组合导航误差矩阵X_i,c和协方差矩阵P_i,c，通过步骤(3-1)～步骤(3-6)，的方式计算组合导航ANP值，并存储为ANP_i,c,posi。

步骤(4-7)，根据Db_sf,velo的排序，提取排序中的前c个子滤波器，并重复步骤(4-3)～步骤(4～6)，速度指标存储为ANP_i,c,velo，并跳转至步骤(4-8)。

步骤(4-8)，根据Db_sf,atti的排序，提取排序中的前c个子滤波器，并重复步骤(4-3)～步骤(4～6)，姿态指标存储为ANP_i,c,atti，并跳转至步骤(4-9)。

步骤(4-9)，c<m，则c＝c+1，并重复步骤(4-2)～步骤(4-8)，否则，形成以惯性导航i为基准的组合导航库，其公式如下：

其中，IN_l,posi是由Db_sf,posi中前l个子滤波器进行组合形成的组合导航，Db_IN,posi是由IN_l,posi形成的组合导航库；IN_l,velo是由Db_sf,velo中前l个子滤波器进行组合形成的组合导航，Db_IN,velo是由IN_l,velo形成的组合导航库；IN_l,atti是由Db_sf,atti中前l个子滤波器进行组合形成的组合导航，Db_IN,atti是由IN_l,atti形成的组合导航库。

步骤(5)，计算组合导航的ANP值并进行对比，修正各惯性导航输出，包括如下具体步骤：

步骤(5-1)，对比Db_IN,posi中ANP_i,c,posi值最小的一组为IN_x,posi，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正，其公式如下：

其中，L_i，λ_i和h_i是第i个惯导输出的经度、纬度和高度；

和

是第x套组合导航的位置误差值。

步骤(5-2)，对比Db_IN,velo中ANP_i,c,velo值最小的一组为IN_y,velo，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正，其公式如下：

其中，v_ei，v_ni和v_ui是第i个惯导输出的东向、北向和天向速度；

和

是第y套组合导航的速度误差值。

步骤(5-3)，对比Db_IN,atti中ANP_i,c,atti值最小的一组为IN_z,atti，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正，其公式如下：

其中，C_bc是机体系到计算系的转换矩阵；γ_i，θ_i和ψ_i是第i个惯导输出的横滚角、俯仰角和航向角；

和

是第z套组合导航的姿态误差值。

步骤(6)，对比各惯导ANP值，并选取对应信息形成最优导航输出，包括如下具体步骤：

步骤(6-1)，对比ANP_i,posi、ANP_i,velo和ANP_i,atti，ANP_i,posi值最小的惯导编号为i_posi，ANP_i,velo值最小的惯导编号为i_velo，ANP_i,atti值最小的惯导编号为i_atti，则导航系统输出X_out(t)的公式如下：

步骤(6-2)，对每一个惯性导航输出进行修正，其公式如下：

X_ipout(t)＝X_out(t)。

步骤(6-3)，若t<t_end，则t＝t+1，并重复步骤(1)～步骤(6)；否则结束导航。

为了验证本发明所提出的一种基于性能预估的多源信息融合自适应导航方法的有效性，进行数字仿真分析。仿真选取2套惯性导航系统配合3套辅助导航系统模拟惯性导航冗余配置情况下的本发明方法的实用性。图4是该情况下采用本发明后的各惯导组合导航库及最终输出的位置、速度和姿态的误差曲线图。

对比图4(a)与图4(b)、图4(c)和图4(d)可以看出，采用本发明，惯性导航系统可以通过对比性能指标，在位置、速度和姿态等各方面选取最优的组合方式进行修正，能够有效的在系统所包含的多种组合导航方式中，选取当前时刻预估性能最优的组合方式和导航输出信息，合成最优的组合导航输出。与未优化前相比采用本发明方法进行预估和筛选后，导航精度和稳定性有一定的提升。而在导航系统工作过程中，随着部分导航传感器的阶段性失效，采用本发明方法可以实时的在组合导航库中选取最优的组合方式，有效的避免了失效导航系统对整体导航精度的影响。同时，在导航方式重启的过程中，能够实时防止其精度未收敛时干扰组合导航结果。

由图4(e)可以看出，在多套惯导冗余并行工作的情况下，本发明方法可以实现系统根据其工作状态和精度进行自适应调整输出，选取惯性导航系统中精度最佳的导航信息作为最优输出，具有良好的应用价值。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种基于性能预估的多源信息融合自适应导航方法，其特征在于，包括以下步骤：

(1)获取多源导航系统中惯性导航及其他辅助导航的测量数据；

(2)计算以惯性导航为基准的子滤波器；

(3)计算子滤波器的性能指标并进行排序，形成状态序列；

(4)根据状态序列，计算组合导航，形成组合导航库；

(5)计算组合导航的ANP值并进行对比，修正各惯性导航输出；

(6)对比各惯导ANP值，并选取对应信息形成最优导航输出。

2.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(1)包括以下步骤：

(11)惯性导航系统数量为n,其他辅助导航系统数量为m，当前时刻为t；

(12)设定t＝0，并根据n个惯性导航系统的基本参数确定其误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)；

(13)对冗余配置的n个惯性导航系统同时执行(14)-(16)；

(14)t＝t+1；

(15)采集捷联惯性导航系统的导航信息，其中第i个捷联惯性导航系统的导航信息，存储为该导航系统预输出：

Y_ipout(t)＝(γ_i θ_i ψ_i v_ei v_ni v_ui L_i λ_i h_i)

其中，γ_i、θ_i和ψ_i分别是第i个惯性导航系统输出的横滚角、俯仰角和航向角，v_ei、v_ni和v_ui分别是第i个惯性导航系统输出的东向速度、北向速度和天向速度；L_i、λ_i和h_i分别是第i个惯性导航系统输出的经度、纬度和高度；

(16)根据导航系统预输出信息Y_ipout(t)和前一时刻的导航信息Y_out(t-1)，结合飞行任务，确认当前时刻的性能评估指标；

(17)对m个辅助导航系统执行(18)；

(18)采集m个辅助导航系统信息，其中第j个辅助导航系统的信息存储为Y_j(t)：

Y_j(t)＝(γ_j θ_j ψ_j v_ej v_nj v_uj L_j λ_j h_j)

其中，γ_j、θ_j和ψ_j分别是第j个辅助导航系统输出的横滚角、俯仰角和航向角，v_ej、v_nj和v_uj分别是第j个辅助导航系统输出的东向速度、北向速度和天向速度；L_j、λ_j和h_j分别是第j个辅助导航系统输出的经度、纬度和高度。

3.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(2)包括以下步骤：

(21)j为第j个辅助导航，设j＝1；

(22)提取步骤(1)中获得的第i个惯性导航系统的误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，通过标准卡尔曼滤波的方式将第i个惯性导航和第j个辅助导航进行信息融合，形成一个子滤波器，记为Sf_i,j，并保存其误差矩阵X_i,j和协方差矩阵P_i,j；

(23)若j≤m，则j＝j+1，并转至步骤(22)，否则，转至步骤(3)。

4.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(3)包括以下步骤：

(31)利用卡尔曼滤波的误差矩阵X_i,j和协方差矩阵P_i,j，提取以经纬度为单位的位置的误差矩阵和协方差矩阵，分别记为E_pos和P_pos：

(32)将经纬度位置误差E_pos转化为水平面内的位置误差矩阵e_pos和协方差矩阵p_pos：

e_pos＝[δx δy]

(33)提取速度和姿态的误差矩阵和协方差矩阵，分别记为e_velo、e_atti、p_velo和p_atti：

e_velo＝[δv_e δv_n δv_u],e_atti＝[δγ δθ δψ]

p_velo＝cov[e_velo],p_atti＝cov[e_atti]；

(34)对位置、速度、姿态的协方差矩阵分别进行分解得：

p＝AΛA^-1

式中，A是p矩阵对应特征值λ_l的特征向量；

(35)根据特征向量λ_l可以获得95％误差椭圆的长短半径，其表达式为：

(36)根据95％误差椭圆的长短半径计算不确定度值，位置、速度和姿态评价指标分别记为ANP_i,j,posi、ANP_i,j,velo和ANP_i,j,atti：

ANP_i,j＝a·axis_major

(37)分别存储ANP_i,j及其对应的误差矩阵X_i,j和协方差矩阵P_i,j；

(38)分别对比位置、速度和姿态的ANP_i,j值，按ANP_i,j值从小到大进行存储，形成状态序列，其公式如下：

位置序列：

速度序列：

姿态序列：

其中，Db_sf,posi是形成的位置序列，Sf_k,posi是位置序列中排在位置k的子滤波器，ANP_k,posi是该自滤波器的位置ANP值，速度序列、姿态序列与位置序列类同。

5.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(4)包括以下步骤：

(41)设定c＝1；

(42)根据Db_sf,posi的排序，提取排序中的前c个子滤波器；

(43)提取误差矩阵X_IMU,i(t-1)、协方差矩阵P_IMU,i(t-1)和噪声方差矩阵Q_IMU,i(t-1)，对子滤波器的信息和全局滤波器的信息进行分配，第k个子滤波器的信息分配原则如下：

(44)根据(43)中的X_c,k，P_c,k和Q_c,k，采用标准卡尔曼滤波算法，求得子滤波器的误差矩阵X_i,c,k、协方差矩阵P_i,c,k和噪声方差矩阵Q_i,c,k；

(45)采用联邦滤波的方式对全局滤波器信息进行融合，获得全局滤波器的误差矩阵X_i,c、协方差矩阵P_i,c和噪声方差矩阵Q_i,c，其计算公式如下：

(46)提取组合导航误差矩阵X_i,c和协方差矩阵P_i,c，计算组合导航ANP值，并存储为ANP_i,c,posi；

(47)根据Db_sf,velo的排序，提取排序中的前c个子滤波器，并重复(43)-(46)，速度指标存储为ANP_i,c,velo，并跳转至步骤(47)；

(48)根据Db_sf,atti的排序，提取排序中的前c个子滤波器，并重复(43)-(46)，姿态指标存储为ANP_i,c,atti，并跳转至(49)；

(49)c<m，则c＝c+1，并重复(42)-(48)，否则，形成以惯性导航i为基准的组合导航库，记为Db_IN：

其中，IN_l,posi是由Db_sf,posi中前l个子滤波器进行组合形成的组合导航，Db_IN,posi是由IN_l,posi形成的组合导航库；IN_l,velo是由Db_sf,velo中前l个子滤波器进行组合形成的组合导航，Db_IN,velo是由IN_l,velo形成的组合导航库；IN_l,atti是由Db_sf,atti中前l个子滤波器进行组合形成的组合导航，Db_IN,atti是由IN_l,atti形成的组合导航库。

6.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(5)包括以下步骤：

(51)对比Db_IN,posi中ANP_i,c,posi值最小的一组为IN_x,posi，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

ANP_i,posi＝min(ANP_i,c,posi)＝ANP_i,INx,posi

其中，L_i，λ_i和h_i是第i个惯导输出的经度、纬度和高度；

和

是第x套组合导航的位置误差值；

(52)对比Db_IN,velo中ANP_i,c,velo值最小的一组为IN_y,velo，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

ANP_i,velo＝min(ANP_i,c,velo)＝ANP_i,INy,velo

其中，v_ei，v_ni和v_ui是第i个惯导输出的东向、北向和天向速度；

和

是第y套组合导航的速度误差值；

(53)对比Db_IN,atti中ANP_i,c,atti值最小的一组为IN_z,atti，记录其位置性能指标并保存采用其输出对惯性导航i进行位置修正：

ANP_i,atti＝min(ANP_i,c,atti)＝ANP_i,INz,atti

其中，C_bc是机体系到计算系的转换矩阵；其中，γ_i，θ_i和ψ_i是第i个惯导输出的横滚角、俯仰角和航向角；

和

是第z套组合导航的姿态误差值。

7.根据权利要求1所述的基于性能预估的多源信息融合自适应导航方法，其特征在于，所述步骤(6)包括以下步骤：

(61)对比ANP_i,posi、ANP_i,velo和ANP_i,atti，ANP_i,posi值最小的惯导编号为i_posi，ANP_i,velo值最小的惯导编号为i_velo，ANP_i,atti值最小的惯导编号为i_atti，则导航系统输出Y_out(t)为：

(62)对每一个惯性导航输出进行修正：

Y_ipout(t)＝Y_out(t)；

(63)判断导航是否结束，若是，则结束计算，否则跳转至步骤(1)。

Images