CN103714045A - 面向异步多速率不均匀采样观测数据的信息融合估计方法 - Google Patents

Abstract

本发明提供了一种面向异步多速率不均匀采样观测数据的信息融合估计方法，有效解决了面向异步多速率不均匀采样观测数据的信息融合估计问题。步骤(1)：向计算机输入数据；步骤(2)：输入系统矩阵和系统误差方差阵，输入(k-1,k]时刻获得的来自所有传感器的观测数据和观测矩阵，以及观测噪声方差阵，并输入传感器i的观测的采样时刻t_i，记录(k-1,k]时刻观测数据的个数；步骤(3)：计算状态预测值与状态预测误差协方差矩阵；步骤(4)，计算状态的融合估计值和相应的估计误差协方差矩阵；步骤(5)得到所求状态的融合估计值和融合估计误差协方差矩阵；步骤(6)：重复步骤得任意时刻的状态融合估计值与融合估计误差协方差矩阵，即完成信息融合估计。

Description

面向异步多速率不均匀采样观测数据的信息融合估计方法

技术领域

本发明属于信息处理方面的多传感器信息融合技术领域，涉及一种面向异步多速率不均匀采样观测数据的信息融合估计方法。

背景技术

数据融合是一个从多层次、多方面处理来自多个来源的数据和信息的过程，通过相关处理以实现目标的检测、关联、相关和状态估计，并对情况和威胁进行完整且及时的评估。融合估计指的是如何充分利用包含在多组数据里的信息以得到对一个量的最优估计。它被广泛用于众多军事和民用领域，如目标跟踪、组合导航、故障检测和控制等。

对于状态融合估计，最早期的工作是基于相同采样率的同步测量值进行的。因为在实际应用中，异步多传感器融合非常普遍，人们逐渐开始关注异步数据的融合。基于连续系统，Alouani和他的合作者Bar-Shalom等，提出了一些有效的异步多传感器融合的算法。对于离散时间系统而言，相关的研究方法包括基于多尺度系统理论的方法，批处理的方法和基于多速率的滤波器设计的算法等。

随着小波变换的出现，人们开始分析多尺度现象和多尺度信号。在多个尺度上描述信号“是一个自然的事情”因为“研究的现象可能具有多尺度特征或物理上具有多尺度特性”，并且“观测数据常常是在不同尺度上获得的”。随着小波变换和金字塔表示法的发展，多速率的数字滤波算法和多尺度信号处理的研究急剧增加。在20世纪80年代末和90年代初，为了系统地和有效地描述多尺度统计信号处理算法，提出了一个多尺度系统理论的统计框架。多尺度系统理论提供了一个尺度递归的方式来处理在不同的尺度上的信号，这意味着在多尺度系统中“尺度发挥一个类似时间的作用”。基于这个想法，多个不同传感器不同尺度获得的数据的最优估计可以有效地融合。相关的结果包括：多分辨率分布式滤波，多分辨率、递归卡尔曼滤波器，和一些多尺度建模和数据融合算法。

上面列出的多尺度建模和融合算法的基本思想是通过对最细尺度上的状态进行小波分解，建立多尺度模型，将不同传感器与不同采样率的测量对应到二叉树上（一般传感器之间的抽样比是2的整数次幂关系），然后利用小波重构从粗尺度到细尺度将数据融合，并生成最细尺度上的状态估计。但是这样做产生很多问题。首先，数据融合沿着尺度进行，类似传统滤波器沿着时间处理信号，这样做，通常需要分组和分批处理一定范围里的数据，因此计算量很大；第二，通过使用小波分解生成多尺度模型，系统噪声很容易交叠并且在每个较粗尺度上噪声变得相关；第三，当从不同传感器获得数据，传感器之间的采样率通常局限于2的整数幂关系，否则，需要用到M带小波变换，这使得多尺度多速率的数据融合问题更为复杂。

发明内容

本发明提供了一种面向异步多速率不均匀采样观测数据的信息融合估计方法，解决了面向异步多速率不均匀采样观测数据的信息融合估计问题，并将数据进行更为有效充分的利用。

本发明通过下述技术方案实现：

