CN107193009A - 一种模糊自适应多交互模型的多uuv协同系统水下目标跟踪算法 - Google Patents

Abstract

本发明提出一种模糊自适应多交互模型的多UUV协同系统水下目标跟踪算法，首先根据多UUV协同系统的纯方位目标跟踪原理，建立目标跟踪系统的离散型非线性的状态与观测方程。其次根据水下目标运动的特点，结合五种目标运动模型，依据其动力学状态转移矩阵进行分析，提出五种模型之间的耦合不等式关系，优化选择适应水下目标跟踪的运动模型集合。再次采用中间型的高斯分布函数作为隶属度函数，将模型概率作为评价每个模型获取滤波新息和对应的协方差的评价指标，设计模型转移概率的模糊推理。最后设计与实现FAIMM的算法步骤。本发明在多UUV协同系统的纯方位目标跟踪过程中，选择最少数量的目标运动集合，并通过模糊推理实现模型转移概率的自适应变化，减少模型之间的无序竞争，滤波精确更高，能够满足多UUV协同系统水下目标跟踪的需求。

Description

一种模糊自适应多交互模型的多UUV协同系统水下目标跟踪 算法

技术领域

本发明属于水下目标跟踪领域，具体为一种模糊自适应多交互模型的多UUV协同系统水下目标跟踪算法。

背景技术

无人水下航行器(Unmanned Underwater Vehicle，UUV)水下被动目标跟踪主要利用UUV携带的水声设备被动接受水下运动目标(潜艇)的方位信息来估计目标的距离、速度和加速度等运动特性，实现对水下目标的定位与跟踪，这种方式也被称为纯方位目标跟踪(Bearing-Only Tracking)。由于该观测方式属于被动接受目标辐射信号，具有较强的隐蔽性，往往能给水下目标出其不意的毁灭性打击。但是单站UUV由于获取水下信息受限，造成观测系统的客观性很低，不能够很好的获取水下运动目标的运动信息，而多UUV协同系统组成的纯方位目标跟踪成为研究重点。

在多UUV协同系统的纯方位目标跟踪过程中,常用跟踪算法多交互模型(Interacting Multiple Model,IMM)被认为是解决水下机动目标跟踪较好的方法之一。IMM作为一种具有马尔可夫切换系数的算法是建立在广义伪贝叶斯算法的基础上，其核心思想是设计模型集合来匹配和映射目标的不同运动状态，与对应模型的滤波器并行工作,模型间利用马尔可夫链以概率矩阵实现切换,各模型滤波器通过估计状态的组合实现交互，状态估计输出为各个滤波器的状态基于Bayes推理的融合结果。

IMM兼顾目标运动模型和滤波算法两个方面，利用马尔可夫过程描述模型间的转换，对所有滤波器的输入输出进行加权综合，能较好的解决单个机动目标跟踪问题。一般地，为了更好的覆盖水下目标运动的实际状态，选取的运动模型越多越好，但是随着模型数量的增加计算量会呈指数增长，同时还会引起多模型之间的无序竞争，反而降低了跟踪算法的跟踪精度和实时性。换句话说，IMM在UUV实际应用过程中还存在以下两个问题：(1)如何选择和优化合适的目标运动模型，(2)如何设计合适、准确的模型转换概率。

发明内容

本发明针对多UUV协同系统的纯方位目标跟踪问题，在多交互模型IMM的基础上，提出一种模糊自适应多交互模型(Fuzzy Adaptive Interacting Multiple Model,FAIMM)目标跟踪算法，主要设计目标运动模型集合(Motion Mode Set,MMS)优化选择和与模型转移概率Mode Transition Probability,MTP)的模糊推理，实现多UUV协同系统对水下目标的定位与跟踪。

本发明技术方案为：

所述一种模糊自适应多交互模型的多UUV协同系统水下目标跟踪算法，其特征在于：包括以下步骤：

步骤1：进行参数初始化，初始化参数包括模型初始概率p_ij(0)，最大模型概率p_max，模糊推理参数c_DM和σ，采样周期T，并确定目标运动模型集合中采用一个匀速模型和两个协同转弯模型；

步骤2：对于第k时刻的递推循环估计，采用以下步骤进行：

步骤2.1：输入交互：

