CN110579740B - 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 - Google Patents

Abstract

本发明公开了一种基于自适应联邦卡尔曼滤波的无人船组合导航方法，利用无人船组合导航系统进行导航，所述无人船组合导航系统包括SINS、GPS、Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合，输出无人船的位置、速度和姿态信息。本发明应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型，减小各个子系统之间的故障干扰，提高无人船导航系统的可靠性与稳定性。本发明在联邦卡尔曼滤波的基础上，设计联邦卡尔曼滤波的子滤波器的信息分配因子，在保障系统的容错能力的前提下，能够有效抑制系统的异常扰动，减小分配原则对滤波精度的影响。

Description

一种基于自适应联邦卡尔曼滤波的无人船组合导航方法

技术领域

本发明属于传感器信息融合领域，尤其涉及一种基于自适应联邦卡尔曼滤波的无人船组合导航方法。

背景技术

由于在海上应用的灵活性和多功能性，无人船在军事生产等各个方面都有着广泛的应用。联邦卡尔曼滤波应用于无人船、无人机、无人车上，组合导航可以检测出导航子系统的故障，将正常的子系统的导航信息进行组合，提高系统的容错性与自适应性。卡尔曼滤波、粒子滤波、联邦滤波等方法广泛用于组合导航传感器的信息融合。针对多传感器的信息融合领域，联邦卡尔曼滤波效果最为显著，但是其滤波精度受信息分配原则影响，以及在载体发生异常扰动时滤波精度较低的问题。

发明内容

为解决现有技术的上述问题，本发明要设计一种能够提高滤波精度的基于自适应联邦卡尔曼滤波的无人船组合导航方法。

为实现上述目的，本发明的技术方案如下：

一种基于自适应联邦卡尔曼滤波的无人船组合导航方法，利用无人船组合导航系统进行导航，所述无人船组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合，输出无人船的位置、速度和姿态信息。所述自适应联邦卡尔曼滤波算法基于自适应联邦卡尔曼滤波器实现。所述自适应联邦卡尔曼滤波器包括主滤波器、SINS/GPS子滤波器、SINS/Compass子滤波器和自适应信息分配因子计算模块；所述主滤波器分别与SINS/GPS子滤波器和SINS/Compass子滤波器双向连接。所述SINS/GPS子滤波器输出位置、速度信息的状态估计值和估计误差协方差矩阵，SINS/Compass子滤波器输出姿态信息的状态估计值和估计均方误差协方差矩阵。自适应信息分配因子计算模块通过两个子滤波器的预测状态矢量的估计均方误差协方差矩阵计算出自适应信息分配因子，将归一化后的自适应信息分配因子送入主滤波器。主滤波器内进行时间更新过程和信息融合过程，将两个子滤波器的状态估计值和状态估计均方误差协方差矩阵与主滤波器的状态估计值进行信息融合，得到全局最优状态估计值，并反馈到两个子滤波器，进行子滤波器的重置。

所述组合导航方法，包括以下步骤：

A、建立无人船组合导航系统的状态方程

选择东北天地理坐标系为导航坐标系，状态方程考虑捷联惯导系统、加速度计、陀螺仪的误差因素，则所述组合导航系统的状态方程为：

其中，

为组合导航系统的状态估计值，F为组合导航系统的状态转移矩阵，X为组合导航系统的状态变量，G为组合导航系统的控制矩阵，W为组合导航系统的白噪声矩阵。

组合导航系统的状态变量X为：

式中，

分别为无人船东、北、天方向的姿态误差角；δV_E、δV_N、δV_U分别为无人船东、北、天方向的速度误差；δλ、δL、δh分别为无人船的纬度、经度、高度的误差；ε_bx、ε_by、ε_bz分别为载体系下陀螺仪东、北、天方向的常值漂移；ε_rx、ε_ry、ε_rz分别为载体系下陀螺仪东、北、天方向的慢变漂移；▽_x、▽_y、▽_z分别为载体系下加速度计东、北、天方向的常值随机误差。

组合导航系统的白噪声矩阵W为：

W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