一种面向异步多速率不均匀采样观测数据的信息融合估计方法，包括以下步骤：

步骤(1)：向计算机输入传感器个数N，输入每一个传感器的采样速率S_i,i=1,2,…,N，同时输入传感器之间的采样比n_i=S₁\S_i,1≤i≤N，输入初始状态均值x₀，初始状态估计误差方差阵P₀，其中，x₀∈Rⁿ为n维实向量，P₀∈R^n×n是n维矩阵，且P₀>0为正定矩阵；n_i是用于描述传感器i和传感器1之间采样率关系的量，取值范围为正整数；

步骤(2)：对时刻k=1,2,…，输入系统矩阵A(k)和系统误差方差阵Q(k)，输入(k-1,k]时刻获得的来自所有传感器的观测数据y_i(k_i)和观测矩阵C_i(k_i)，以及观测噪声方差阵R_i(k_i)，并输入传感器i的观测y_i(k_i)的采样时刻t_i，记录(k-1,k]时刻观测数据的个数M_k，其中：

y_i(k_i)：第i个传感器的观测，其维数为m_i，取值范围为：m_i≤n；

A(k)：系统矩阵，用于描述状态间转移的量，其取值范围为：特征值在单位圆内的满秩矩阵，设目标状态的维数为n，则A(k)∈R^n×n；

C_i(k_i)：观测矩阵，用于描述观测数据的维数和观测数据含义的量，其维数为m_i，即 $C_i\left(k_i\right)\∈R^{m_i\×n};$

Q(k)：k时刻系统误差方差阵，用于描述系统建模误差的量，其维数为n×n，取值范围为非负定矩阵；

R_i(k_i)：观测误差方差，用于描述观测误差偏差的量，其维数为m_i×m_i，取值范围为非负定矩阵；

M_k：(k-1,k]时刻观测到的测量数据个数，M_k≤N；

步骤(3)，在时刻k=1,2,…，利用下式计算状态预测值与状态预测误差协方差矩阵：

$\hat{x}\left(k\right|k-1)=A\left(k\right)\hat{x}(k-1|k-1)$

P(k|k-1)=A(k)P(k-1|k-1)A^T(k)+Q(k)

其中，

P(0|0)=P₀；并记P₀(k|k)=P(k|k-1)；

步骤(4)，在时刻k=1,2,…，利用步骤(3)计算出的

和P₀(k|k)，以及步骤(2)输入的观测数据y_i(k_i),i=1,2,…,M_k，对i=1,2,…,M_k利用下式依次计算状态的融合估计值和相应的估计误差协方差矩阵：

${\hat{x}}_i\left(k\right|k)={\hat{x}}_{i-1}\left(k\right|k)+K_i\left(k\right)[y_i\left(k_i\right)-{\stackrel{\‾}{C}}_i\left(k\right){\hat{x}}_{i-1}\left(k\right|k)]$

$P_i\left(k\right|k)=P_{i-1}\left(k\right|k)-K_i\left(k\right){\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k)$

$K_i\left(k\right)=P_{i-1}(k-k){\stackrel{\‾}{C}}_i^T\left(k\right){[{\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k){\stackrel{\‾}{C}}_i^T\left(k\right)+R_i\left(k\right)]}^{-1}$

其中，

$I+\underset{m=1}{\overset{j-1}{\mathrm{\Σ}}}\underset{l=m}{\overset{j-1}{\mathrm{\Π}}}{A}^{-1}(n_i(k_i-1)+l)+\underset{m=0}{\overset{n_i-j-1}{\mathrm{\Σ}}}\underset{l=j+m}{\overset{j}{\mathrm{\Π}}}A(n_i(k_i-1)+l),j=k-n_i(k_i-1);$

步骤(5)，在时刻k=1,2,…，记 $\hat{x}(k-k)={\hat{x}}_{M_k}\left(k\right|k),$ $P\left(k\right|k)=P_{M_k}\left(k\right|k),$ 输出

和P(k|k)，即得到时刻k所求状态的融合估计值和融合估计误差协方差矩阵；

