CN110926466A - 一种面向无人船组合导航信息融合的多尺度数据分块算法 - Google Patents

Abstract

本发明公开了一种面向无人船组合导航信息融合的多尺度数据分块算法，简称MDS，包括以下步骤：建立组合导航系统数学模型；建立组合导航多尺度系统模型；设计最优融合算法。本发明针对无人船组合导航系统多传感器异步采样的问题，提出一种多尺度数据分块的信息融合方法(MDS)，相比于传统联邦滤波(FKF)的分布式滤波算法，有效的解决了传感器信息异步融合的问题。本发明在原有多尺度滤波方法的基础上，将多尺度滤波方法与数据分块的方法结合，相比于小波变换的方法，该方法计算量更小，更简便，并提高了无人船组合导航系统的信息融合效果。

Description

一种面向无人船组合导航信息融合的多尺度数据分块算法

技术领域

本发明属于多传感器信息融合领域，尤其涉及一种多尺度数据分块算法在无人船SINS/GPS/Compass组合导航系统的多传感器信息融合中的应用研究。

背景技术

组合导航系统指各种导航传感器，通过控制器进行控制组合而成的导航系统。组合导航系统广泛应用于汽车、船舶等应用领域，用于提供精确的信息参数。由于捷联惯导系统可以提供较多的导航参数，所以组合导航系统大多以捷联惯导为基础进行设计。目前，多传感器信息融合方法广泛应用于组合导航的数据处理融合，多传感器信息融合常用的滤波结构包括集中式滤波结构和分布式滤波结构。集中式滤波结构简单、精度更高，但是可靠性低、计算负担大。分布式滤波具有容错性高、可靠性强等优点，所以在组合导航多传感器信息融合领域得到了广泛应用。常用的分布式滤波方法主要为联邦卡尔曼滤波，在此基础上，将自适应、模糊等方法与之结合，具备了更高的应用价值。

目前，组合导航系统多传感器信息融合方法得到了广泛研究，但在实际应用过程中，由于传感器性能限制、信号变化率相差较大、采样周期不同等问题限制，每个导航传感器的采样率不尽相同，而且其中包括传感器固有延迟和通信延迟，所以导航传感器并不是同步将传感器数据发送回融合中心，造成多传感器信息融合的异步问题，降低组合导航系统导航精度，增大了组合导航系统的误差。

发明内容

为解决现有技术存在的上述问题，本发明要设计一种能够解决传感器信息融合异步问题的面向无人船组合导航信息融合的多尺度数据分块算法。

为了实现上述目的，本发明的技术方案如下：一种面向无人船组合导航信息融合的多尺度数据分块算法，简称MDS，包括以下步骤：

A、建立组合导航系统数学模型

针对无人船组合导航多传感器信息融合问题，建立无人船组合导航系统的数学模型。所述无人船组合导航系统包括捷联惯导系统传感器即SINS传感器、全球定位系统传感器即GPS传感器、三维电子罗盘传感器即Compass传感器和嵌入式处理器。所述嵌入式处理器内采用多尺度数据分块算法进行传感器信息融合，输出无人船的位置、速度和姿态信息。采用SINS传感器的误差方程作为组合导航系统的状态方程，导航坐标系选用东北天坐标系即NEU，通过对SINS传感器的性能及误差源分析，组合导航系统的状态方程和量测方程分别描述为：

其中，

为组合导航系统的状态估计值；F(t)为组合导航系统的状态转移矩阵；X(t)为组合导航系统的状态变量；W(t)为组合导航系统的白噪声矩阵；i＝1、2、3，Z₁(t)为SINS/GPS子滤波器速度的量测值，Z₂(t)为SINS/GPS子滤波器位置的量测值，Z₃(t)为SINS/Compass子滤波器姿态的量测值，H_i(t)为组合导航系统的量测矩阵，V_i(t)为组合导航系统的量测噪声，t为时间。

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

式中，

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

分别为无人船导航坐标系下加速度计东、北、天方向的常值随机误差。

