CN103065037B - 非线性系统基于分散式容积信息滤波的目标跟踪方法 - Google Patents

Abstract

本发明属于目标跟踪领域，主要涉及一种非线性系统基于分散式容积信息滤波的目标跟踪方法。现有的容积卡尔曼的非线性系统目标跟踪方法是在过程噪声与测量噪声不相关及各测量噪声也互不相关的假设前提下进行的。这就大大限制了它的使用范围。本发明推导了噪声相关的扩展卡尔曼信息滤波，并在时间更新与测量更新这两个过程中嵌入容积卡尔曼信息滤波，也就解决了噪声相关的问题，使得本发明的方法实用性大大增强，另外本发明是基于分散式的，利用矩阵对角化原理，很大程度上降低了矩阵的维数，避免了高维带来的维数灾难问题。

Description

非线性系统基于分散式容积信息滤波的目标跟踪方法

技术领域

本发明属于目标跟踪领域，主要涉及一种非线性系统基于分散式容积信息滤波的目标跟踪方法。

背景技术

多传感器目标跟踪是一门多学科交叉技术。近年来，随着传感器技术、计算机技术、通信技术和信息处理技术的发展，特别是军事上的迫切需求，多传感器目标跟踪技术的研究内容日益深入和广泛。军事上主要应用于指挥、控制、通信和情报系统，同时在机器人、民航航管等领域也有重要应用价值。目前对目标跟踪有了很多比较好的算法，如卡尔曼滤波算法(KF)，无迹卡尔曼滤波算法(UKF)，求容积卡尔曼滤波算法(CKF)等，然而众所周知，当所有传感器测量值到达融合中心进行集中处理时这些算法具有很高的计算复杂度。所以，信息滤波器被提了出来并且得到了广泛的应用由于在计算方面有比卡尔曼滤波算法更优越的性能并且容易初始化。实际上，信息滤波算法本质上是用协方差阵的逆表示的卡尔曼滤波算法。

目前关于非线性滤波的目标跟踪算法最新进展是容积信息滤波算法(SCIF)，但由于此算法的前提是任何噪声之间是不相关的，所以大大限制了它的应用范围。在实际当中往往由于天气，跟踪同一个目标，同样的环境，多传感器的异步采样等原因，不仅过程噪声与观测噪声之间相关(相关噪声I)，各传感器观测噪声之间也可能相关(相关噪声II)。本发明给出了噪声I与噪声II同时存在下的基于分散式容积卡尔曼信息滤波的目标跟踪方法(DSCIF-CN)。

发明内容

为了解决噪声相关的情况，本发明提出了非线性系统基于分散式容积卡尔曼信息滤波的目标跟踪方。为了方便描述本发明的内容，首先本发明针对多传感器目标系统建立模型，包括两个方程，状态方程和观测方程，分别如下所示：

x_k＝f_k-1(x_k-1)+w_k,k-1   (1)

z_i,k＝h_i,k(x_k)+v_i,k   (2)

其中，k是时间指标及i(i＝1,2,...,N)表示第i个传感器；x_k∈R^n×1是系统状态向量，表示第i个观测向量；f_k-1:R^n×1→R^n×1，均为已知的非线性方程；过程噪声w_k,k-1和观测噪声v_i,k都为零均值的高斯白噪声，它们的方差分别为Q_k,k-1和R_i,k且满足 $E\left\{w_{k,k-1}{w}_{k,k-1}^T\right\}=Q_{k,k-1},E\left\{v_{i,k}{v}_{j,l}^T\right\}=R_{\mathrm{ij},k}{\mathrm{\δ}}_{\mathrm{kl}},E\left\{w_{k,k-1}{v}_{i,l}^T\right\}=D_{i,k}{\mathrm{\δ}}_{\mathrm{kl}}$ 其中R_ij,k为第i个传感器观测噪声与第j个传感器观测噪声的协方差矩阵，D_i,k为过程噪声与第i个传感器观测噪声的互协方差矩阵，δ_k,l为脉冲函数，即k＝l时，δ_k,l＝1，k≠l时，δ_k,l＝0。初始状态为x₀及协方差矩阵P_0|0满足：

$E\left\{x_0\right\}={\hat{x}}_{0|0},E\left\{\right[x_0-{\hat{x}}_{0|0}\left]{[x_0-{\hat{x}}_{0|0}]}^T\right\}=P_{0|0}---\left(\text{3}\right)$

运用集中式扩维，将N个观测方程融合成一个观测方程，也就是：

z_k＝h_k(x_k)+v_k   (4)