根据公式

计算状态混合估计以及协方差混合估计其中r为目标运动模型集合中的模型个数，和pⁱ(k-1|k-1)分别表示k-1时刻第i个模型的状态估计与状态协方差，i,j＝1,2…r，μ_ij(k-1)为k-1时刻混合概率：

为第j个模型归一化常数，p_ij(k-1)表示k-1时刻模型M_i转移到M_j的转移概率，μ_i(k-1)表示k-1时刻模型M_i的概率；

步骤2.2：条件滤波：

以步骤2.1得到的和作为k-1时刻第j个模型的输入，利用第j个模型对应的扩展卡尔曼滤波器进行条件滤波，计算滤波输出和p^j(k|k)；

步骤2.3：概率更新：

根据公式

计算k时刻模型j的似然函数Λ_j(k)，其中v_j(k)为模型j的滤波新息，S_j(k)为v_j(k)对应的协方差；得到模型j更新后的概率为

式中

步骤2.4：根据公式

自适应推理模型转移概率p_ij(k)，式中为隶属度函数：

和分别表示隶属度函数的中心和宽度，n表示模糊规则个数；表示第n个模糊规则对应的MTP矩阵

步骤2.5：综合输出：

根据公式

计算k时刻的总体估计和总体估计误差协方差p(k|k)；

步骤3：取k＝k+1，返回步骤2，直至跟踪结束。

有益效果

本发明在多UUV协同系统的纯方位目标跟踪过程中，选择最少数量的目标运动集合，并通过模糊推理实现模型转移概率的自适应变化，减少模型之间的无序竞争，滤波精确更高，能够满足多UUV协同系统水下目标跟踪的需求。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1：模糊自适应多交互模型的多UUV协同系统水下目标跟踪原理；

图2：UUV与目标之间的相对位置关系；

图3：高斯分布的隶属度函数；

图4：FAIMM框架设计；

图5：FAIMM水下目标跟踪轨迹；

图6：IMM水下目标跟踪轨迹。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例中，首先根据多UUV协同系统的纯方位目标跟踪原理，建立目标跟踪系统的离散型非线性的状态与观测方程。其次，根据水下目标运动的特点，结合匀速模型(CV)，匀加速模型(CA),协同转弯模型(CT),辛格模型(SG)和“当前”统计模型(CS)等常用的五种目标运动模型，依据其动力学状态转移矩阵进行分析，提出五种模型之间的耦合不等式关系，优化选择适应水下目标跟踪的运动模型集合(Motion Mode Set,MMS)。再次，采用中间型的高斯分布函数作为隶属度函数，将模型概率作为评价每个模型获取滤波新息和对应的协方差的评价指标，设计模型转移概率Mode Transition Probability,MTP)的模糊推理。最后，设计与实现FAIMM的算法步骤，并进行实验验证分析。模糊自适应多交互模型的多UUV协同系统纯方位水下目标跟踪原理如图1所示。

1.目标跟踪原理的非线性建模

UUV观测站点利用声呐传感系统对目标进行方位观测:方位角(Azimuth Angle,β)和俯仰角(Pitch Angle,α)，其中β表征目标水平方向的位置信息，α表征目标垂直方向的位置信息。具体UUV与目标之间的相对位置关系在笛卡尔坐标系中如图2所示。

由于水下目标跟踪系统中背景噪声干扰往往较大，并且目标可能会进行机动操作致使系统具有很大的随机性和非线性。因此，目标跟踪系统采用离散型非线性的状态与观测方程：

其中表示k时刻目标状态：位置x_k,y_k,z_k和速度信息，Φ(k)表示状态转移矩阵，G(k)表示系统输入噪声矩阵，W(k)表示过程噪声；Z(k)表示观测状态，H(k)表示观测矩阵,V(k)表示观测噪声。

在图2中，Z(k)表示观测状态主要包括β(k)和α(k)，其定义如：

考虑到水下目标运动具有速度慢,设定目标运动深度不变，目标深度z_k为常量，则β_k和α_k转化为：

其中，β_k和α_k的测量误差是相互独立的零均值高斯白噪声V(k)＝[ν_β(k),ν_α(k)]，其方差矩阵表示为R＝diag[λ_β,λ_α]。