无人船组合导航系统共有3组观测值，分别为GPS传感器测得的无人船速度与SINS传感器测得的无人船速度之间的差值、GPS传感器测得的无人船位置与SINS传感器测得的无人船位置之间的差值、Compass传感器测得的无人船姿态与SINS传感器测得的无人船姿态之间的差值。

取GPS传感器测得的无人船速度与SINS测得的无人船速度之间的差值作为SINS/GPS子滤波器速度的量测值，定义速度量测方程为：

式中：

V₁(t)＝[M_E,M_N,M_U]^T

其中，H₁(t)为SINS/GPS子滤波器速度的量测矩阵；V₁(t)为SINS/GPS子滤波器速度的量测噪声，当做白噪声处理；V_IE、V_IN、V_IU分别为SINS传感器测得的无人船的东、北、天方向的速度；V_GE、V_GN、V_GU分别为GPS传感器测得的无人船的东、北、天方向的速度；M_E、M_N、M_U分别为GPS传感器测得的无人船的东、北、天方向的速度误差。

取GPS传感器测得的无人船位置与SINS传感器测得的无人船位置之间的差值作为SINS/GPS子滤波器位置的量测值，定义位置量测方程为：

式中：

V₂(t)＝[N_N,N_E,N_U]^T

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

R_N＝R_e(1+f sin²L)

其中，H₂(t)为SINS/GPS子滤波器位置的量测矩阵；V₂(t)为SINS/GPS子滤波器位置的量测噪声，当做白噪声处理。N_N、N_E、N_U分别为GPS传感器测得的无人船的纬度、经度、高度的误差；λ_I、L_I、h_I分别为SINS传感器测得的无人船的东、北、天方向的位置；λ_G、L_G、h_G分别为GPS传感器测得的无人船的东、北、天方向的位置；R_e为地球赤道半径，取6378137m；f为地球扁率，取1/298.257；O_3×6为3行6列的零矩阵；O_3×9为3行9列的零矩阵；L为纬度。

将Compass传感器测得的无人船姿态与SINS传感器测得的无人船姿态之间的差值作为SINS/Compass子滤波器的量测值，则观测方程为：

式中：

H₃(t)＝[I_3×3,0_3×15]

V₃(t)＝[δα_c,δβ_c,δγ_c]^T

其中，H₃(t)为SINS/Compass子滤波器姿态的量测矩阵；V₃(t)为SINS/Compass子滤波器姿态的量测噪声，是均值为零的观测白噪声；

分别为SINS测得的无人船的东、北、天方向的姿态，

分别为Compass测得的无人船的东、北、天的姿态；δα_E、δα_N、δα_U分别为Compass测得的无人船的东、北、天方向的姿态误差；I_3×3为3行3列的单位矩阵，O_3×15为3行15列单位矩阵。

B、建立组合导航多尺度系统模型

B1、描述组合导航多尺度系统

对组合导航系统的状态方程和量测方程进行离散化，根据多尺度理论，各传感器以不同采样率对数据进行观测，在最高采样率下即最细尺度上建立状态方程，将组合导航多尺度系统模型描述为：

X_N(m+1)＝F_N(m)X_N(m)+W_N(m) m≥0

Z_d(m)＝H_d(m)X(m)+V_d(m)

其中，N＝3，传感器d以不同的采样率S_d进行观测，且传感器之间的的采样是异步的，其中d＝1、2、3，分别表示SINS传感器、GPS传感器、Compass传感器，SINS传感器为最高采样率的传感器，即最细尺度传感器；Compass传感器为最低采样率的传感器，即最粗尺度的传感器。GPS传感器为中采样率的传感器，即中尺度的传感器。其中，m为组合导航系统数学模型离散化后的时间。

各传感器采样率间的关系满足：

S₃＝2S₂

S₂＝3S₁

X_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的状态变量；F_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的一步转移矩阵；W_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的噪声，其为零均值高斯白噪声，并满足特点如下：

E{W_N(m)}＝0

其中，g是采样时刻，δ_mg为克罗尼克函数、表示同一传感器不同采样时刻的的相关性。