步骤(6)，将k+1赋值给k，重复步骤(2)-(5)，即得任意时刻k=1,2,…的状态融合估计值与融合估计误差协方差矩阵，即完成信息融合估计。

本发明具有以下优点：

(1)利用本发明可以融合具有不同采样速率、非同步观测的测量数据，并实时在线给出状态的融合估计结果。和已有方法相比，本发明的方法对观测数据的利用最为及时、有效，所得到的估计结果更优。

(2)能够提高系统的可靠性。以导航为背景，如果只有单一传感器参与导航，则万一传感器发生故障，导航系统将彻底崩溃。而多传感器同时发生故障的可能性很小，因此多传感器集成提供信息无疑将提高系统的可靠性。由于不同导航系统的采样率不同，采样异步也经常发生，因此，本发明的方法对实际系统数据融合具有更好的适用性。

(3)抗噪、抗干扰能力强，能够提高系统的定位精度。即使在所有传感器都无故障的情况下，采用多传感器信息融合技术融合利用多个传感器的信息将保留各传感器的优点，使得融合估计结果要优于基于任一单个传感器的估计结果。

(4)提高系统的鲁棒性。在有若干传感器受到干扰时，总会有一部分传感器可以提供信息，进而使系统能够不受干扰连续运行。

(5)本发明所提出的方法计算量适当，因为在最细尺度上建立动态系统，技术上使用最细尺度上的“移动平均”描述粗尺度信号。在获得融合估计值的过程中，只利用了Kalman滤波和线性更新公式，不存在状态的扩维和复杂矩阵求逆。

(6)方法得到的状态估计是方差最小意义下最优的，因为状态增益阵是利用正交定理导出的。

(7)本发明可直接用于真实数据的融合，并且方法实施简单，易于推广。

附图说明

图1为本发明所述方法的计算机流程框图。

图2为多传感器异步不均匀采样示意图，图中画出的三个传感器的采样率之比为12：4：3。

图3为计算机仿真曲线之原始信号和相应的测量；其中子图(a)，子图(b)和子图(c)分别表示传感器1、传感器2和传感器3的观测；在各子图中，实线所示为原始信号，点线所示为本发明模型产生的观测，虚线为文献[1]（L.P.Yan,B.S.Liu,and D.H.Zhou,The modeling and estimation ofasynchronous multirate multisensor dynamic systems.Aerospace Scienceand Technology,vol.10,no.1,pp.63-71,2006）所示平均观测模型产生的观测，点划线为文献[2]（L.P.Yan,D.H.Zhou,M.Y.Fu,andY.Q.Xia,State estimation for asynchronous multirate multisensor dynamicsystems with missing measurements.IET Signal Processing,vol.4,no.6,pp.728-739,2010）所示前向模型产生的观测；

图4为计算机仿真曲线之信号和估计，子图(a)和(b)分别画出了对位置和速度的估计，其中，实线表示原始信号，点线表示本发明得到的估计，虚线表示文献[1]方法得到的估计，点划线是对传感器1进行Kalman滤波的结果。

图5为计算机仿真曲线之统计估计误差，子图(a)和(b)分别画出了对位置和速度的统计估计误差，其中，实线表示本发明得到的误差，点线表示文献[1]方法得到的估计误差，虚线是对传感器1进行Kalman滤波的估计误差。

图6为计算机仿真曲线之统计估计绝对误差，子图(a)和(b)分别画出了对位置和速度的估计误差绝对值的统计结果，其中，实线表示本发明得到的误差，点线表示文献[1]方法得到的估计误差，虚线是对传感器1进行Kalman滤波的估计误差。

图7为计算机仿真曲线之与前向模型相比的信号和估计，子图(a)和(b)分别画出了对位置和速度的估计，其中，实线表示原始信号，点线表示本发明得到的估计，虚线表示文献[2]方法得到的估计。

图8为计算机仿真曲线之与前向模型相比的统计估计误差，子图(a)和(b)分别画出了对位置和速度的统计估计误差，其中，实线表示本发明得到的误差，点线表示文献[2]方法得到的估计误差，虚线是对传感器1进行Kalman滤波的估计误差。