2.多交互模型(IMM)

多交互模型(IMM)是设计模型集合来匹配和映射目标的不同运动状态，与对应模型的滤波器并行工作,模型间利用马尔可夫链以概率矩阵实现切换,各模型滤波器通过估计状态的组合实现交互,状态估计输出为各个滤波器的融合结果。IMM是一个递推循环的模型，每一个递推过程主要涵盖四个步骤，具体分析如下：

Step1输入交互：有限模型集合M＝{M₁,M₂…M_r},r表示模型个数,p_ij表示模型M_i转移到M_j的转移概率,μ_i(k-1)表示k-1时刻模型M_i的概率,i,j＝1,2…r,则k-1时刻混合概率μ_ij(k-1)为：

c_j为第j个模型归一化常数。

和pⁱ(k-1|k-1)分别表示k-1时刻第i个模型对应滤波器i的状态估计与状态协方差，i,j＝1,2…r，则状态与协方差的混合估计分别为：

Step2条件滤波：和作为k-1时刻第j个模型的输入，利用对应的扩展卡尔曼滤波器EKF进行条件滤波，计算滤波输出和

Step3概率更新：若模型j的滤波新息v_j(k)和对应的协方差S_j(k)，则在k时刻模型j的似然函数Λ_j(k)为：

则模型概率更新计算为：

式中，

Step4综合输出(Combination)：经过以上计算，则k时刻的总体估计和总体估计误差协方差p(k|k)分别为：

3.运动模型集合MMS的优化选取

IMM模型应用于水下目标跟踪时，常用的目标运动模型有以下五种：匀速模型(CV)，匀加速模型(CA),协同转弯模型(CT),辛格模型(SG)和“当前”统计模型(CS)。根据从运动学模型角度对这五种模型进行分析，给出MMS的详细优化选取过程。

CV模型用于跟踪匀速运动目标，其一维和二维状态转移矩阵分别为：

CA模型用于跟踪匀加速直线运动目标，其一维态转移矩阵分别为：

SG模型将目标加速度描述成时间相关随机过程，其一维态转移矩阵分别为：

式中，T为时间常数，1/α是一个与机动时间有关的常量，若α→∞，根据公式(14)和(15),则若α→0，根据公式(12)和(15),则可见SG模型随着加速因子α的变化在CV和CA模型之间变化，即CV和CA模型为SG的极限形式。

CS模型采用非零均值的修正瑞利分布来表征目标机动的加速度特征，其一维态转移矩阵与SG模型相同：

CT模型表示目标机动转弯过程，其二维态转移矩阵分别为：

式中，T为时间常数，w为转角角速度，若w→0，根据公式(13)和(17),则即CT退化为CV模型，可见CT模型与CV模型具有高度的耦合性。

由公式(12)～(17)综合可知，以上五个模型之间具有相似性和耦合性，根据运动学模型建立不等式关系：

CV＜SG＝CS≤CA＜CT (18)

根据公式(18)蕴含意义可知，SG,CS和CA模型可以根据CV与CT模型的权重之和协调获得；同时，CT模型中虽然转弯方向不知，但是转弯角速度w决定的最大值一般情况下可知。因此，MMS优化选取一个CV模型和两个CT模型共三个模型可以满足水下目标的跟踪的需要，这样不仅可以减少MMS模型数量和计算资源，还可以确保跟踪效果。

4.模糊推理的时变模型转移概率设计

马尔可夫链模型转移概率MTP作为IMM模型水下目标跟踪的另一关键因素，其直接影响模型误差以及模型概率估计的准确性，因此合理地选择状态转移矩阵显得尤为重要。一般情况下，IMM模型之间的转移概率在跟踪目标时根据一定的规律性对其进行固定设置。但是，这种固定方式设置的MTP存在无用模型对有用模型的竞争，进而降低了跟踪精确度。为此，利用模糊推理设计时变模型转移概率(TVMTP)自适应调整模型概率，在MMS确定的情况下减少无用模型对目标跟踪精度的影响。由于模型概率μ_i(k)表示作为评价每个模型获取滤波新息v_i(k-1)和对应的协方差S_i(k-1)的评价指标。因此，TVMTP主要思想是将μ_i(k-1)作为模糊推理的输入，通过模糊规则推理自动调整MTP。