其中，ω_gx、ω_gy、ω_gz分别为陀螺仪东、北、天方向的白噪声；ω_rx、ω_ry、ω_rz分别为陀螺仪东、北、天方向的一阶马尔科夫过程驱动白噪声；ω_ax、ω_ay、ω_az为加速度计东、北、天方向的一阶马尔科夫过程驱动白噪声。

陀螺仪的误差模型为：

ε＝ε_b+ε_r+ω_g

式中，ε_b为随机常数；ε_r为一阶马尔科夫过程随机噪声，ε_r满足下式：

T_g为陀螺仪相关时间，ω_r为陀螺仪的一阶马尔科夫过程驱动白噪声。

加速度计的误差模型为：

▽＝▽_a

式中，▽为加速度计的误差，

T_a为加速度计相关时间，ω_a为加速度计的一阶马尔科夫过程驱动白噪声，▽_a为加速度计的速度误差。

组合导航系统的状态转移矩阵为：

式中，F_N为9行9列的基本导航参数的系统阵，具体如下：

/>

F_N(2,7)＝-ω_ie sinL

F_N(4,2)＝-f_U

F_N(5,1)＝f_U

F_N(5,3)＝-f_E

F_N(6,1)＝-f_N

F_N(6,2)＝-f_E

F_N(6,7)＝-2V_Eω_ie sinL

/>

F_N(9,6)＝1

其余元素为0。

其中ω_ie为地球自转角速率，f_E、f_U分别为东、天方向的地球偏扁率，R_E为地球赤道半径，R_M地球表面上的任一点处沿子午圈的主曲率半径；R_N为地球表面上的任一点处沿卯酉圈的主曲率半径；L为经度。

F_M＝diag{0，0,0,-1/T_rx,-1/T_ry,-1/T_rz,-1/T_ax,-1/T_ay,-1/T_az}

式中：

θ、

ψ分别为无人船的横滚角、俯仰角、航向角。

组合导航系统的控制矩阵为：

B、设计SINS/GPS子滤波器

SINS/GPS子滤波器采用位置和速度组合模式，建立SINS/GPS子滤波器的位置、速度的量测方程为：

Z₁＝HX₁+V₁

Z₁为SINS/GPS子滤波器的量测值，H₁为SINS/GPS子滤波器的量测矩阵，V₁为SINS/GPS子滤波器的量测噪声。

SINS的速度信息为：

GPS的速度信息为：

SINS的位置信息为：

GPS的位置信息为：

式中，v_IE、v_IN、v_IU分别为SINS东、北、天方向的速度信息；v_GE、v_GN、v_GU分别为GPS东、北、天方向的速度信息；λ_I、L_I、h_I分别为SINS东、北、天方向的位置信息；λ_G、L_G、h_G分别为GPS东、北、天方向的位置信息；v_N、v_E、v_U分别为无人船东、北、天方向的速度真值；λ_t、L_t、h_t分别为无人船纬度、经度、高度的位置真值；δv_E、δv_N、δv_U分别为SINS东、北、天方向的速度误差；M_N、M_E、M_U分别为GPS东、北、天方向的速度误差；δλ、δL、δh分别为SINS的纬度、经度、高度的误差，N_N、N_E、N_U分别为GPS的纬度、经度、高度的误差。

取SINS和GPS的速度差值作为SINS/GPS子滤波器量测值，定义速度量测方程为：

式中：

Z_V为SINS/GPS子滤波器速度信息的量测值，H_V为SINS/GPS子滤波器速度信息的量测矩阵，V_V为SINS/GPS子滤波器速度信息的量测噪声，O_3×3为3行3列的零矩阵，O_3×12为3行12列的零矩阵。

取SINS和GPS的位置差值作为SINS/GPS子滤波器量测值，定义位置量测方程为：

式中：

V_p＝[N_N,N_E,N_U]^T

R_M＝R_e(1-2f+3fsin²L)

R_N＝R_e(1+fsin²L)