图9为计算机仿真曲线之与前向模型相比的统计绝对估计误差，子图(a)和(b)分别画出了对位置和速度的估计误差绝对值的统计结果，其中，实线表示本发明得到的误差，点线表示文献[2]方法得到的估计误差，虚线是对传感器1进行Kalman滤波的估计误差。

图10为计算机仿真曲线之估计误差协方差的迹，其中，实线表示本发明得到的估计结果，点线表示文献[1]方法得到的估计结果，虚线表示对传感器1进行Kalman滤波的结果，点划线是文献[2]方法得到的估计结果。

具体实施方式

下面举例对本发明作进一步介绍。

有N个传感器对同一目标进行观测的异步、多速率传感器、离散时间线性动态系统可以描述为

x(k+1)=A(k)x(k)+w(k)

y_i(k_i)=C_i(k_i)x_i(k)+v_i(k_i),i=1,2,…N

其中，x(k)∈Rⁿ是kT时间在1尺度上的系统状态，T=1是传感器1的采样周期。A(k)∈R^n×n是系统转移矩阵，w(k)是系统噪声，建模为高斯分布。

是传感器i在时刻t_i的第k_i次测量。传感器i的采样周期是传感器1的n_i倍，这里n_i是一个已知的正整数。

是测量矩阵。x_i(k_i)是x(k),k∈[n_i(k_i-1)+1,n_ik_i]从N尺度到i尺度的粗投影。当i=1时，我们认为k₁=k,x₁(k₁)=x(k)。v_i(k_i)是满足高斯分布的测量白噪声。不同传感器之间的采样可以是非同步、不均匀的，采样率之间满足下列关系：

S₁=n_iS_i,1≤i≤N

初始状态x(0)独立于w(k)和v_i(k_i),i=1,2,…,N，假定服从高斯分布。

面向异步多速率不均匀采样观测数据的信息融合估计方法的具体实现如下：

步骤(1)，向计算机输入传感器个数N；输入每一个传感器的采样速率S_i,i=1,2,…,N；同时输入传感器之间的采样比n_i=S₁\S_i,1≤i≤N；输入初始状态均值x₀，初始状态估计误差方差阵P₀，其中，x₀∈Rⁿ，P₀∈R^n×n且P₀>0；n_i是用于描述传感器i和传感器1之间采样率关系的一个量，取值范围为正整数；

步骤(2)，对时刻k=1,2,…，输入系统矩阵A(k)和系统误差方差阵Q(k)；输入(k-1,k]时刻获得的来自所有传感器的观测数据y_i(k_i)和观测矩阵C_i(k_i)，以及观测噪声方差阵R_i(k_i)；并输入传感器i的观测y_i(k_i)的采样时刻t_i；记录(k-1,k]时刻观测数据的个数M_k；相关参数的设定值和需要满足的要求如下：

y_i(k_i)：第i个传感器的观测，其维数为m_i，取值范围为：m_i≤n；

A(k)：系统矩阵，用于描述状态间转移的一个量。其取值范围为：特征值在单位圆内的满秩矩阵，如目标状态的维数为n，则A(k)∈R^n×n；

C_i(k_i)：观测矩阵，用于描述观测数据的维数和观测数据含义的一个量，其维数为m_i，即 $C_i\left(k_i\right)\∈R^{m_i\×n};$

Q(k)：k时刻系统误差方差，用于描述系统建模误差的一个量。其维数为n×n，取值范围为非负定矩阵；

R_i(k_i)：观测误差方差，用于描述观测误差偏差的一个量。其维数为m_i×m_i，取值范围为非负定矩阵；

M_k：(k-1,k]时刻观测到的测量数据个数，M_k≤N。

步骤(3)，在时刻k=1,2,…，利用下式计算状态预测值与状态预测误差协方差矩阵：

$\hat{x}\left(k\right|k-1)=A\left(k\right)\hat{x}(k-1|k-1)$

P(k|k-1)=A(k)P(k-1|k-1)A^T(k)+Q(k)

其中，

P(0|0)=P₀；并记

P₀(k|k)=P(k|k-1)。

步骤(4)，在时刻k=1,2,…，利用步骤(3)计算出的