由于潜艇目标在水下目标跟踪的过程中，往往以低速进行连续机动，因此采用中间型的高斯分布函数作为隶属度函数，具体定义为：

式中，和分别表示第i隶属度函数的中心和宽度，n＝0,1,2…,r表示模糊规则个数，A_i表示输入变量的模糊术语，主要分为负大(Recessive Big,RB),负中(RecessiveMedium,RM),零(Zero,ZO),正中(Dominant Medium,DM)和正大(Dominant Big,DB),其分布如图5所示。

由于模型概率μ_i(k-1)归一化后其和为1，因此可以直接采用模型概率作为输入，则RB和DB中心隶属函数设定为和ZO中心隶属函数设定RM和DM中心隶属函数则主要根据公式(19)进行设定：如果模型之间相似度高，则接近如果模型之间相似度低，则远离因此RM中心隶属函数则设定为c_RM＝(1-c_DM)/(r-1)，同时为了计算方便RB,RM,ZO,DM,DB的宽度设置相同

模糊规则推理计算形式建立如下：

式中，Rⁿ表示第n个模糊规则，表示第n个模糊规则对应的MTP矩阵，其计算：

式中，p_max模型概率最大值。

则对应的模糊规则的解模糊计算：

式中，

p_ij(k)表示所求解的时变MTP,用其代替公式(4)～(5)中的模型转移概率进行IMM递推。

根据以上分析，则形成FAIMM流程如图4所示，具体步骤如下所示：

Step1初始化：运动模型集合MMS(1个CT模型和2个CT模型),模型初始概率p_ij(0)，最大模型概率p_max，模糊推理参数c_DM和σ，采样周期T；

Step2利用公式(6)-(7)进行输入交互，和

Step3利用扩展卡尔曼滤波器EKF进行条件滤波，输出和p^j(k|k)；

Step4利用(8)～(9)进行模型转移更新μ_j(k)；

Step5利用(19)～(23)模型转移概率MTP的自适应推理p_ij(k)；

Step6利用(10)～(11)状态估计输出和p(k|k)；

Step7 k＝k+1，转向Step2，直到结束。

应用上述方法，本实施例中假设9个的UUV随机静态分布在4000×7000水下空间，对敌方潜艇目标进行跟踪。设定UUV与潜艇固定航深，则水下三维空间退化为二维空间目标跟踪，潜艇状态向量为x_k与y_k分别表示X与Y轴位置，与分别表示X与Y轴速度,航深z_k＝-10，起始位置X(0|0)＝[4000 5500 25 25]^T，初始航向，测量噪声方差R＝diag{σ_β,σ_α}＝diag{0.2,0.2}。其中目标在时间1-100s,120-130s和140-200s以25m/s速度匀速直线，在时间100-120s和130-140内分别以1.62°/s和-1.62°/s进行转弯运动。

运动模型集合MMS由一个CV模型和2个CT模型组成r＝3，对应3个KF滤波器，UUV采样周期为T＝2s，模型初始概率p_ij(0)＝0.33，模型概率最大值p_max＝0.98；模糊推理参数c_DM＝0.66，σ＝0.33。

在上述设定条件下目标跟踪的轨迹如图5～6所示，同时为了验证FAIMM方法的有效性，通过与IMM方法在相同设定条件下使用MATLAB进行100次蒙特卡洛，进行统计实现根均方误差(RMSE)定量化分析如表1所示。

表1 蒙特卡洛统计分析结果

从图5和图6对比可以看出，在开始匀速阶段，FAIMM与IMM两种方法可以实现对目标的有效跟踪，但是在机动转弯过程中FAIMM能够跟踪机动过程，而IMM方法则会产生较大响应延迟，随着时间增长，产生更大误差，同时在X-Y方向位置与速度误差都低于IMM。之所以FAIMM能够实现对目标运动过程的有效跟踪得益于在保证有效且最少的运动目标集MMS，并且能够自适应调节模型概率实现了在目标运动过程的匹配，减少了目标模型之间的竞争。