式中：Z_P为SINS/GPS子滤波器速度信息的量测值，H_P为SINS/GPS子滤波器速度信息的量测矩阵，V_P为SINS/GPS子滤波器速度信息的量测噪声，V_P当做白噪声处理；R_e为地球赤道半径；取6378137m；f为地球扁率，取1/298.257；O_3×6为3行6列的零矩阵；O_3×9为3行9列的零矩阵。

采用速度与位置的组合方式，建立SINS/GPS子滤波器的量测方程为：

C、设计SINS/Compass子滤波器

建立无人船组合导航SINS/Compass子滤波器的姿态的量测方程为：

Z₂＝H₂X+V₂

式中，Z₂为SINS/Compass子滤波器的量测值，H₂为SINS/Compass子滤波器的量测矩阵，V₂为SINS/Compass子滤波器的量测噪声。

SINS的姿态信息为：

Compass的姿态信息为：

式中，

分别为SINS东、北、天方向的姿态信息；/>

分别为Compass东、北、天的姿态信息；/>

分别为无人船东、北、天方向的姿态真值；

分别为SINS东、北、天方向的姿态误差；δα_E、δα_N、δα_U分别为Compass的东、北、天方向的姿态误差。

将SINS和Compass输出的姿态信息的差值作为SINS/Compass子滤波器的测量值，则观测方程为：

式中，H₂＝[I_3×3,0_3×15]，I_3×3为3行3列的单位矩阵，O_3×15为3行15列单位矩阵，V₂＝[δα_c,δβ_c,δγ_c]^T是均值为零的观测白噪声。

D、实现自适应联邦卡尔曼滤波的过程

SINS/GPS子滤波器和SINS/Compass子滤波器并行运行，将SINS、GPS、Compass数据进行分散式处理，在主滤波器中进行信息融合。两个子滤波器的估计误差协方差阵及状态估计值被送入主滤波器，并与主滤波器的估计值进行信息融合得到全局最优估计。按照信息分配原则将全局最优估计反馈到子滤波器，完成一次滤波，并重置两个子滤波器的估计值。

所述自适应联邦卡尔曼滤波器的信息融合过程在主滤波器内进行，包括信息分配、时间更新、量测更新、信息融合四个步骤，具体步骤如下：

D1、信息分配

将全局的状态估计、误差方差阵、噪声信息分配给两个子滤波器和主滤波器，分配原则为：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，P_i,k-1为子滤波器k-1时刻的估计均方误差协方差矩阵，P_g,k-1为k-1时刻的全局最优估计均方误差协方差矩阵，Q_i,k-1为子滤波器噪声k-1时刻的系统噪声方差阵，Q_g,k-1为主滤波器噪声k-1时刻的系统噪声方差阵，

为子滤波器k-1时刻的状态估计，/>

为主滤波器k-1时刻的状态估计。

信息分配因子β_i满足信息守恒原则，即

主滤波器中无信息分配，执行状态估计和协方差阵的时间更新，即P_m,k＝0。

D2、时间更新

时间更新在两个子滤波器间独立进行，分别获得两个子滤波器的状态预测矢量和一步预测均方误差协方差矩阵：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，P_i,k|k-1为子滤波器的一步预测均方误差协方差矩阵，

为子滤波器的状态预测矢量，Φ_i,k|k-1为子滤波器k-1时刻到k时刻的一步转移矩阵，Γ_i,k-1为噪声驱动阵。

D3、量测更新

两个子滤波器接收量测信息并独立进行量测更新过程，分别获得两个子滤波器的状态估计值、滤波增益和估计均方误差协方差矩阵，并传至主滤波器：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，

为子滤波器k时刻的状态估计，K_i,k为子滤波器k时刻的滤波增益，P_i,k为估计均方误差协方差矩阵，Z_i,k为子滤波器k时刻的量测值，H_i,k子滤波器k时刻的量测矩阵，R_i,k子滤波器k时刻的量测噪声方差阵，I单位矩阵。

量测更新过程仅在子滤波器内进行，主滤波器内只进行时间更新过程。

其中

为主滤波器k-1时刻的状态估计，/>

为主滤波器k-1时刻的状态预测矢量。/>

D4、信息融合

把两个子滤波器的状态估计信息和主滤波器的状态估计进行融合，从而得到全局状态估计信息：