和P₀(k|k)，以及步骤(1)输入的观测数据y_i(k_i),i=1,2,…,M_k与相关参数，对i=1,2,…,M_k，利用下式依次计算状态的融合估计值和相应的估计误差协方差矩阵：

${\hat{x}}_i\left(k\right|k)={\hat{x}}_{i-1}\left(k\right|k)+K_i\left(k\right)[y_i\left(k_i\right)-{\stackrel{\‾}{C}}_i\left(k\right){\hat{x}}_{i-1}\left(k\right|k)]$

$P_i\left(k\right|k)=P_{i-1}\left(k\right|k)-K_i\left(k\right){\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k)$

$K_i\left(k\right)=P_{i-1}(k-k){\stackrel{\‾}{C}}_i^T\left(k\right){[{\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k){\stackrel{\‾}{C}}_i^T\left(k\right)+R_i\left(k\right)]}^{-1}$

其中，

$I+\underset{m=1}{\overset{j-1}{\mathrm{\Σ}}}\underset{l=m}{\overset{j-1}{\mathrm{\Π}}}{A}^{-1}(n_i(k_i-1)+l)+\underset{m=0}{\overset{n_i-j-1}{\mathrm{\Σ}}}\underset{l=j+m}{\overset{j}{\mathrm{\Π}}}A(n_i(k_i-1)+l),j=k-n_i(k_i-1);$

步骤(5)，在时刻k=1,2,…，记 $\hat{x}(k-k)={\hat{x}}_{M_k}\left(k\right|k),$ $P\left(k\right|k)=P_{M_k}\left(k\right|k),$ 输出

和P(k|k)，即得到时刻k所求状态的融合估计值和融合估计误差协方差矩阵。

步骤(6)，将k+1赋值给k，重复步骤(2)-(5)，即得任意时刻k=1,2,…的状态融合估计值与融合估计误差协方差矩阵。

下面将通过仿真实验测试本发明方法的有效性。

某三个传感器的雷达跟踪系统可用下式描述：

x(k+1)=A(k)x(k)+w(k)

y_i(k_i)=C_i(k_i)x_i(k)+v_i(k_i),i=1,2,…N

其中，x(k)∈Rⁿ是kT时间在1尺度上的系统状态，T=1是传感器1的采样周期。A(k)∈R^n×n是系统转移矩阵，w(k)是系统噪声，建模为高斯分布。

$A\left(k\right)=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]$

$Q\left(k\right):=Q=\left[\begin{array}{cc}\frac{T^4}{4}& \frac{T^3}{2}\\ \frac{T^3}{2}& T^2\end{array}\right].\×q$

这里T=1s是采样周期， $q=\left[\begin{array}{cc}10& 0.8\\ 0.8& 0.1\end{array}\right]$ 是扰动参数。点乘“.×”的结果是两个相同维数矩阵对应元素的乘积的矩阵，即，

因此，交换律适合两个矩阵的点乘。

传感器1和2观测位置，传感器3观测速度，因此，测量矩阵是

C₁(k)=[1 0]

C₂(k)=[1 0]

C₃(k)=[0 1]

测量噪声协方差是已知的：

R₁(k₁):=R₁=Q(1,1)×r₁

R₂(k₂):=R₂=Q(1,1)×r₂

R₃(k₃):=R₃=Q(2,2)×r₃

其中r₁=r₂=10000,r₃=100，并且Q(i,j),i,j=1,2表示式(16)中Q的第(i,j)个元素，初始条件是

$x_0=\left[\begin{array}{c}10\\ 0.5\end{array}\right],P_0=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right]$

仿真是在所有的测量来自三个传感器的情况下完成的。蒙特卡罗仿真结果表示在图3-10和表1、表2中。

表1:不同算法性能比较

表2：均方差误差

为方便起见，我们列出在这一部分用到的术语和缩写。

(1)均方误差(MSE)，即，估计误差协方差的迹。%即误差的平方从仿真次数M=1到M=100的100次的统计平均。

(2)统计估计误差(SEE)，即从仿真次数M=1到M=100的100次统计平均估计误差。