Z_d(m)表示组合导航多尺度系统在m时刻的量测值；Z₁(m)、Z₂(m)、Z₃(m)分别为离散化后m时刻的SINS/GPS子滤波器速度的量测值、SINS/GPS子滤波器位置的量测值、SINS/Compass子滤波器姿态的量测值、H_d(m)表示组合导航多尺度系统m时刻的量测矩阵；V_d(m)表示组合导航多尺度系统m时刻的观测噪声，其中V_d(m)为高斯白噪声序列，其协方差为R_d(m)，并满足特点如下：

E{V_d(m)}＝0

E{V_d(m)W_N(m)}＝0

状态变量的初始值X_N(0)为随机向量，且有：

E{X_N(0)}＝X₀

E{[X_N(0)-X₀][X_N(0)-X₀]^T}＝P₀

B2、对组合导航系统进行数据分块

针对组合导航系统各传感器多速率异步采样，建立组合导航系统多尺度系统模型，采用数据分块的算法进行数据分块，将一定长度的采样时间内的状态数据列为一个数据块，对一个数据块的状态数据进行最优融合，定义数据块的长度为：

即状态向量和测量向量被分割成长度为M_d的数据块。

进行数据分块后的状态方程表示为如下形式：

X_N(m+1)＝X_N(m)F_N(m)+W_N(m)

组合导航多尺度系统噪声的协方差阵Q_i(m)表示为：

E{W_N(m)}＝0

B3、建立多尺度转换关系

传感器d的状态信息在传感器N的尺度上的有如下的转换关系，所述的状态信息包括位置、速度和姿态：

其中：

其中，

为

维矩阵，由

个n维矩阵单位阵组成，其中n为3。

由上述推导得，传感器d与传感器N之间在尺度上有如下关系：

组合导航多尺度系统的量测方程为：

Z_d(m)＝H_d(m)X_d(m)+V_d(m)

其中，组合导航多尺度系统的量测值为：

组合导航多尺度系统的量测噪声为：

组合导航多尺度系统的量测矩阵为：

E{V_d(m)}＝0

R_d(m)＝diag{R_d(mM+1),R_d(mM+2),...,R_d(mM)}

其中，j＝1、2、3；

C、设计最优融合算法

在多传感器异步信息融合过程中，如果传感器d在m时刻有测量值，那么m与n_d之间呈整数倍关系。通过用mod(m,n_d)等于零来判断传感器d在m时刻有无测量值，有如下两种情况：

其中，n₁＝1,n₂＝3,n₃＝6。

C1、滤波更新1：若传感器d在m时刻无测量值，则最细尺度上的融合估计值

和相应的误差协方差

为：

由卡尔曼滤波得到m时刻的估计值

和相应的误差协方差P_N(m|m)。

状态一步预测矩阵为：

状态估计矩阵为：

滤波增益矩阵为：

K_N(m)＝P_N(m+1|m)H_N(m)[H_N(m)P_N(m+1|m)H_N(m)+R_N(m)]^-1

一步预测均方误差阵为：

P_N(m|m)＝[I-K_N(m)H_N(m)]P_N(m+1|m)

估计均方误差阵为：

P_N(m|m-1)＝F_N(m-1)P_N(m-1|m-1)F_N(m-1)+Q_N(m-1)

C2、滤波更新2：若传感器d在m时刻有测量值，则m时刻基于所有传感器的最优估计为：

其中，

β₀(d,m)和β_p(d_p,m)为信息分配因子，分别表示如下：

由卡尔曼滤波得到q_p时刻的估计值

和相应的误差协方差

如下：

状态一步预测矩阵为：

状态估计矩阵为：

滤波增益矩阵为：

一步预测均方误差阵为：

估计均方误差阵为：

结束。

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

1、本发明针对无人船组合导航系统多传感器异步采样的问题，提出一种多尺度数据分块的信息融合方法(MDS)，相比于传统联邦滤波(FKF)的分布式滤波算法，有效的解决了传感器信息异步融合的问题。

2、本发明在原有多尺度滤波方法的基础上，将多尺度滤波方法与数据分块的方法结合，相比于小波变换的方法，该方法计算量更小，更简便，并提高了无人船组合导航系统的信息融合效果。

附图说明