式中，P_g,k为k时刻的全局最优估计均方误差协方差矩阵，P_1,k为SINS/GPS子滤波器k时刻的估计均方误差协方差矩阵，P_2,k为SINS/Compass子滤波器k时刻的估计均方误差协方差矩阵，P_m,k为主滤波器k时刻的估计均方误差协方差矩阵，

为k时刻的全局最优状态估计。

D5、计算自适应信息分配因子

最优自适应因子满足以下条件：

最优自适应因子表示如下：

几何状态矢量与预测状态矢量的偏差为：

其中，几何状态矢量

为：

式中，∑_i,k为Z_i,k的等效权矩阵。

其中，

为预测状态矢量，P_i,k|k-1为预测状态矢量的估计协方差矩阵，/>

为预测状态矢量的理论协方差矩阵。

当估计量基于当前时刻的预测状态矢量，预测状态矢量的估计协方差矩阵如下：

最优自适应因子α_k的期望值小于1，表示为：

式中：

/>

则最优自适应因子基于预测状态矢量的估计协方差矩阵表示为：

由上式表示的自适应因子类似于由模型的预测状态与量测的估计状态之间的差异构造的自适应因子。

自适应因子表示为预测状态矢量的形式为：

其中，c为常数，

为基于预测状态矢量/>

构造的统计量，其表达式为：

通过基于预测状态矢量的最优自适应因子的计算方法求得自适应联邦卡尔曼滤波器的自适应信息分配因子为：

其中，b为常数，取0.85～1.0。

为确保自适应信息分配因子满足信息守恒定律，对自适应信息分配因子进行归一化处理。自适应信息分配因子归一化为：

式中，β'_i,k为归一化后的子滤波器在k时刻的自适应信息分配因子。

与现有技术相比，本发明具有以下有益效果：

1、本发明提出一种无人船的SINS/GPS/Compass组合导航系统，应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型，采用联邦卡尔曼滤波，减小各个子系统之间的故障干扰，提高无人船导航系统的可靠性与稳定性。

2、本发明在联邦卡尔曼滤波的基础上，根据预测状态矢量的估计均方误差协方差矩阵的最优自适应因子的计算方法，设计联邦卡尔曼滤波的子滤波器的信息分配因子，提出一种自适应联邦卡尔曼滤波算法，仿真实验证明了本发明有效补偿了系统的异常扰动和模型误差，相比于非自适应联邦卡尔曼滤波算法，在保障系统的容错能力的前提下，能够有效抑制系统的异常扰动，减小分配原则对滤波精度的影响。

3、本发明采用计算基于预测状态矢量的估计均方误差协方差矩阵的最优自适应因子的方法选取联邦滤波器的信息分配因子，相比于预测残差的最优自适应因子的计算方法，该方法具有更高的可靠性。

附图说明

图1是本发明的流程示意图。

图2是纬度误差的EKF、AEKF仿真波形图。

图3是经度误差的EKF、AEKF仿真波形图。

图4是高度误差的EKF、AEKF仿真波形图。

图5是东向速度误差的EKF、AEKF仿真波形图。

图6是北向速度误差的EKF、AEKF仿真波形图。

图7是天向速度误差的EKF、AEKF仿真波形图。

图8是东向姿态误差的EKF、AEKF仿真波形图。

图9是北向姿态误差的EKF、AEKF仿真波形图。

图10是天向姿态误差的EKF、AEKF仿真波形图。

具体实施方式

下面结合附图对本发明进行进一步地描述。

本发明的自适应联邦卡尔曼滤波器是基于现有联邦卡尔曼滤波器改进而来，因为联邦卡尔曼滤波器的子滤波器是对状态估计值的权矩阵调整，自适应联邦卡尔曼滤波器是对状态估计值的协方差阵调整，因此两者的修正参数具有等价性。

如果采用自适应联邦卡尔曼滤波的自适应因子来表示联邦卡尔曼滤波器的信息分配因子，可以提高联邦卡尔曼滤波器的自适应性，下面对两者之间的等价性进行证明。