$\tilde{x}\left(k\right|k)=\frac{1}{M}\underset{j=1}{\overset{M}{\mathrm{\Σ}}}{\tilde{x}}^j\left(k\right|k)$

这里

是第j次仿真的估计误差。若

则

表示x服从均值为μ，方差为Σ的高斯分布。

(3)统计绝对估计误差(SAEE)，即，M次仿真过程中估计误差绝对值的统计平均值。

${\tilde{x}}^a\left(k\right|k)=\frac{1}{M}\underset{j=1}{\overset{M}{\mathrm{\Σ}}}\left|{\tilde{x}}^j\left(k\right|k)\right|$

是

沿着时间的平均，我们记为“MSAEE”，是统计绝对估计误差均值的缩写。

当系统模型确定，“MSE”是用来测量真实情况下估计误差的一个理论指标，它可以通过离线方式获得。没有反映真实的测量情况就不能说是一个非常现实的评价指标。“SEE”和“SAEE”是与实际估计误差相关的评价估计结果的指标，统计平均减小了单一运行时估计误差的随机性。

在表1和表2中，“PA”表示本发明当前模型所得的结果。“FM”表示使用分布式融合算法得到的前向模型结果（文献[2]）。“KF”表示基于传感器1所得数据卡尔曼滤波下的结果。

表1和表2给出各个模型测量结果，其中数值越小表示所对应的估计越优。由表1可知，通过融合，本发明模型的位置和速度误差均比利用一个传感器卡尔曼滤波和基于前向模型所示的方法获得的误差要小。由表2可知，本发明模型的均方根误差均值比基于卡尔曼滤波模型和前向模型的小，这说明发明的方法是非常有效且有价值的。

观察图3-10，从图3中可以看出由不同模型产生的测量结果没有明显的不同。从图4可以看出，利用本发明的方法得到的对位置、速度的估计误差比基于前向模型的方法得到的位置、速度估计误差要小，因此，本发明的方法优于前向模型所示方法，比前向模型更为有效。

从图5可以看出利用本发明的方法与平均方法的位置、速度统计估计误差差不多，均比卡尔曼滤波方法的位置、速度统计估计误差要小，因此，利用本发明的方法在统计估计误差方面和平均方法差不多，比卡尔曼滤波方法要好。

从图6可知，利用本发明的方法与平均方法的位置、速度统计绝对估计误差差不多，均比卡尔曼滤波方法的位置、速度统计绝对估计误差要小，说明利用本发明的方法在统计绝对估计误差方面和平均方法差不多，比卡尔曼滤波方法要好。

从图7(a)可以看出，利用本发明的方法得到的对位置的估计比基于前向模型的方法得到的对位置估计更接近实际值，从图7(b)可以看出，利用本发明的方法得到的对速度的估计比基于文献前向模型的方法得到的对速度估计更接近实际值，说明本发明的方法优于前向模型所示方法，比前向模型所示方法更为有效。

观察图8，利用本发明的方法得到的对位置、速度统计估计误差比基于前向模型的方法得到的对位置、速度统计估计误差要小，说明本发明方法比前向模型所示方法更为有效。

从图9可以看出利用本发明的方法得到的对位置、速度统计绝对估计误差比基于前向模型的方法得到的对位置、速度统计绝对估计误差要小，说明本发明的方法方法比前向模型所示方法更为有效。

从图10可以看出利用本发明的方法得到的估计误差协方差的迹，与平均方法得到的结果差不多，小于基于前向模型所示方法和卡尔曼滤波方法所得到的估计误差协方差的迹。

总之，从本部分的实验可以看出，与经典的Kalman滤波方法和文献[2]方法相比，本发明用于状态估计具有更高的估计精度；本发明的精读与文献[1]所示精度相当，但是比文献[1]具有更高计算效率，因为本发明不存在状态或观测的扩维，而文献[1]需要对观测和状态进行扩维。因此，发明的方法是非常有效且有价值的。

Claims (1)

1.一种面向异步多速率不均匀采样观测数据的信息融合估计方法，其特征在于，包括以下步骤：