图1是多传感器异步采样的示意图。

图2是多尺度数据分块的组合导航信息融合流程图。

图3是MDS算法与FKF算法的纬度误差仿真波形图。

图4是MDS算法与FKF算法的经度误差仿真波形图。

图5是MDS算法与FKF算法的高度误差仿真波形图。

图6是MDS算法与FKF算法的东向速度误差仿真波形图。

图7是MDS算法与FKF算法的北向速度误差仿真波形图。

图8是MDS算法与FKF算法的天向速度误差仿真波形图。

图9是MDS算法与FKF算法的天向姿态误差仿真波形图。

图10是MDS算法与FKF算法的天向姿态误差仿真波形图。

图11是MDS算法与FKF算法的天向姿态误差仿真波形图。

具体实施方式

下面结合附图对本发明进行进一步地描述。为了说明本发明的有效性和可行性，MATLAB仿真系统采用无人船的动态仿真的方式，航行轨迹按照无人船的特性进行设定，对本发明进行验证。

为模拟多传感器异步采样的效果，分别设置SINS、GPS、Compass三个传感器的采样周期的比例为1:2:3，对MEMS惯性传感器的性能指标设定如表1所示。

表1传感器的误差项设定

无人船的初始参数设定为：初始航向45°、初始速度5m/s、初始位置为东经124.0°、北纬39.0°。实验时，取原始尺度N＝3，则式中M_N＝3，设最高采样率的传感器的采样周期为200s，连续200次蒙特卡洛仿真的结果如图3-11所示。

对相同的SINS、GPS、Compass三个传感器的仿真数据，本发明与联邦卡尔曼算法的滤波实验进行比较。

图1为多传感器异步采样的示意图，由图可知异步采样的原理，各传感器采样周期不一致，不能同时进行数据融合，所以将一定长度的采样时间进行数据分块，对一个数据块内的数据进行最优融合。

图2为多尺度数据分块的信息融合流程图，针对异步采样问题，首先，建立组合导航多尺度系统模型，进行数据分块；然后，滤波更新1和滤波更新2两个滤波模块根据m时刻有无测量值，采用不同的滤波模块进行滤波更新；最后，采用最优融合的方法，求取最优融合的无偏估计值及其相应的估计误差协方差矩阵。

图3-5分别为本发明和联邦滤波算法(FKF)的纬度、经度、高度误差仿真效果图。图6-8分别为本发明和联邦滤波算法(FKF)的东向、北向、天向速度误差的仿真波形图。图9-11分别为分别为本发明和联邦滤波算法(FKF)的东向、北向、天向姿态误差的仿真波形图。

综合分析，通过两种算法的仿真结果对比，本发明的9项观测数据结果，仅有1项误差结果相比于联邦滤波算法偏大，但是误差结果已控制在无人船航行标准的要求范围内，而联邦滤波算法在起始阶段误差漂移较大，且误差上下波动较大，所以本发明具有更高的可靠性和稳定性。

综上所述，本发明在异步采样情况下有效的降低了传感器误差，提高了航行精度，对无人船组合导航系统的实际航行应用有极大的帮助和提高。

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

Claims (1)

1.一种面向无人船组合导航信息融合的多尺度数据分块算法，其特征在于：包括以下步骤：

A、建立组合导航系统数学模型

针对无人船组合导航多传感器信息融合问题，建立无人船组合导航系统的数学模型；所述无人船组合导航系统包括捷联惯导系统传感器即SINS传感器、全球定位系统传感器即GPS传感器、三维电子罗盘传感器即Compass传感器和嵌入式处理器；所述嵌入式处理器内采用多尺度数据分块算法进行传感器信息融合，输出无人船的位置、速度和姿态信息；采用SINS传感器的误差方程作为组合导航系统的状态方程，导航坐标系选用东北天坐标系即NEU，通过对SINS传感器的性能及误差源分析，组合导航系统的状态方程和量测方程分别描述为：

其中，