设子滤波器的滤波估计值、主滤波器的滤波估计值和全局状态估计分别为X_i,k、X_m,k、X_g,k，相应的权矩阵分别为∑_i,k、∑_m,k、∑_g,k，相应的协方差矩阵分别为P_i,k、P_m,k、P_g,k。

自适应联邦卡尔曼滤波通过自适应因子使滤波参数自适应调整，从而获得最优的滤波效果。

利用最小二乘原理，自适应联邦卡尔曼滤波的极值原则为：

其中，α_k为自适应因子，V_k和

分别为状态预测信息和观测向量的误差向量，相应的协方差矩阵分别为∑_k和/>

根据预测状态矢量和观测矢量的误差方差推导出极值函数为：

其中λ_k为拉格朗日乘数。

则自适应联邦卡尔曼滤波解为：

其中，

为自适应联邦卡尔曼滤波的增益矩阵为：

由于：

且/>

所以：

将联邦卡尔曼滤波的增益矩阵写成下式：

如果α_k＝β_i则G_ik和

等价，说明联邦卡尔曼滤波器的子滤波器和自适应联邦卡尔曼滤波器在形式上是等价的。

通过以上证明可以得出，联邦卡尔曼滤波器的信息分配因子可以通过最优自适应因子的计算方法计算得到。因而提出一种自适应联邦卡尔曼滤波算法，能够增加子滤波器的自适应性，提高滤波精度。

为了说明本发明的有效性和可行性，在SINS的误差模型下进行Matlab仿真，对本发明提出的自适应联邦卡尔曼滤波算法进行验证。假设无人船的初始位置为东经121.4°，北纬39.0°。传感器的误差参数设置为：陀螺仪的常值漂移为pi/180°/h，陀螺仪的慢变漂移为pi/180°/h，加速度计的偏置为1g，三维电子罗盘的姿态误差为0.5°，GPS的测速误差为0.1m/s，位置误差为10m。

表1展示联邦卡尔曼滤波方法(EKF)与自适应联邦卡尔曼滤波方法(AEKF)的仿真误差参数范围对比，从图中可以看出自适应联邦卡尔曼滤波算法(AEKF)相比于联邦卡尔曼滤波算法(EKF)的误差均可以收敛于更小的范围，当EKF算法能将误差较大时，AEKF算法仍能降低误差，控制误差在较小的范围，EKF算法获得较小误差时，AEKF算法的误差也能控制在较小的范围。图1展示的是SINS/GPS/Compass联邦滤波器结构图，从图中可以看出自滤波器和主滤波器的数据处理过程。图2展示的是纬度、经度、高度误差的EKF、AEKF仿真波形图，从图中可以看出AEKF算法的纬度、经度、高度误差都可以控制在较小的范围内，相比于EKF算法效果更佳。图3展示的是东向、北向、天向速度误差的EKF、AEKF仿真波形图，从图中可以看出AEKF算法的东向、北向、天向速度误差都可以控制在较小的范围内，相比于EKF算法效果更佳。图4展示的是东向、北向、天向姿态误差的EKF、AEKF仿真波形图，从图中可以看出EKF算法的天向姿态误差的范围过大，AEKF算法较好的控制了误差，将误差控制在合理范围内。可以得出结论：以上实验验证了本发明所提出的算法方案的有效性和优越性。

表1 EKF和AEKF仿真误差参数比对

	纬度误差	经度	高度
				EKF	-1.885～1.939	-0.710～0.618	-3.857～4.010
AEKF	-1.840～1.435	-0.714～0.616	-0.592～0.488
					东向速度误差	北向速度误差	天向速度误差
EKF	-0.693～1.201	-3.693～14.896	-0.505～0.745
				AEKF	-0.693～1.201	-0.559～1.226	-0.377～0.586
	东向姿态误差	北向姿态误差	天向姿态误差
				EKF	-0.209～0.266	-0.210～0.268	-3690.023～4708.490
AEKF	-0.224～0.246	-0.210～0.267	-51.710～100.228

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。

Claims (1)