总的来说，在多UUV协同系统的纯方位目标跟踪过程中，FAIMM优化选择最少数量的目标运动集合，并通过模糊推理实现模型转移概率的自适应变化，减少模型之间的无序竞争，滤波精确更高，能够满足多UUV协同系统水下目标跟踪的需求。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种模糊自适应多交互模型的多UUV协同系统水下目标跟踪算法，其特征在于：

包括以下步骤：

步骤1：进行参数初始化，初始化参数包括模型初始概率p_ij(0)，最大模型概率p_max，模糊推理参数c_DM和σ，采样周期T，并确定目标运动模型集合中采用一个匀速模型和两个协同转弯模型；

步骤2：对于第k时刻的递推循环估计，采用以下步骤进行：

步骤2.1：输入交互：

根据公式

计算状态混合估计以及协方差混合估计其中r为目标运动模型集合中的模型个数，和pⁱ(k-1|k-1)分别表示k-1时刻第i个模型的状态估计与状态协方差，i,j＝1,2…r，μ_ij(k-1)为k-1时刻混合概率：

<mrow> <msub> <mi>&amp;mu;</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>c</mi> <mi>j</mi> </msub> </mfrac> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

为第j个模型归一化常数，p_ij(k-1)表示k-1时刻模型M_i转移到M_j的转移概率，μ_i(k-1)表示k-1时刻模型M_i的概率；

步骤2.2：条件滤波：

以步骤2.1得到的和作为k-1时刻第j个模型的输入，利用第j个模型对应的扩展卡尔曼滤波器进行条件滤波，计算滤波输出和p^j(k|k)；

步骤2.3：概率更新：

根据公式

<mrow> <msub> <mi>&amp;Lambda;</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <msqrt> <mrow> <mn>2</mn> <mi>&amp;pi;</mi> <mo>|</mo> <msub> <mi>S</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </msqrt> </mfrac> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mi>v</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>(</mo> <mi>k</mi> <mo>)</mo> <msubsup> <mi>S</mi> <mi>j</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mo>(</mo> <mi>k</mi> <mo>)</mo> <msub> <mi>v</mi> <mi>j</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>)</mo> </mrow> </mrow>

计算k时刻模型j的似然函数Λ_j(k)，其中v_j(k)为模型j的滤波新息，S_j(k)为v_j(k)对应的协方差；得到模型j更新后的概率为

<mrow> <msub> <mi>&amp;mu;</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mi>c</mi> </mfrac> <msub> <mi>&amp;Lambda;</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>c</mi> <mi>j</mi> </msub> </mrow>

式中

步骤2.4：根据公式

<mrow> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msub> <mi>b</mi> <mi>n</mi> </msub> <msubsup> <mi>p</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mi>n</mi> </msubsup> <mo>/</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msub> <mi>b</mi> <mi>n</mi> </msub> </mrow>

自适应推理模型转移概率p_ij(k)，式中为隶属度函数：

<mrow> <msub> <mi>f</mi> <msubsup> <mi>A</mi> <mi>i</mi> <mi>n</mi> </msubsup> </msub> <mrow> <mo>(</mo> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>(</mo> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>=</mo> <mi>exp</mi> <mo>&amp;lsqb;</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>c</mi> <mi>i</mi> <mi>n</mi> </msubsup> </mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>i</mi> <mi>n</mi> </msubsup> </mfrac> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>&amp;rsqb;</mo> </mrow>

和分别表示隶属度函数的中心和宽度，n表示模糊规则个数；表示第n个模糊规则对应的MTP矩阵

<mrow> <msub> <mi>p</mi> <mi>j</mi> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mn>1</mn> <mo>/</mo> <mi>r</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>n</mi> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>n</mi> <mo>&amp;NotEqual;</mo> <mn>0</mn> <mo>,</mo> <mi>a</mi> <mi>n</mi> <mi>d</mi> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <msub> <mi>p</mi> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mo>)</mo> <mo>/</mo> <mi>r</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>n</mi> <mo>&amp;NotEqual;</mo> <mn>0</mn> <mo>,</mo> <mi>a</mi> <mi>n</mi> <mi>d</mi> <mo>,</mo> <mi>j</mi> <mo>&amp;NotEqual;</mo> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>

步骤2.5：综合输出：

根据公式

计算k时刻的总体估计和总体估计误差协方差p(k|k)；

步骤3：取k＝k+1，返回步骤2，直至跟踪结束。