为组合导航系统的状态估计值；F(t)为组合导航系统的状态转移矩阵；X(t)为组合导航系统的状态变量；W(t)为组合导航系统的白噪声矩阵；i＝1、2、3，Z₁(t)为SINS/GPS子滤波器速度的量测值，Z₂(t)为SINS/GPS子滤波器位置的量测值，Z₃(t)为SINS/Compass子滤波器姿态的量测值，H_i(t)为组合导航系统的量测矩阵，V_i(t)为组合导航系统的量测噪声，t为时间；

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

式中，

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

分别为无人船导航坐标系下加速度计东、北、天方向的常值随机误差；

无人船组合导航系统共有3组观测值，分别为GPS传感器测得的无人船速度与SINS传感器测得的无人船速度之间的差值、GPS传感器测得的无人船位置与SINS传感器测得的无人船位置之间的差值、Compass传感器测得的无人船姿态与SINS传感器测得的无人船姿态之间的差值；

取GPS传感器测得的无人船速度与SINS测得的无人船速度之间的差值作为SINS/GPS子滤波器速度的量测值，定义速度量测方程为：

式中：

V₁(t)＝[M_E,M_N,M_U]^T

其中，H₁(t)为SINS/GPS子滤波器速度的量测矩阵；V₁(t)为SINS/GPS子滤波器速度的量测噪声，当做白噪声处理；V_IE、V_IN、V_IU分别为SINS传感器测得的无人船的东、北、天方向的速度；V_GE、V_GN、V_GU分别为GPS传感器测得的无人船的东、北、天方向的速度；M_E、M_N、M_U分别为GPS传感器测得的无人船的东、北、天方向的速度误差；

取GPS传感器测得的无人船位置与SINS传感器测得的无人船位置之间的差值作为SINS/GPS子滤波器位置的量测值，定义位置量测方程为：

式中：

V₂(t)＝[N_N,N_E,N_U]^T

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

R_N＝R_e(1+f sin² L)

其中，H₂(t)为SINS/GPS子滤波器位置的量测矩阵；V₂(t)为SINS/GPS子滤波器位置的量测噪声，当做白噪声处理；N_N、N_E、N_U分别为GPS传感器测得的无人船的纬度、经度、高度的误差；λ_I、L_I、h_I分别为SINS传感器测得的无人船的东、北、天方向的位置；λ_G、L_G、h_G分别为GPS传感器测得的无人船的东、北、天方向的位置；R_e为地球赤道半径，取6378137m；f为地球扁率，取1/298.257；O_3×6为3行6列的零矩阵；O_3×9为3行9列的零矩阵；L为纬度；

将Compass传感器测得的无人船姿态与SINS传感器测得的无人船姿态之间的差值作为SINS/Compass子滤波器的量测值，则观测方程为：

式中：

H₃(t)＝[I_3×3,0_3×15]

V₃(t)＝[δα_c,δβ_c,δγ_c]^T

其中，H₃(t)为SINS/Compass子滤波器姿态的量测矩阵；V₃(t)为SINS/Compass子滤波器姿态的量测噪声，是均值为零的观测白噪声；

分别为SINS测得的无人船的东、北、天方向的姿态，

分别为Compass测得的无人船的东、北、天的姿态；δα_E、δα_N、δα_U分别为Compass测得的无人船的东、北、天方向的姿态误差；I_3×3为3行3列的单位矩阵，O_3×15为3行15列单位矩阵；

B、建立组合导航多尺度系统模型

B1、描述组合导航多尺度系统

对组合导航系统的状态方程和量测方程进行离散化，根据多尺度理论，各传感器以不同采样率对数据进行观测，在最高采样率下即最细尺度上建立状态方程，将组合导航多尺度系统模型描述为：

X_N(m+1)＝F_N(m)X_N(m)+W_N(m)m≥0

Z_d(m)＝H_d(m)X(m)+V_d(m)

其中，N＝3，传感器d以不同的采样率S_d进行观测，且传感器之间的的采样是异步的，其中d＝1、2、3，分别表示SINS传感器、GPS传感器、Compass传感器，SINS传感器为最高采样率的传感器，即最细尺度传感器；Compass传感器为最低采样率的传感器，即最粗尺度的传感器；GPS传感器为中采样率的传感器，即中尺度的传感器；其中，m为组合导航系统数学模型离散化后的时间；