1.一种基于自适应联邦卡尔曼滤波的无人船组合导航方法，其特征在于：利用无人船组合导航系统进行导航，所述无人船组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即Compass和嵌入式处理器；所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合，输出无人船的位置、速度和姿态信息；所述自适应联邦卡尔曼滤波算法基于自适应联邦卡尔曼滤波器实现；所述自适应联邦卡尔曼滤波器包括主滤波器、SINS/GPS子滤波器、SINS/Compass子滤波器和自适应信息分配因子计算模块；所述主滤波器分别与SINS/GPS子滤波器和SINS/Compass子滤波器双向连接；所述SINS/GPS子滤波器输出位置、速度信息的状态估计值和估计误差协方差矩阵，SINS/Compass子滤波器输出姿态信息的状态估计值和估计均方误差协方差矩阵；自适应信息分配因子计算模块通过两个子滤波器的预测状态矢量的估计均方误差协方差矩阵计算出自适应信息分配因子，将归一化后的自适应信息分配因子送入主滤波器；主滤波器内进行时间更新过程和信息融合过程，将两个子滤波器的状态估计值和状态估计均方误差协方差矩阵与主滤波器的状态估计值进行信息融合，得到全局最优状态估计值，并反馈到两个子滤波器，进行子滤波器的重置；

所述组合导航方法，包括以下步骤：

A、建立无人船组合导航系统的状态方程

选择东北天地理坐标系为导航坐标系，状态方程考虑捷联惯导系统、加速度计、陀螺仪的误差因素，则所述组合导航系统的状态方程为：

其中，

为组合导航系统的状态估计值，F为组合导航系统的状态转移矩阵，X为组合导航系统的状态变量，G为组合导航系统的控制矩阵，W为组合导航系统的白噪声矩阵；

组合导航系统的状态变量X为：

式中，

分别为无人船东、北、天方向的姿态误差角；δV_E、δV_N、δV_U分别为无人船东、北、天方向的速度误差；δλ、δL、δh分别为无人船的纬度、经度、高度的误差；ε_bx、ε_by、ε_bz分别为载体系下陀螺仪东、北、天方向的常值漂移；ε_rx、ε_ry、ε_rz分别为载体系下陀螺仪东、北、天方向的慢变漂移；/>

分别为载体系下加速度计东、北、天方向的常值随机误差；

组合导航系统的白噪声矩阵W为：

W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

其中，ω_gx、ω_gy、ω_gz分别为陀螺仪东、北、天方向的白噪声；ω_rx、ω_ry、ω_rz分别为陀螺仪东、北、天方向的一阶马尔科夫过程驱动白噪声；ω_ax、ω_ay、ω_az为加速度计东、北、天方向的一阶马尔科夫过程驱动白噪声；

陀螺仪的误差模型为：

ε＝ε_b+ε_r+ω_g

式中，ε_b为随机常数；ε_r为一阶马尔科夫过程随机噪声，ε_r满足下式：、

T_g为陀螺仪相关时间，ω_r为陀螺仪的一阶马尔科夫过程驱动白噪声；

加速度计的误差模型为：

式中，

为加速度计的误差，/>

T_a为加速度计相关时间，ω_a为加速度计的一阶马尔科夫过程驱动白噪声，/>

为加速度计的速度误差；

组合导航系统的状态转移矩阵为：

式中，F_N为9行9列的基本导航参数的系统阵，具体如下：

F_N(2,7)＝-ω_ie sinL

F_N(4,2)＝-f_U

F_N(5,1)＝f_U

F_N(5,3)＝-f_E

F_N(6,1)＝-f_N

F_N(6,2)＝-f_E

F_N(6,7)＝-2V_Eω_ie sinL

F_N(9,6)＝1

其余元素为0；

其中ω_ie为地球自转角速率，f_E、f_U分别为东、天方向的地球偏扁率，R_E为地球赤道半径，R_M地球表面上的任一点处沿子午圈的主曲率半径；R_N为地球表面上的任一点处沿卯酉圈的主曲率半径；L为经度；

F_M＝diag{0，0,0,-1/T_rx,-1/T_ry,-1/T_rz,-1/T_ax,-1/T_ay,-1/T_az}