步骤(1)：向计算机输入传感器个数N，输入每一个传感器的采样速率S_i,i=1,2,…,N，同时输入传感器之间的采样比n_i=S₁\S_i,1≤i≤N，输入初始状态均值x₀，初始状态估计误差方差阵P₀，其中，x₀∈Rⁿ,Rⁿ表示n维实向量，P₀∈R^n×n是n维矩阵，且P₀>0为正定矩阵；n_i是用于描述传感器i和传感器1之间采样率关系的量，取值范围为正整数；

步骤(2)：对时刻k=1,2,…，输入系统矩阵A(k)和系统误差方差阵Q(k)，输入(k-1,k]时刻获得的来自所有传感器的观测数据y_i(k_i)和观测矩阵C_i(k_i)，以及观测噪声方差阵R_i(k_i)，并输入传感器i的观测y_i(k_i)的采样时刻t_i，记录(k-1,k]时刻观测数据的个数M_k，其中：

y_i(k_i)：第i个传感器的观测，其维数为m_i，取值范围为：m_i≤n；

A(k)：系统矩阵，用于描述状态间转移的量，其取值范围为：特征值在单位圆内的满秩矩阵，设目标状态的维数为n，则A(k)∈R^n×n；

C_i(k_i)：观测矩阵，用于描述观测数据的维数和观测数据含义的量，其维数为m_i，即 $C_i\left(k_i\right)\∈R^{m_i\×n};$

Q(k)：k时刻系统误差方差阵，用于描述系统建模误差的量，其维数为n×n，取值范围为非负定矩阵；

R_i(k_i)：观测误差方差，用于描述观测误差偏差的量，其维数为m_i×m_i，取值范围为非负定矩阵；

M_k：(k-1,k]时刻观测到的测量数据个数，M_k≤N；

步骤(3)：在时刻k=1,2,…，利用下式计算状态预测值与状态预测误差协方差矩阵：

$\hat{x}\left(k\right|k-1)=A\left(k\right)\hat{x}(k-1|k-1)$

P(k|k-1)=A(k)P(k-1|k-1)A^T(k)+Q(k)

其中，

P(0|0)=P₀；并记

P₀(k|k)=P(k|k-1)；

步骤(4)：在时刻k=1,2,…，利用步骤(3)计算出的和P₀(k|k)，以及步骤(2)输入的观测数据y_i(k_i),i=1,2,…,M_k，对i=1,2,…,M_k利用下式依次计算状态的融合估计值和相应的估计误差协方差矩阵：

${\hat{x}}_i\left(k\right|k)={\hat{x}}_{i-1}\left(k\right|k)+K_i\left(k\right)[y_i\left(k_i\right)-{\stackrel{\‾}{C}}_i\left(k\right){\hat{x}}_{i-1}\left(k\right|k)]$

$P_i\left(k\right|k)=P_{i-1}\left(k\right|k)-K_i\left(k\right){\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k)$

$K_i\left(k\right)=P_{i-1}(k-k){\stackrel{\‾}{C}}_i^T\left(k\right){[{\stackrel{\‾}{C}}_i\left(k\right)P_{i-1}\left(k\right|k){\stackrel{\‾}{C}}_i^T\left(k\right)+R_i\left(k\right)]}^{-1}$

其中，

$I+\underset{m=1}{\overset{j-1}{\mathrm{\Σ}}}\underset{l=m}{\overset{j-1}{\mathrm{\Π}}}{A}^{-1}(n_i(k_i-1)+l)+\underset{m=0}{\overset{n_i-j-1}{\mathrm{\Σ}}}\underset{l=j+m}{\overset{j}{\mathrm{\Π}}}A(n_i(k_i-1)+l),j=k-n_i(k_i-1);$

步骤(5)：在时刻k=1,2,…，记 $\hat{x}(k-k)={\hat{x}}_{M_k}\left(k\right|k),$ $P\left(k\right|k)=P_{M_k}\left(k\right|k),$ 输出

和P(k|k)，即得到时刻k所求状态的融合估计值和融合估计误差协方差矩阵；

步骤(6)：将k+1赋值给k，重复步骤(2)-(5)，即得任意时刻k=1,2,…的状态融合估计值与融合估计误差协方差矩阵，即完成信息融合估计。

Images