各传感器采样率间的关系满足：

S₃＝2S₂

S₂＝3S₁

X_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的状态变量；F_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的一步转移矩阵；W_N(m)表示m时刻在最高采样率下的组合导航多尺度系统的噪声，其为零均值高斯白噪声，并满足特点如下：

E{W_N(m)}＝0

其中，g是采样时刻，δ_mg为克罗尼克函数、表示同一传感器不同采样时刻的的相关性；

Z_d(m)表示组合导航多尺度系统在m时刻的量测值；Z₁(m)、Z₂(m)、Z₃(m)分别为离散化后m时刻的SINS/GPS子滤波器速度的量测值、SINS/GPS子滤波器位置的量测值、SINS/Compass子滤波器姿态的量测值、H_d(m)表示组合导航多尺度系统m时刻的量测矩阵；V_d(m)表示组合导航多尺度系统m时刻的观测噪声，其中V_d(m)为高斯白噪声序列，其协方差为R_d(m)，并满足特点如下：

E{V_d(m)}＝0

E{V_d(m)W_N(m)}＝0

状态变量的初始值X_N(0)为随机向量，且有：

E{X_N(0)}＝X₀

E{[X_N(0)-X₀][X_N(0)-X₀]^T}＝P₀

B2、对组合导航系统进行数据分块

针对组合导航系统各传感器多速率异步采样，建立组合导航系统多尺度系统模型，采用数据分块的算法进行数据分块，将一定长度的采样时间内的状态数据列为一个数据块，对一个数据块的状态数据进行最优融合，定义数据块的长度为：

即状态向量和测量向量被分割成长度为M_d的数据块；

进行数据分块后的状态方程表示为如下形式：

X_N(m+1)＝X_N(m)F_N(m)+W_N(m)

组合导航多尺度系统噪声的协方差阵Q_i(m)表示为：

E{W_N(m)}＝0

B3、建立多尺度转换关系

传感器d的状态信息在传感器N的尺度上的有如下的转换关系，所述的状态信息包括位置、速度和姿态：

其中：

其中，

为

维矩阵，由

个n维矩阵单位阵组成，其中n为3；

由上述推导得，传感器d与传感器N之间在尺度上有如下关系：

组合导航多尺度系统的量测方程为：

Z_d(m)＝H_d(m)X_d(m)+V_d(m)

其中，组合导航多尺度系统的量测值为：

组合导航多尺度系统的量测噪声为：

组合导航多尺度系统的量测矩阵为：

E{V_d(m)}＝0

R_d(m)＝diag{R_d(mM+1),R_d(mM+2),...,R_d(mM)}

其中，j＝1、2、3；

C、设计最优融合算法

在多传感器异步信息融合过程中，如果传感器d在m时刻有测量值，那么m与n_d之间呈整数倍关系；通过用mod(m,n_d)等于零来判断传感器d在m时刻有无测量值，有如下两种情况：

其中，n₁＝1,n₂＝3,n₃＝6；

C1、滤波更新1：若传感器d在m时刻无测量值，则最细尺度上的融合估计值

和相应的误差协方差

为：

由卡尔曼滤波得到m时刻的估计值

和相应的误差协方差P_N(m|m)；

状态一步预测矩阵为：

状态估计矩阵为：

滤波增益矩阵为：

一步预测均方误差阵为：

P_N(m|m)＝[I-K_N(m)H_N(m)]P_N(m+1|m)

估计均方误差阵为：

P_N(m|m-1)＝F_N(m-1)P_N(m-1|m-1)F_N(m-1)+Q_N(m-1)

C2、滤波更新2：若传感器d在m时刻有测量值，则m时刻基于所有传感器的最优估计为：

其中，

β₀(d,m)和β_p(d_p,m)为信息分配因子，分别表示如下：

p＝1,2

由卡尔曼滤波得到q_p时刻的估计值

和相应的误差协方差

如下：

状态一步预测矩阵为：

状态估计矩阵为：

滤波增益矩阵为：

一步预测均方误差阵为：

估计均方误差阵为：

结束。

Images