式中：

θ、

ψ分别为无人船的横滚角、俯仰角、航向角；

组合导航系统的控制矩阵为：

B、设计SINS/GPS子滤波器

SINS/GPS子滤波器采用位置和速度组合模式，建立SINS/GPS子滤波器的位置、速度的量测方程为：

Z₁＝HX₁+V₁

Z₁为SINS/GPS子滤波器的量测值，H₁为SINS/GPS子滤波器的量测矩阵，V₁为SINS/GPS子滤波器的量测噪声；

SINS的速度信息为：

GPS的速度信息为：

SINS的位置信息为：

GPS的位置信息为：

式中，v_IE、v_IN、v_IU分别为SINS东、北、天方向的速度信息；v_GE、v_GN、v_GU分别为GPS东、北、天方向的速度信息；λ_I、L_I、h_I分别为SINS东、北、天方向的位置信息；λ_G、L_G、h_G分别为GPS东、北、天方向的位置信息；v_N、v_E、v_U分别为无人船东、北、天方向的速度真值；λ_t、L_t、h_t分别为无人船纬度、经度、高度的位置真值；δv_E、δv_N、δv_U分别为SINS东、北、天方向的速度误差；M_N、M_E、M_U分别为GPS东、北、天方向的速度误差；δλ、δL、δh分别为SINS的纬度、经度、高度的误差，N_N、N_E、N_U分别为GPS的纬度、经度、高度的误差；

取SINS和GPS的速度差值作为SINS/GPS子滤波器量测值，定义速度量测方程为：

式中：

Z_V为SINS/GPS子滤波器速度信息的量测值，H_V为SINS/GPS子滤波器速度信息的量测矩阵，V_V为SINS/GPS子滤波器速度信息的量测噪声，O_3×3为3行3列的零矩阵，O_3×12为3行12列的零矩阵；

取SINS和GPS的位置差值作为SINS/GPS子滤波器量测值，定义位置量测方程为：

式中：

V_p＝[N_N,N_E,N_U]^T

R_M＝R_e(1-2f+3fsin²L)

R_N＝R_e(1+fsin²L)

式中：Z_P为SINS/GPS子滤波器速度信息的量测值，H_P为SINS/GPS子滤波器速度信息的量测矩阵，V_P为SINS/GPS子滤波器速度信息的量测噪声，V_P当做白噪声处理；R_e为地球赤道半径；取6378137m；f为地球扁率，取1/298.257；O_3×6为3行6列的零矩阵；O_3×9为3行9列的零矩阵；

采用速度与位置的组合方式，建立SINS/GPS子滤波器的量测方程为：

C、设计SINS/Compass子滤波器

建立无人船组合导航SINS/Compass子滤波器的姿态的量测方程为：

Z₂＝H₂X+V₂

式中，Z₂为SINS/Compass子滤波器的量测值，H₂为SINS/Compass子滤波器的量测矩阵，V₂为SINS/Compass子滤波器的量测噪声；

SINS的姿态信息为：

Compass的姿态信息为：

式中，

分别为SINS东、北、天方向的姿态信息；/>

分别为Compass东、北、天的姿态信息；/>

分别为无人船东、北、天方向的姿态真值；/>

分别为SINS东、北、天方向的姿态误差；δα_E、δα_N、δα_U分别为Compass的东、北、天方向的姿态误差；

将SINS和Compass输出的姿态信息的差值作为SINS/Compass子滤波器的测量值，则观测方程为：

式中，H₂＝[I_3×3,0_3×15]，I_3×3为3行3列的单位矩阵，O_3×15为3行15列单位矩阵，V₂＝[δα_c,δβ_c,δγ_c]^T是均值为零的观测白噪声；

D、实现自适应联邦卡尔曼滤波的过程

SINS/GPS子滤波器和SINS/Compass子滤波器并行运行，将SINS、GPS、Compass数据进行分散式处理，在主滤波器中进行信息融合；两个子滤波器的估计误差协方差阵及状态估计值被送入主滤波器，并与主滤波器的估计值进行信息融合得到全局最优估计；按照信息分配原则将全局最优估计反馈到子滤波器，完成一次滤波，并重置两个子滤波器的估计值；