其中， $z_k={[z_{1,k}^T,z_{2,k}^T,...,z_{N,k}^T]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(5\right)$

$h_k\left(x_k\right)={[h_{1,k}^T\left(x_k\right),h_{2,k}^T\left(x_k\right),...,h_{N,k}^T\left(x_k\right)]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(6\right)$

$v_k={[v_{1,k}^T,v_{2,k}^T,...,v_{N,k}^T]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(7\right)$

我们也可以得到：

$D_k=[D_{1,k}^T,D_{2,k}^T,...,D_{N,k}^T]\∈R^{n\×{\mathrm{\Σ}}_{i=1}^N{m}_i}---\left(8\right)$

针对(1)(4)所描述的多传感器系统模型，给出如下迭代算法，具体包括两个模块：时间更新和测量更新。

1.时间更新

步骤1.1分别计算k-1时刻第i个容积点X_i,k-1|k-1,k-1时刻第i个传播容积点和k-1时刻一步状态预测

首先，假设k-1时刻的状态估计和它的协方差矩阵P_k-1|k-1已知.分解P_k-1|k-1有：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(10\right)$

其中S_k-1|k-1称为k-1时刻开方值。

其次，从(11)式计算传播容积点i＝1,2,…,M＝2n

$X_{i,k|k-1}^{*}=f_{k-1}\left(X_{i,k-1|k-1}\right)---\left(11\right)$

其中， $X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(12\right)$

并且这里[1]_i是点集合[1]的第i个列向量.例如如果那么它表示下面的集合：

$\left\{\begin{array}{c}\left(\begin{array}{c}1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\end{array}\right)\end{array}\right\}$

最后，计算k-1时刻状态的一步预测：

${\hat{x}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}---\left(13\right)$

步骤1.2根据下式计算k-1时刻一步开方S_k|k-1；

$S_{k|k-1}=\mathrm{Tria}\left(\begin{array}{c}\left[\begin{array}{cc}{\mathrm{\γ}}_{k|k-1}^{*}& S_{Q_{k,k-1}}\end{array}\right]\end{array}\right)---\left(14\right)$

这里Tria表示QR分解，将分解得到的上三角矩阵的转置赋给S_k|k-1，是Q_k,k-1的开方根，即：并且

${\mathrm{\γ}}_{k|k-1}^{*}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k-1}& ...& X_{M,k|k-1}^{*}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(15\right)$

$Y_{k|k-1}=P_{k|k-1}^{-1}={\left({\mathrm{\γ}}_{k|k-1}{\mathrm{\γ}}_{k|k-1}^T\right)}^{-1}$

步骤1.3使用下面的式子得到k-1时刻一步信息矩阵Y_k|k-1；

令 ${\mathrm{\γ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& ...& X_{M,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(16\right)$

然后利用即可得到一步信息矩阵Y_k|k-1。

步骤1.4使用式(17)式计算k-1时刻一步预测信息状态向量

$\left\{\begin{array}{c}{\hat{y}}_{k|k-1}=P_{k|k-1}^{-1}{\hat{x}}_{k|k-1}=\frac{1}{M}\left(Y_{k|k-1}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}\right)\\ {\mathrm{\υ}}_k=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{Z}_{i,k|k-1}\end{array}\right.---\left(17\right)$

2.测量更新

步骤2.1分别计算k-1时刻第i个一步容积点X_i,k|k-1,k-1时刻第i个一步传播容积点Z_i,k|k-1和k-1时刻观测一步预测

首先，计算k-1时刻一步容积点X_i,k|k-1，如下式所示：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}---\left(18\right)$

进而可利用下式计算k-1时刻一步传播容积点，

Z_i,k|k-1＝h_k(X_i,k|k-1)   (19)

然后，利用式(20)计算k-1时刻观测一步预测

${\hat{z}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{Z}_{i,k|k-1}---\left(20\right)$

步骤2.2计算k-1时刻互协方差矩阵

首先，根据下式计算k-1时刻开方新息协方差矩阵

$S_{\tilde{z}\tilde{z},k|k-1}=\mathrm{Tria}\left(\begin{array}{c}\left[\begin{array}{cc}{\mathrm{\ζ}}_{k|k-1}^{*}& S_{R_k}\end{array}\right]\end{array}\right)---\left(21\right)$

其中，表示R_k的开方，如及

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{Z}_{1,k|k-1}-{\hat{z}}_{k|k-1}& Z_{2,k|k-1}-{\hat{z}}_{k|k-1}& ...& Z_{M,k|k-1}-{\hat{z}}_{k|k-1}\end{array}\right]$

然后，计算k-1时刻互协方差矩阵

$P_{\tilde{x}\tilde{z},k|k-1}={\mathrm{\γ}}_{k|k-1}{\mathrm{\ζ}}_{k|k-1}^T---\left(22\right)$

步骤2.3下面计算k时刻相关信息矩阵I_k和信息状态增益i_k；

由于及我们可以得到是一个的对称矩阵，继而也是一个对称矩阵，且由实对称矩阵的性质知：存在正交矩阵和对角矩阵使得

所以 ${\tilde{R}}_k^{-1}={\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^T,$ 其中 ${\widetilde{\mathrm{\Σ}}}_k=\mathrm{diag}\{{\widetilde{\mathrm{\λ}}}_{1,k},{\widetilde{\mathrm{\λ}}}_{2,k},...,{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{i=1}^N{\text{m}}_i,k}\},$ 是的第l个特征值， $(l=\mathrm{1,2},...,{\mathrm{\Σ}}_{i=1}^N{m}_i).$

记 ${\widetilde{\mathrm{\Σ}}}_{i,k}=\mathrm{diag}\{{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+1,k},{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+2,k},...,{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{\text{m}}_j+m_i,k}\},$ i＝1,2,…,N

且 ${\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}=\mathrm{diag}\{1/{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+1,k},{1/\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+2,k},...,{1/\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{\text{m}}_j+m_i,k}\}$

我们有： ${\widetilde{\mathrm{\Σ}}}_k=\mathrm{diag}\{{\widetilde{\mathrm{\Σ}}}_{1,k},{\widetilde{\mathrm{\Σ}}}_{2,k},...,{\widetilde{\mathrm{\Σ}}}_{N,k}\},{\widetilde{\mathrm{\Σ}}}_k^{-1}=\mathrm{diag}\{{\widetilde{\mathrm{\Σ}}}_{1,k}^{-1},{\widetilde{\mathrm{\Σ}}}_{2,k}^{-1},...,{\widetilde{\mathrm{\Σ}}}_{N,k}^{-1}\}$

根据噪声相关的扩展卡尔曼信息滤波器算法：

$\left\{\begin{array}{c}{Y}_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ {\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\end{array}\right.---\left(23\right)$

得到信息矩阵I_k是由一些式子的线性组合组成：

$\begin{array}{c}{I}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^T{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}---\left(24\right)$

其中， ${\widetilde{\mathrm{\Ω}}}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k$

相似地，信息状态增量i_k的分散式为：

$\begin{array}{c}{i}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\\ =\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{T}_{i,k}\end{array}---\left(25\right)$

其中， $T_k={\tilde{U}}_k^T({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})$

步骤2.4利用下式计算k时刻信息矩阵Y_k|k和更新的信息状态向量

${\tilde{R}}_k=R_k-D_k^T{P}_{k|k-1}^{-1}{D}_k---\left(26\right)$

$\left\{\begin{array}{c}{\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{T}_{i,k}\\ Y_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}\right.---\left(27\right)$

步骤2.5用下式获得k时刻状态估计和相应协方差矩阵P_k|k；

$\left\{\begin{array}{c}{P}_{k|k}=Y_{k|k}^{-1}\\ {\hat{x}}_{k|k}=P_{k|k}^{-1}{\hat{y}}_{k|k}\end{array}\right.---\left(28\right)$

不断重复上面两个模块的内容，就可实现对目标状态的跟踪估计。

本发明的有益效果：

本发明提出的目标跟踪方法，在时间更新和测量更新中利用对称矩阵的对角化性质解决了噪声的相关性，并利用分散式的算法降低了矩阵的维数。从而使本方法能在噪声I与噪声II同时存在的条件下也能对目标进行很好的跟踪。附图说明

图1为本发明跟踪方法的流程图；

图2为本发明中的纯方位跟踪系统图；

图3A为本发明仿真在X方向(正东方向)的跟踪效果图；

图3B为本发明仿真在Y方向(正北方向)的跟踪效果图；

图3C为本发明仿真在X方向(正东方向)的跟踪误差图；

图3D为本发明仿真在Y方向(正北方向)的跟踪误差图。

具体实施方式

本发明的实施流程图如图1所示，具体实施方式如下：

为了解决噪声相关的情况，本发明提出了噪声相关条件下的分散式容积信息滤波(DSCIF-CN)设计方法。为了方便描述本发明的内容，首先本发明针对多传感器目标系统建立模型，包括两个方程，状态方程和观测方程，分别如下所示：

x_k＝f_k-1(x_k-1)+w_k,k-1   (1)

z_i,k＝h_i,k(x_k)+v_i,k   (2)

其中，k是时间指标及i(i＝1,2,...,N)表示第i个传感器；x_k∈R^n×1是系统状态向量，表示第i个观测向量；f_k-1:R^n×1→R^n×1，均为已知的非线性方程；过程噪声w_k,k-1和观测噪声v_i,k都为零均值的高斯白噪声，它们的方差分别为Q_k,k-1和R_i,k且满足 $E\left\{w_{k,k-1}{w}_{k,k-1}^T\right\}=Q_{k,k-1},E\left\{v_{i,k}{v}_{j,l}^T\right\}=R_{\mathrm{ij},k}{\mathrm{\δ}}_{\mathrm{kl}},E\left\{w_{k,k-1}{v}_{i,l}^T\right\}=D_{i,k}{\mathrm{\δ}}_{\mathrm{kl}}$ 其中R_ij,k为第i个传感器观测噪声与第j个传感器观测噪声的协方差矩阵，D_i,k为过程噪声与第i个传感器观测噪声的互协方差矩阵，δ_k,l为脉冲函数，即k＝l时，δ_k,l＝1，k≠l时，δ_k,l＝0。初始状态为x₀及协方差矩阵P_0|0满足：

$E\left\{x_0\right\}={\hat{x}}_{0|0},E\left\{\right[x_0-{\hat{x}}_{0|0}\left]{[x_0-{\hat{x}}_{0|0}]}^T\right\}=P_{0|0}---\left(\text{3}\right)$

运用集中式扩维，将N个观测方程融合成一个观测方程，也就是：

z_k＝h_k(x_k)+v_k   (4)

其中， $z_k={[z_{1,k}^T,z_{2,k}^T,...,z_{N,k}^T]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(5\right)$

$h_k\left(x_k\right)={[h_{1,k}^T\left(x_k\right),h_{2,k}^T\left(x_k\right),...,h_{N,k}^T\left(x_k\right)]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(6\right)$

$v_k={[v_{1,k}^T,v_{2,k}^T,...,v_{N,k}^T]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(7\right)$

我们也可以得到：

$D_k=[D_{1,k}^T,D_{2,k}^T,...,D_{N,k}^T]\∈R^{n\×{\mathrm{\Σ}}_{i=1}^N{m}_i}---\left(8\right)$

针对(1)(4)所描述的多传感器系统模型，本发明给出如下迭代算法，具体包括两个模块：时间更新和测量更新。

1.时间更新

步骤1.1分别计算k-1时刻第i个容积点X_i,k-1|k-1,k-1时刻第i个传播容积点和k-1时刻一步状态预测

首先，假设k-1时刻的状态估计和它的协方差矩阵P_k-1|k-1已知.分解P_k-1|k-1有：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(10\right)$

其中S_k-1|k-1称为k-1时刻开方值。

其次，从(11)式计算传播容积点：(i＝1,2,…,M＝2n)

$X_{i,k|k-1}^{*}=f_{k-1}\left(X_{i,k-1|k-1}\right)---\left(11\right)$

其中，

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(12\right)$

并且这里,[1]_i是点集合[1]的第i个列向量.例如如果那么它表示下面的集合：

$\left\{\begin{array}{c}\left(\begin{array}{c}1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\end{array}\right)\end{array}\right\}$

最后，计算k-1时刻状态的一步预测：

${\hat{x}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}---\left(13\right)$

步骤1.2根据下式计算k-1时刻一步开方S_k|k-1；

$S_{k|k-1}=\mathrm{Tria}\left(\begin{array}{c}\left[\begin{array}{cc}{\mathrm{\γ}}_{k|k-1}^{*}& S_{Q_{k,k-1}}\end{array}\right]\end{array}\right)---\left(14\right)$

这里Tria表示QR分解，将分解得到的上三角矩阵的转置赋给S_k|k-1，是Q_k,k-1的开方根，即：并且

${\mathrm{\γ}}_{k|k-1}^{*}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k-1}& ...& X_{M,k|k-1}^{*}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(15\right)$

$Y_{k|k-1}=P_{k|k-1}^{-1}={\left({\mathrm{\γ}}_{k|k-1}{\mathrm{\γ}}_{k|k-1}^T\right)}^{-1}$

步骤1.3使用下面的式子得到k-1时刻一步信息矩阵Y_k|k-1；

令 ${\mathrm{\γ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& ...& X_{M,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(16\right)$

然后利用即可得到一步信息矩阵Y_k|k-1。

步骤1.4使用式(17)式计算k-1时刻一步预测信息状态向量

$\left\{\begin{array}{c}{\hat{y}}_{k|k-1}=P_{k|k-1}^{-1}{\hat{x}}_{k|k-1}=\frac{1}{M}\left(Y_{k|k-1}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}\right)\\ {\mathrm{\υ}}_k=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{Z}_{i,k|k-1}\end{array}\right.---\left(17\right)$

2.测量更新

步骤2.1分别计算k-1时刻第i个一步容积点X_i,k|k-1,k-1时刻第i个一步传播容积点Z_i,k|k-1和k-1时刻观测一步预测

首先，计算k-1时刻一步容积点X_i,k|k-1，如下式所示：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}---\left(18\right)$

进而可利用下式计算k-1时刻一步传播容积点，

Z_i,k|k-1＝h_k(X_i,k|k-1)   (19)

然后，利用式(20)计算k-1时刻观测一步预测

${\hat{z}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{Z}_{i,k|k-1}---\left(20\right)$

步骤2.2计算k-1时刻互协方差矩阵

首先，根据下式计算k-1时刻开方新息协方差矩阵

$S_{\tilde{z}\tilde{z},k|k-1}=\mathrm{Tria}\left(\begin{array}{c}\left[\begin{array}{cc}{\mathrm{\ζ}}_{k|k-1}^{*}& S_{R_k}\end{array}\right]\end{array}\right)---\left(21\right)$

其中，表示R_k的开方，如及

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{Z}_{1,k|k-1}-{\hat{z}}_{k|k-1}& Z_{2,k|k-1}-{\hat{z}}_{k|k-1}& ...& Z_{M,k|k-1}-{\hat{z}}_{k|k-1}\end{array}\right]$

然后，计算k-1时刻互协方差矩阵

$P_{\tilde{x}\tilde{z},k|k-1}={\mathrm{\γ}}_{k|k-1}{\mathrm{\ζ}}_{k|k-1}^T---\left(22\right)$

步骤2.3下面计算k时刻相关信息矩阵I_k和信息状态增益i_k；

由于我们可以得到是一个的对称矩阵，继而也是一个对称矩阵，且由实对称矩阵的性质知：存在正交矩阵和对角矩阵使得

所以 ${\tilde{R}}_k^{-1}={\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^T,$ 其中 ${\widetilde{\mathrm{\Σ}}}_k=\mathrm{diag}\{{\widetilde{\mathrm{\λ}}}_{1,k},{\widetilde{\mathrm{\λ}}}_{2,k},...,{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{i=1}^N{\text{m}}_i,k}\},$ 是的第l个特征值， $(l=\mathrm{1,2},...,{\mathrm{\Σ}}_{i=1}^N{m}_i).$

记 ${\widetilde{\mathrm{\Σ}}}_{i,k}=\mathrm{diag}\{{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+1,k},{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+2,k},...,{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{\text{m}}_j+m_i,k}\},$ i＝1,2,…,N；

且 ${\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}=\mathrm{diag}\{1/{\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+1,k},{1/\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{m}_j+2,k},...,{1/\widetilde{\mathrm{\λ}}}_{{\mathrm{\Σ}}_{j=1}^{i-1}{\text{m}}_j+m_i,k}\}$

我们有： ${\widetilde{\mathrm{\Σ}}}_k=\mathrm{diag}\{{\widetilde{\mathrm{\Σ}}}_{1,k},{\widetilde{\mathrm{\Σ}}}_{2,k},...,{\widetilde{\mathrm{\Σ}}}_{N,k}\},{\widetilde{\mathrm{\Σ}}}_k^{-1}=\mathrm{diag}\{{\widetilde{\mathrm{\Σ}}}_{1,k}^{-1},{\widetilde{\mathrm{\Σ}}}_{2,k}^{-1},...,{\widetilde{\mathrm{\Σ}}}_{N,k}^{-1}\}$

根据噪声相关的扩展卡尔曼信息滤波器算法：

$\left\{\begin{array}{c}{Y}_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ {\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\end{array}\right.---\left(23\right)$

得到信息矩阵I_k是由一些式子的线性组合组成：

$\begin{array}{c}{I}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^T{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}---\left(24\right)$

其中， ${\widetilde{\mathrm{\Ω}}}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k$

相似地，信息状态增量i_k的分散式为：

$\begin{array}{c}{i}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\mathrm{\Σ}}}_k^{-1}{\tilde{U}}_k^{-1}({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})\\ =\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{T}_{i,k}\end{array}---\left(25\right)$

其中， $T_k={\tilde{U}}_k^T({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})$

步骤2.4利用下式计算k时刻信息矩阵Y_k|k和更新的信息状态向量

${\tilde{R}}_k=R_k-D_k^T{P}_{k|k-1}^{-1}{D}_k---\left(26\right)$

$\left\{\begin{array}{c}{\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{T}_{i,k}\\ Y_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\mathrm{\Σ}}}_{i,k}^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}\right.---\left(27\right)$

步骤2.5用下式获得k时刻状态估计和相应协方差矩阵P_k|k；

$\left\{\begin{array}{c}{P}_{k|k}=Y_{k|k}^{-1}\\ {\hat{x}}_{k|k}=P_{k|k}^{-1}{\hat{y}}_{k|k}\end{array}\right.---\left(28\right)$

不断重复上面两个模块的内容，就可实现对目标状态的跟踪估计。

方法试验：

在本试验中，我们采用本发明方法对纯方位系统进行目标跟踪估计。为了更好的阐释本试验，首先对图2中的各个参数加以解释：s_i1与s_i2是两个传感器，{α_i,k,β_i,k}为这两个传感器的观测角度，p_j(j＝1,2)为传感器的坐标位置，d为两个传感器的距离。本试验中，目标有4个状态，即x_k＝[x_1,k x_2,k y_1,k y_2,k]^Τ，x_1,k与y_1,k是目标在正东方向和正北方向的位置坐标，x_2,k与y_2,k是目标在正东方向和正北方向的速度大小。假如目标做匀速直线运动，状态方程为：x_k＝F_k,k-1x_k-1+w_k,k-1，观测方程为： $z_{i,k}=\left[\begin{array}{c}\arccos \left[\frac{x_{1,k}}{\sqrt{x_{1,k}^2+y_{1,k}^2}}\right]\\ \arccos \left[\frac{x_{1,k}-d}{\sqrt{{(x_{1,k}-d)}^2+y_{1,k}^2}}\right]\end{array}\right]+v_{i,k},(i=1,...,N),$ 这里h_1,k(x_k)＝…＝h_N,k(x_k)＝h_k(x_k)本实验参数设置如下： $F_{k-1}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],$ T为目标跟踪系统的融合周期，设置T＝1s，过程噪声协方差矩阵 $Q_{k,k-1}=\left[\begin{array}{cccc}{T}^3/3& T^2/2& 0& 0\\ T^2/2& T& 0& 0\\ 0& 0& T^3/3& T^2/2\\ 0& 0& T^2/2& T\end{array}\right]\×0.5,$ 初始状态与协方差矩阵分别为： ${\hat{x}}_{0|0}={\left[\begin{array}{cccc}0& 10& 0& 10\end{array}\right]}^T,P_{0|0}=\left[\begin{array}{cccc}100& 0& 0& 0\\ 0& 10& 0& 0\\ 0& 0& 100& 0\\ 0& 0& 0& 10\end{array}\right]$ 为了显示噪声相关性，我们用v_k＝cw_k,k-1产生观测噪声，其中c为噪声相关系数，这里设置 $c_1=\left[\begin{array}{cccc}0.0001& 0.01& 0.01& 0.001\\ 0.0001& 0.01& 0.01& 0.01\end{array}\right],c_2=\left[\begin{array}{cccc}0.001& 0.01& 0.11& 0.01\\ 0.0001& 0.11& 0.01& 0.1\end{array}\right]$

图3A为对X方向的状态估计跟踪效果图，图中X-Displacement为目标在X方向的状态位置，本发明的跟踪方法DSCIF-CN的曲线基本与目标在X方向的状态重合，跟踪效果很好。

图3B为对Y方向的状态估计跟踪效果图，图中Y-Displacement为目标在Y方向的状态位置，本文的跟踪方法DSCIF-CN的曲线基本与目标在Y方向的状态重合，跟踪效果很好。

图3C为本发明跟踪方法DSCIF-CN在X方向对目标状态估计跟踪的误差，其误差前5个时刻较大，但之后误差稳定的趋于1以下，在实际允许范围内。

图3D为本发明跟踪方法DSCIF-CN在Y方向对目标状态估计跟踪的误差，其误差也在前5个时刻震荡较大，但在之后误差趋于稳定1左右，在实际允许范围内。

Claims (1)

1.非线性系统基于分散式容积信息滤波的目标跟踪方法，其特征在于：

针对多传感器目标系统建立模型，包括两个方程，状态方程和观测方程，分别如下所示：

x_k＝f_k-1(x_k-1)+w_k,k-1        (1)

z_i,k＝h_i,k(x_k)+v_i,k        (2)

其中，k是时间指标，i表示第i个传感器，i＝1,2,...N；x_k∈R^n×1是系统状态向量，表示第i个观测向量；f_k-1:R^n×1→R^n×1，均为已知的非线性方程；过程噪声w_k,k-1和观测噪声v_i,k都为零均值的高斯白噪声，它们的方差分别为Q_k,k-1和R_i,k且满足

$E\left\{w_{k,k-1}{w}_{k,k-1}^T\right\}=Q_{k,k-1},E\left\{v_{i,k}{v}_{j,l}^T\right\}=R_{ij,k}{\mathrm{\δ}}_{kl},E\left\{w_{k,k-1}{v}_{i,l}^T\right\}=D_{i,k}{\mathrm{\δ}}_{kl}$

其中R_ij,k为第i个传感器观测噪声与第j个传感器观测噪声的协方差矩阵，D_i,k为过程噪声与第i个传感器观测噪声的互协方差矩阵，δ_k,l为脉冲函数，即k＝l时，δ_k,l＝1，k≠l时，δ_k,l＝0；初始状态为x₀及协方差矩阵P_0|0满足：

$E\left\{x_0\right\}={\hat{x}}_{0|0},E\{\[x_0-{\hat{x}}_{0|0}\]{\[x_0-{\hat{x}}_{0|0}\]}^T\}=P_{0|0}---\left(3\right)$

运用集中式扩维，将N个观测方程融合成一个观测方程，也就是：

z_k＝h_k(x_k)+v_k          (4)

其中， $z_k={\[z_{1,k}^T,z_{2,k}^T,...,z_{N,k}^T\]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(5\right)$

$h_k\left(x_k\right)={\[h_{1,k}^T\left(x_k\right),h_{2,k}^T\left(x_k\right),...,h_{N,k}^T\left(x_k\right)\]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(6\right)$

$v_k={\[v_{1,k}^T,v_{2,k}^T,...,v_{N,k}^T\]}^T\∈R^{{\mathrm{\Σ}}_{i=1}^N{m}_i\×1}---\left(7\right)$

由此可以得到：

$D_k=\[D_{1,k}^T,D_{2,k}^T,...,D_{N,k}^T\]\∈R^{n\×{\mathrm{\Σ}}_{i=1}^N{m}_i}---\left(8\right)$

针对式(1)(4)所描述的多传感器系统模型，给出如下迭代算法，具体包括两个模块：时间更新和测量更新；

1.时间更新

步骤1.1分别计算k-1时刻第i个容积点X_i,k-1|k-1,k-1时刻第i个传播容积点和k-1时刻一步状态预测

首先，假设k-1时刻的状态估计和它的协方差矩阵P_k-1|k-1已知.分解P_k-1|k-1有：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(10\right)$

其中S_k-1|k-1称为k-1时刻开方值；

其次，从(11)式计算传播容积点i＝1,2,…,M＝2n

$X_{i,k|k-1}^{*}=f_{k-1}\left(X_{i,k-1|k-1}\right)---\left(11\right)$

其中，

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(12\right)$

并且这里,[1]_i是点集合[1]的第i个列向量；

最后，计算k-1时刻状态的一步预测：

${\hat{x}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\Σ}}{X}_{i,k|k-1}^{*}---\left(13\right)$

步骤1.2根据下式计算k-1时刻一步开方S_k|k-1；

$S_{k|k-1}=Tria\left(\left[\begin{array}{cc}{\mathrm{\γ}}_{k|k-1}^{*}& S_{Q_{k,k-1}}\end{array}\right]\right)---\left(14\right)$

这里Tria表示QR分解，将分解得到的上三角矩阵的转置赋给S_k|k-1，是Q_k,k-1的开方根，即：并且

${\mathrm{\γ}}_{k|k-1}^{*}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k}& \mathrm{...}& X_{M,k|k-1}^{*}-{\hat{x}}_{k|k}\end{array}\right]---\left(15\right)$

$Y_{k|k-1}=P_{k|k-1}^{-1}{\left({\mathrm{\γ}}_{k|k-1}{\mathrm{\γ}}_{k|k-1}^T\right)}^{-1}$

步骤1.3使用下面的式子得到k-1时刻一步信息矩阵Y_k|k-1；

令 ${\mathrm{\γ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{M,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(16\right)$

然后利用即可得到一步信息矩阵Y_k|k-1；

步骤1.4使用式(17)式计算k-1时刻一步预测信息状态向量

$\{\begin{array}{c}{\hat{y}}_{k|k-1}=P_{k|k-1}^{-1}{\hat{x}}_{k|k-1}=\frac{1}{M}\left(Y_{k|k-1}\underset{i=1}{\overset{M}{\Σ}}{X}_{i,k|k-1}^{*}\right)\\ {\mathrm{\υ}}_k=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{M}\underset{i=1}{\overset{M}{\Σ}}{Z}_{i,k|k-1}\end{array}---\left(17\right)$

2.测量更新

步骤2.1分别计算k-1时刻第i个一步容积点X_i,k|k-1,k-1时刻第i个一步传播容积点Z_i,k|k-1和k-1时刻观测一步预测

首先，计算k-1时刻一步容积点X_i,k|k-1，如下式所示：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ζ}}_i+{\hat{x}}_{k|k-1}---\left(18\right)$

进而可利用下式计算k-1时刻一步传播容积点，

Z_i,k|k-1＝h_k(X_i,k|k-1)          (19)

然后，利用式(20)计算k-1时刻观测一步预测

${\hat{z}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\Σ}}{Z}_{i,k|k-1}---\left(20\right)$

步骤2.2计算k-1时刻互协方差矩阵

首先，根据下式计算k-1时刻开方新息协方差矩阵

$S_{\tilde{z}\tilde{z},k|k-1}=Tria\left(\left[\begin{array}{cc}{\mathrm{\ζ}}_{k|k-1}^{*}& S_{{\stackrel{\‾}{R}}_k}\end{array}\right]\right)---\left(21\right)$

其中，表示R_k的开方；

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{Z}_{1,k|k-1}-{\hat{z}}_{k|k}& Z_{2,k|k-1}-{\hat{z}}_{k|k}& \mathrm{...}& Z_{M,k|k-1}-{\hat{z}}_{k|k}\end{array}\right]$

然后，计算k-1时刻互协方差矩阵

$P_{\tilde{x}\tilde{z},k|k-1}={\mathrm{\γ}}_{k|k-1}{\mathrm{\ζ}}_{k|k-1}^T---\left(22\right)$

步骤2.3下面计算k时刻相关信息矩阵I_k和信息状态增益i_k；

由于 $D_k\∈R^{n\×{\mathrm{\Σ}}_{i=1}^N{m}_i}$ 及 $P_{k|k-1}^{-1}\∈R^{n\×n},$ 可以得到是一个的对称矩阵，继而也是一个对称矩阵，且由实对称矩阵的性质知：存在正交矩阵和对角矩阵使得

所以 ${\tilde{R}}_k^{-1}={\tilde{U}}_k{\widetilde{\Σ}}_k^{-1}{\tilde{U}}_k^T,$ 其中 ${\widetilde{\Σ}}_k=diag\left\{{\tilde{L}}_{1,k},{\widetilde{\mathrm{\λ}}}_{2,k},...,{\widetilde{\mathrm{\λ}}}_{{\Σ}_{i=1}^N{m}_i,k}\right\},$ 是的第l个特征值， $(l=1,2,...,{\mathrm{\Σ}}_{i=1}^N{m}_i);$

记 ${\widetilde{\Σ}}_{i,k}=diag\{{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+1,k},{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+2,k},...,{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+m_i,k}\},i=1,2,...,N$

且 ${\widetilde{\Σ}}_{i,k}^{-1}=diag\{1/{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+1,k},1/{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+2,k},...,1/{\widetilde{\mathrm{\λ}}}_{{\Σ}_{j=1}^{i-1}{m}_j+m_i,k}\}$

有： ${\widetilde{\Σ}}_k=diag\{{\widetilde{\Σ}}_{1,k},{\widetilde{\Σ}}_{2,k},...,{\widetilde{\Σ}}_{N,k}\},{\widetilde{\Σ}}_k^{-1}=diag\{{\widetilde{\Σ}}_{1,k}^{-1},{\widetilde{\Σ}}_{2,k}^{-1},...,{\widetilde{\Σ}}_{N,k}^{-1}\}$

根据噪声相关的扩展卡尔曼信息滤波器算法：

$\{\begin{array}{c}{Y}_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ {\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}\left({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\end{array}---\left(23\right)$

得到信息矩阵I_k是由一些式子的线性组合组成：

$\begin{array}{c}{I}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\Σ}}_k^{-1}{\tilde{U}}_k^T{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =\underset{i=1}{\overset{N}{\Σ}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\Σ}}_k^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}---\left(24\right)$

其中， ${\widetilde{\mathrm{\Ω}}}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k$

信息状态增量i_k的分散式为：

$\begin{array}{c}{i}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}\left({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\widetilde{\Σ}}_k^{-1}{\tilde{U}}_k^{-1}\left({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\\ =\underset{i=1}{\overset{N}{\Σ}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\Σ}}_k^{-1}{T}_{i,k}\end{array}---\left(25\right)$

其中， $T_k={\tilde{U}}_k^T({\mathrm{\υ}}_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})$

步骤2.4利用下式计算k时刻信息矩阵Y_k|k和更新的信息状态向量

${\tilde{R}}_k=R_k-D_k^T{P}_{k|k-1}^{-1}{D}_k---\left(26\right)$

$\left\{\begin{array}{c}{\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+\underset{i=1}{\overset{N}{\Σ}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\Σ}}_{i,k}^{-1}{T}_{i,k}\\ Y_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+\underset{i=1}{\overset{N}{\Σ}}{\widetilde{\mathrm{\Ω}}}_{i,k}{\widetilde{\Σ}}_{i,k}^{-1}{\widetilde{\mathrm{\Ω}}}_{i,k}^T\end{array}\right.---\left(27\right)$

步骤2.5用下式获得k时刻状态估计和相应协方差矩阵P_k|k；

$\{\begin{array}{c}{P}_{k|k}=Y_{k|k}^{-1}\\ {\hat{x}}_{k|k}=P_{k|k}^{-1}{\hat{y}}_{k|k}\end{array}---\left(28\right)$

不断重复上面两个模块的内容，就可实现对目标状态的跟踪估计。