所述自适应联邦卡尔曼滤波器的信息融合过程在主滤波器内进行，包括信息分配、时间更新、量测更新、信息融合四个步骤，具体步骤如下：

D1、信息分配

将全局的状态估计、误差方差阵、噪声信息分配给两个子滤波器和主滤波器，分配原则为：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，P_i,k-1为子滤波器k-1时刻的估计均方误差协方差矩阵，P_g,k-1为k-1时刻的全局最优估计均方误差协方差矩阵，Q_i,k-1为子滤波器噪声k-1时刻的系统噪声方差阵，Q_g,k-1为主滤波器噪声k-1时刻的系统噪声方差阵，

为子滤波器k-1时刻的状态估计，/>

为主滤波器k-1时刻的状态估计；

信息分配因子β_i满足信息守恒原则，即

主滤波器中无信息分配，执行状态估计和协方差阵的时间更新，即P_m,k＝0；

D2、时间更新

时间更新在两个子滤波器间独立进行，分别获得两个子滤波器的状态预测矢量和一步预测均方误差协方差矩阵：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，P_i,k|k-1为子滤波器的一步预测均方误差协方差矩阵，

为子滤波器的状态预测矢量，Φ_i,k|k-1为子滤波器k-1时刻到k时刻的一步转移矩阵，Γ_i,k-1为噪声驱动阵；/>

D3、量测更新

两个子滤波器接收量测信息并独立进行量测更新过程，分别获得两个子滤波器的状态估计值、滤波增益和估计均方误差协方差矩阵，并传至主滤波器：

式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，

为子滤波器k时刻的状态估计，K_i,k为子滤波器k时刻的滤波增益，P_i,k为估计均方误差协方差矩阵，Z_i,k为子滤波器k时刻的量测值，H_i,k子滤波器k时刻的量测矩阵，R_i,k子滤波器k时刻的量测噪声方差阵，I单位矩阵；

量测更新过程仅在子滤波器内进行，主滤波器内只进行时间更新过程；

其中

为主滤波器k-1时刻的状态估计，/>

为主滤波器k-1时刻的状态预测矢量；

D4、信息融合

把两个子滤波器的状态估计信息和主滤波器的状态估计进行融合，从而得到全局状态估计信息：

式中，P_g,k为k时刻的全局最优估计均方误差协方差矩阵，P_1,k为SINS/GPS子滤波器k时刻的估计均方误差协方差矩阵，P_2,k为SINS/Compass子滤波器k时刻的估计均方误差协方差矩阵，P_m,k为主滤波器k时刻的估计均方误差协方差矩阵，

为k时刻的全局最优状态估计；

D5、计算自适应信息分配因子

最优自适应因子满足以下条件：

最优自适应因子表示如下：

几何状态矢量与预测状态矢量的偏差为：

其中，几何状态矢量

为：/>

式中，∑_i,k为Z_i,k的等效权矩阵；

其中，

为预测状态矢量，P_i,k|k-1为预测状态矢量的估计协方差矩阵，/>

为预测状态矢量的理论协方差矩阵；

当估计量基于当前时刻的预测状态矢量，预测状态矢量的估计协方差矩阵如下：

最优自适应因子α_k的期望值小于1，表示为：

式中：

则最优自适应因子基于预测状态矢量的估计协方差矩阵表示为：

由上式表示的自适应因子类似于由模型的预测状态与量测的估计状态之间的差异构造的自适应因子；

自适应因子表示为预测状态矢量的形式为：

其中，c为常数，

为基于预测状态矢量/>

构造的统计量，其表达式为：

通过基于预测状态矢量的最优自适应因子的计算方法求得自适应联邦卡尔曼滤波器的自适应信息分配因子为：

其中，b为常数，取0.85～1.0；

为确保自适应信息分配因子满足信息守恒定律，对自适应信息分配因子进行归一化处理；自适应信息分配因子归一化为：

式中，β_i'_,k为归一化后的子滤波器在k时刻的自适应信息分配因子。

Images