CN109671099B - 一种基于路网结构和目标特性的目标跟踪算法 - Google Patents

Abstract

本发明涉及一种基于道路信息库和目标道路约束特性的变结构多模型目标跟踪算法，构建道路信息库，应用路网为基础的道路地理环境自身的约束能力以及不同类型目标对不同道路地理环境的适应能力作为先验信息辅助地面目标跟踪，通过先验知识的充分利用提高地面目标跟踪的状态估计精度，降低目标跟丢率。本发明在地面目标跟踪问题上有其实用价值。

Description

一种基于路网结构和目标特性的目标跟踪算法

技术领域

本发明属于目标跟踪技术领域，具体涉及一种基于道路信息库和目标道路约束特性的变结构多模型目标跟踪算法。

背景技术

对于地面目标而言，大部分地面目标受道路或地理环境的约束，其运动往往具有一定的可预测性，而这些道路和地理信息通常是可以预先获得的，在目标状态估计时利用这些先验信息，在一定程度上可以提高目标状态估计精度，并简化跟踪处理器的计算复杂性。变结构多模型算法是应用先验信息辅助地面目标跟踪的一种思路，其思想是，根据先验信息调整目标运动模型，每一跟踪时刻，在目标所有可能的运动模型中，选择与当前目标真实运动状态匹配的模型子集进行跟踪滤波。基于道路辅助的变结构交互式多模型(VS-IMM)算法，假定不同的道路地理环境对应于不同的目标运动模型集合，模型自适应规则由道路地理信息决定的，根据道路地理信息以及上一步的状态估计值来更新目标的当前运动模型集，其后以IMM作为状态估计器估计目标状态，实现道路地理信息辅助的地面目标跟踪。

需要注意到的是，上述方案仅利用了道路地理环境自身的约束能力作为先验信息来辅助地面目标跟踪，没有考虑不同地形对不同类型目标约束能力的不同，以及不同目标自身运动特性的不同。例如，卡车、炮车、装甲车等目标一般只能在道路和平地上行驶，坦克、步兵作战车等可以在较崎岖的地形上运动，但是，仍受某些地形条件比如沟壑和峭壁的限制。

发明内容

本发明所要解决的，是将以路网为基础的道路地理环境自身的约束能力以及不同类型目标对不同道路地理环境的适应能力作为先验信息，提供一种基于道路信息库和目标道路约束特性的变结构多模型目标跟踪算法。

本发明所提出的技术问题是这样解决的：

一种基于路网结构和目标特性的目标跟踪算法，包括以下步骤：

A.根据实际应用场景建立地理信息知识库和目标运动特征库：

B.建立航迹表，航迹表记录每个航迹的状态估计、航迹的当前运动模型集、航迹的当前匹配地形类型、以及航迹的地形标记，其中，航迹的地形标记包括道路目标标记、节点目标标记和路外目标标记；

C.每一仿真时刻，依据航迹当前地形标记的不同，对不同航迹进行不同的处理流程，每个处理流程包括目标与具体地形位置的匹配、模型集自适应判定以及多模型集的跟踪滤波；

C-1.采用预测不确定区域判别法进行目标与具体地形的匹配，包括是否在道路节点附近的判定、是否在道路节点的判定以及是否在道路上的判定，地形匹配后根据航迹的历史地形标记信息和当前地形匹配信息更新航迹的当前地形标记；

C-2.根据目标匹配的地形位置激活该地形相应的先验目标运动模型集作为候选模型群，通过当前航迹目标的先验地形约束率、近期地形标记和地形转移概率经验矩阵修正当前模型转移马尔科夫矩阵；

C-3.将旧的航迹当前运动模型集以及C-2激活的候选运动模型群组合，作为新的航迹当前运动模型集，以交互式多模型滤波器作为状态估计器估计目标状态，并分别计算旧当前运动模型群、新激活候选群的群模型概率，且由群模型切换判决法判决是否在下一时刻删除旧当前运动模型群和新激活候选群。

本发明的有益效果是：

构建道路信息库，以路网为基础的道路地理环境自身的约束能力以及不同类型目标对不同道路地理环境的适应能力作为先验信息，提出一种基于道路信息库和目标道路约束特性的变结构多模型目标跟踪算法，通过先验知识的充分利用提高地面目标跟踪的状态估计精度，降低目标跟丢率。本发明在地面目标跟踪问题上有其实用价值。

附图说明

图1为本发明所述方法一个仿真周期的流程图；

图2为道路目标处理流程图；

图3为节点目标处理流程图；

图4为路外目标处理流程图。

具体实施方式

下面结合附图和实施例对本发明进行进一步的说明。

本实施例提供一种基于路网结构和目标特性的目标跟踪算法，其流程图如图1所示，包括以下步骤：

A.根据实际应用场景建立地理信息知识库和目标运动特征库：

为传感器监视区域建立基于道路网的地理信息知识库，建模信息包括道路拓扑结构、地形环境条件，以及地形环境对目标运动产生的约束能力，为各运动目标建立对不同道路、地形环境适应能力的目标运动特征库；

A-1.所述道路网建模为一系列道路片段和道路节点，道路节点被放置在道路属性(道路方向、道路可见性、道路开放性)发生变化的位置，以及不同道路的交叉点处，信息库分别保存各道路路段和道路节点的属性以及它们之间的连接关系；

A-2.所述道路和地形对目标运动的约束能力建模为各道路路段、道路节点和其他地形区域对应的目标运动模型集合；

A-3.所述目标运动特征库建模为目标的先验地形约束率和地形转移概率经验矩阵。

B.建立航迹表，航迹表记录每个航迹的状态估计、航迹的当前运动模型集、航迹的当前匹配地形类型、以及航迹的地形标记，其中，航迹的地形标记包括道路目标标记、节点目标标记和路外目标标记；

第k个扫描时刻航迹号为n的航迹状态记为Tⁿ(k)：

其中，Mⁿ(k)为航迹n在当前时刻对应的运动模型集，

和Pⁿ(k)分别为当前时刻的状态估计和协方差估计；

分别为模型r下的状态估计、协方差估计以及模型r在模型集Mⁿ(k)中的模型概率，r为模型集Mⁿ(k)中的模型；τⁿ(k)为航迹的地形标记，αⁿ(k)为航迹的当前地形匹配。

C.每一仿真时刻，依据航迹当前地形标记的不同，对不同航迹进行不同的处理流程，每个处理流程包括目标与具体地形位置的匹配、模型集自适应判定以及多模型集的跟踪滤波，具体如下：

C-1.采用预测不确定区域判别法进行目标与具体地形的匹配，包括是否在道路节点附近的判定、是否在道路节点的判定以及是否在道路上的判定，地形匹配后根据航迹的历史地形标记信息和当前地形匹配信息更新航迹的当前地形标记；

步骤C-1中的预测不确定区域判别法的具体过程为：

目标的预测不确定区域εⁿ(k)定义为：

其中，x和y分别为横纵坐标变量，x_p(k^-)和y_p(k^-)分别为目标预测点的横纵坐标，(x_p(k^-)，y_p(k^-))为由滤波器给出的k时刻目标的位置状态预测，

为k时刻目标状态预测协方差矩阵Pⁿ(k^-)的位置子矩阵，P_xx(k^-)、P_xy(k^-)、P_yx(k^-)、P_yy(k^-)分别为Pⁿ(k^-)的对应元素值，α为预测与确定区域的判决门限值，此时目标的预测不确定区域εⁿ(k)为以目标预测点为中心的一个椭圆区域；

此处，由于使用多模型进行跟踪滤波，每个运动模型都对应一个目标的位置状态预测

和状态预测协方差矩阵

不存在统一的目标位置预测，为了解决这个问题，选择新息协方差矩阵行列式最大的那个运动模型对应的

和

通过判断目标预测不确定区域椭圆与相应地理区域是否相交可以判定目标是否到达某个地理区域。

(1)标记为道路目标的处理流程：对于道路目标，首先判断目标是否到达道路临近节点，如果到达某个节点，进行节点模型集的添加和旧模型集的删除，同时将目标当前位置标记设为该节点；如果目标未到达临近节点，判断目标是否还在道路上，如果仍在道路上，无需模型集的改变直接进行跟踪滤波；如果目标不在道路上，接下来m个时刻对当前道路和当前道路的邻接道路和节点进行判断，如果连续m个时刻目标预测位置均不在道路或节点上，则判定该目标已驶离道路，进行路外模型集的添加和旧模型集的删除，同时将目标当前位置标记设为路外，其中，m为一个经验时刻值，一般取值3～5；流程图见图2；

(2)标记为节点目标的处理流程：对于节点目标，首先判断目标是否还在节点，如果仍在节点上，无需模型集的改变直接进行跟踪滤波；如果目标不在节点，判断目标是否运行到相邻道路，如果运行到某条相邻道路，进行道路模型集的添加和旧模型集的删除，同时将目标当前位置标记设为该道路；如果目标不在节点也不在道路，接下来m个时刻对当前节点和当前节点邻接道路都进行判断，如果连续m个时刻目标预测位置均不在道路或节点上，则判定该目标已驶离道路，进行路外模型集的添加和旧模型集的删除，同时将目标当前位置标记设为路外，其中，m取值前面已给出；流程图见图3；

(3)标记为路外目标的处理流程：对于路外目标，首先判断目标预测是否进入目标不可达区，如果连续c个时刻目标预测位置均在目标不可达区，直接对进行航迹终结，其中，c为经验时刻阈值；否则判断目标是否到达某个道路节点，如果目标到达某个节点或某段道路，进行节点或道路模型集的添加，但暂时不改变目标的当前位置标记，只有连续d个时刻目标预测位置均在节点或道路上，才判定该目标已到达当前节点或当前道路，并且将目标当前位置标记设为当前节点或当前道路，其中，d为一个经验时刻值，一般取值3～5；否则按照路外目标进行跟踪滤波；流程图见图4；

步骤C-1中，目标是否到达道路临近节点的具体的判定过程为：通过目标最大速度判断目标是否在道路节点附近，假设道路节点坐标为(x_junc，y_junc)，判断下式是否成立：

若成立，说明目标在节点附近，其中v_max为目标最大速度，T为仿真间隔。

步骤C-1中，目标是否在道路节点上的具体的判定过程为：假设道路节点坐标为(x_junc，y_junc)，判断下式是否成立：

若成立，则预测目标到达节点(x_junc，y_junc)处，否则不在；其中α_junc为节点判决阈，α_junc决定了节点区域的大小，α_junc越大，节点附近机动区域越大，相当于节点管辖范围越大；

步骤C-1中，目标是否在道路上的具体的判定过程为：假设道路端点为(x₁，y₁)和(x₂，y₂)，目标预测位置即椭圆中心为(x₀，y₀)，不确定区域椭圆区域为：

其中，

k时刻目标状态预测协方差矩阵Pⁿ(k^-)的位置子矩阵，L₁₁、L₁₂、L₂₁、L₂₂分别为Pⁿ(k^-)的对应元素值，α_road为道路不确定区域判决阈值；

首先判断道路两个端点是否在不确定区域椭圆区域内，只要有一点满足条件，则可判定目标预测位置在道路上；如果都不满足，令L₀＝L₁₂+L₂₁，当x₁＝x₂时，计算a₁＝L₂₂，b₁＝L₀(x₁-x₀)，c₁＝L₁₁(x₁-x₀)²-α_road，判断Δ₁＝b₁ ²-4a₁c₁≥0是否成立，若不成立，退出判断；如果成立，则令k₁＝-b₁/2a₁+y₀，继续判断(y₁-k₁)(y₂-k₁)≤0是否成立；若成立，说明道路段与不确定区域椭圆相交，否则不相交；当x₁≠x₂时，令a₂＝L₁₁+L₀A₁+L₂₂A₁ ²，b₂＝L₀B+2L₂₂A₁A₂，c₂＝L₂₂A₂ ²-α_road，其中A₁＝(y₂-y₁)/(x₂-x₁)，A₂＝(x₂y₁-x₁y₂)/(x₂-x₁)-y₀+A₁x₀，判Δ₂＝b₂ ²-4a₂c₂≥0是否成立，若不成立，退出判断；如果成立，则令k₂＝-b₂/2a₂+x₀，继续判断(x₁-k₂)(x₂-k₂)≤0是否成立；若成立，说明道路段与不确定区域椭圆相交，否则不相交。

C-2.根据目标匹配的地形位置激活该地形相应的先验目标运动模型集作为候选模型群，通过当前航迹目标的先验地形约束率、近期地形标记和地形转移概率经验矩阵修正当前模型转移马尔科夫矩阵；

目标地形位置的匹配后，一个仿真周期内的运动模型自适应切换和滤波跟踪流程为：

C-2-1.候选模型群

激活的策略：这里的候选模型群激活决策通过目标当前所在的道路地理环境决定，主要依靠预测道路信息和地理信息判断模型集的激活；

C-2-2.如果有候选模型群被激活，则更新当前目标运动模型集

其中，

为旧的当前运动模型集；

C-2-3.根据目标匹配的地形位置激活该地形相应的先验目标运动模型集作为候选模型群，通过当前航迹目标的先验地形约束率、近期地形标记和地形转移概率经验矩阵修正当前模型转移马尔科夫矩阵{p_ij}，其中，1≤i≤|Mⁿ(k-1)|，1≤j≤|Mⁿ(k)|，|Mⁿ(k-1)|为Mⁿ(k-1)模型个数，|Mⁿ(k)|为Mⁿ(k)模型个数。

C-3.将旧的航迹当前运动模型集以及C-2激活的候选运动模型群组合，作为新的航迹当前运动模型集，以交互式多模型滤波器作为状态估计器估计目标状态，并分别计算旧当前运动模型群、新激活候选群的群模型概率，且由群模型切换判决法判决是否在下一时刻删除旧当前运动模型群和新激活候选群。

C-3-1.借助旧的当前运动模型集

和修正的当前模型转移马尔科夫矩阵{μ_i|j(k-1)}，基于新的当前运动模型集Mⁿ(k)运行VS-IMM(Variable StructureInteracting Multiple Model，变结构交互式多模型算法)，估计目标状态以及各模型概率；

所述VS-IMM估计目标状态以及各模型概率流程如下：

C-3-1-1.k-1时刻运动模型集为Mⁿ(k-1)，k时刻的运动模型集为Mⁿ(k)；

C-3-1-2.滤波器的重初始化：

根据k-1时刻各子模型的状态估计来计算k时刻各子滤波器M_j(k)的初始状态估计和初始协方差矩阵：

其中，

为k-1时刻Mⁿ(k-1)模型集模型i对应的状态估计，

μ_i|j(k-1)为k时刻模型为j的情况下k-1时刻模型为i的条件概率，P_i(k-1)为k-1时刻Mⁿ(k-1)模型集模型i对应的状态估计协方差矩阵；

C-3-1-3.各子模型独立滤波，并计算模型似然值，似然值为：

其中，v_j(k)为量测新息，S_j(k)为新息协方差矩阵；

C-3-1-4.计算模型概率：模型j的概率更新为：

其中，

C-3-1-5.计算融合结果和协方差：

其中，

P_j(k)和μ_j(k)分别为k时刻模型j下的状态估计、协方差估计及模型j在模型集Mⁿ(k)中的模型概率。

C-3-2.分别计算旧当前运动模型集

新激活候选群

的群模型概率；当满足

时，候选模型群

在下一时刻终止；当满足

时，当前起作用的模型群

在下一时刻终止；其中，t₁和t₂为模型终止判决阈值，

为新激活候选群

的模型概率和，

为旧当前运动模型集

的模型概率和。

Claims (1)

1.一种基于路网结构和目标特性的目标跟踪方法，其特征在于，包括以下步骤：

A.根据实际应用场景建立地理信息知识库和目标运动特征库；

步骤A的具体过程为：

为传感器监视区域建立基于道路网的地理信息知识库，建模信息包括道路拓扑结构、地形环境条件，以及地形环境对目标运动产生的约束能力，为各运动目标建立对不同道路、地形环境适应能力的目标运动特征库；

A-1.所述道路网建模为一系列道路片段和道路节点，道路节点被放置在道路属性发生变化的位置，以及不同道路的交叉点处，信息库分别保存各道路路段和道路节点的属性以及它们之间的连接关系；

A-2.所述道路和地形对目标运动的约束能力建模为各道路路段、道路节点和其他地形区域对应的目标运动模型集合；

A-3.所述目标运动特征库建模为目标的先验地形约束率和地形转移概率经验矩阵；

B.建立航迹表，航迹表记录每个航迹的状态估计、航迹的当前运动模型集、航迹的当前匹配地形类型、以及航迹的地形标记，其中，航迹的地形标记包括道路目标标记、节点目标标记和路外目标标记；

步骤B中，第k个扫描时刻航迹号为n的航迹状态记为Tⁿ(k)：

其中，Mⁿ(k)为航迹n在当前时刻对应的运动模型集，

和Pⁿ(k)分别为当前时刻的状态估计和协方差估计；

分别为模型r下的状态估计、协方差估计以及模型r在模型集Mⁿ(k)中的模型概率，r为模型集Mⁿ(k)中的模型；τⁿ(k)为航迹的地形标记，aⁿ(k)为航迹的当前地形匹配；

C.每一仿真时刻，依据航迹当前地形标记的不同，对不同航迹进行不同的处理流程，每个处理流程包括目标与具体地形位置的匹配、模型集自适应判定以及多模型集的跟踪滤波；

C-1.采用预测不确定区域判别法进行目标与具体地形的匹配，包括是否在道路节点附近的判定、是否在道路节点的判定以及是否在道路上的判定，地形匹配后根据航迹的历史地形标记信息和当前地形匹配信息更新航迹的当前地形标记；

步骤C-1中的预测不确定区域判别法的具体过程为：

目标的预测不确定区域εⁿ(k)定义为：

其中，x和y分别为横纵坐标变量，x_p(k^-)和y_p(k^-)分别为目标预测点的横纵坐标，(x_p(k^-),y_p(k^-))为由滤波器给出的k时刻目标的位置状态预测，

为k时刻目标状态预测协方差矩阵Pⁿ(k^-)的位置子矩阵，P_xx(k^-)、P_xy(k^-)、P_yx(k^-)、P_yy(k^-)分别为Pⁿ(k^-)的对应元素值，a为预测与确定区域的判决门限值；

步骤C-1中，

(1)标记为道路目标的处理流程：对于道路目标，首先判断目标是否到达道路临近节点，如果到达某个节点，进行节点模型集的添加和旧模型集的删除，同时将目标当前位置标记设为该节点；如果目标未到达临近节点，判断目标是否还在道路上，如果仍在道路上，无需模型集的改变直接进行跟踪滤波；如果目标不在道路上，接下来m个时刻对当前道路和当前道路的邻接道路和节点进行判断，如果连续m个时刻目标预测位置均不在道路或节点上，则判定该目标已驶离道路，进行路外模型集的添加和旧模型集的删除，同时将目标当前位置标记设为路外，其中，m为一个经验时刻值；

(2)标记为节点目标的处理流程：对于节点目标，首先判断目标是否还在节点，如果仍在节点上，无需模型集的改变直接进行跟踪滤波；如果目标不在节点，判断目标是否运行到相邻道路，如果运行到某条相邻道路，进行道路模型集的添加和旧模型集的删除，同时将目标当前位置标记设为该道路；如果目标不在节点也不在道路，接下来m个时刻对当前节点和当前节点邻接道路都进行判断，如果连续m个时刻目标预测位置均不在道路或节点上，则判定该目标已驶离道路，进行路外模型集的添加和旧模型集的删除，同时将目标当前位置标记设为路外；

(3)标记为路外目标的处理流程：对于路外目标，首先判断目标预测是否进入目标不可达区，如果连续c个时刻目标预测位置均在目标不可达区，直接对进行航迹终结，其中，c为经验时刻阈值；否则判断目标是否到达某个道路节点，如果目标到达某个节点或某段道路，进行节点或道路模型集的添加，但暂时不改变目标的当前位置标记，只有连续d个时刻目标预测位置均在节点或道路上，才判定该目标已到达当前节点或当前道路，并且将目标当前位置标记设为当前节点或当前道路，其中，d为一个经验时刻值，否则按照路外目标进行跟踪滤波；

步骤C-1中目标是否到达道路临近节点的具体的判定过程为：通过目标最大速度判断目标是否在道路节点附近，假设道路节点坐标为(x_junc,y_junc)，判断下式是否成立：

若成立，说明目标在节点附近，其中v_max为目标最大速度，T为仿真间隔；

步骤C-1中目标是否在道路节点上的具体的判定过程为：假设道路节点坐标为(x_junc,y_junc)，判断下式是否成立：

若成立，则预测目标到达节点(x_junc,y_junc)处，否则不在；其中α_junc为节点判决阈；

步骤C-1中目标是否在道路上的具体的判定过程为：假设道路端点为(x₁,y₁)和(x₂,y₂)，目标预测位置即椭圆中心为(x₀,y₀)，不确定区域椭圆区域为：

其中，

k时刻目标状态预测协方差矩阵Pⁿ(k^-)的位置子矩阵，L₁₁、L₁₂、L₂₁、L₂₂分别为Pⁿ(k^-)的对应元素值，α_road为道路不确定区域判决阈值；

首先判断道路两个端点是否在不确定区域椭圆区域内，只要有一点满足条件，则可判定目标预测位置在道路上；如果都不满足，令L₀＝L₁₂+L₂₁，当x₁＝x₂时，计算a₁＝L₂₂，b₁＝L₀(x₁-x₀)，c₁＝L₁₁(x₁-x₀)²-α_road，判断Δ₁＝b₁ ²-4a₁c₁≥0是否成立，若不成立，退出判断；如果成立，则令k₁＝-b₁/2a₁+y₀，继续判断(y₁-k₁)(y₂-k₁)≤0是否成立；若成立，说明道路段与不确定区域椭圆相交，否则不相交；当x₁≠x₂时，令a₂＝L₁₁+L₀A₁+L₂₂A₁ ²，b₂＝L₀B+2L₂₂A₁A₂，c₂＝L₂₂A₂ ²-a_road，其中A₁＝(y₂-y₁)/(x₂-x₁)，A₂＝(x₂y₁-x₁y₂)/(x₂-x₁)-y₀+A₁x₀，判D₂＝b₂ ²-4a₂c₂≥0是否成立，若不成立，退出判断；如果成立，则令k₂＝-b₂/2a₂+x₀，继续判断(x₁-k₂)(x₂-k₂)≤0是否成立；若成立，说明道路段与不确定区域椭圆相交，否则不相交；

C-2.根据目标匹配的地形位置激活该地形相应的先验目标运动模型集作为候选模型群，通过当前航迹目标的先验地形约束率、近期地形标记和地形转移概率经验矩阵修正当前模型转移马尔科夫矩阵；

步骤C-2的具体过程为：

C-2-1.候选模型群

激活的策略：通过目标当前所在的道路地理环境决定，依靠预测道路信息和地理信息判断模型集的激活；

C-2-2.如果有候选模型群被激活，则更新当前目标运动模型集

其中，

为旧的当前运动模型集；

C-2-3.根据目标匹配的地形位置激活该地形相应的先验目标运动模型集作为候选模型群，通过当前航迹目标的先验地形约束率、近期地形标记和地形转移概率经验矩阵修正当前模型转移马尔科夫矩阵{p_ij}，其中，1≤i≤|Mⁿ(k-1)|，1≤j≤|Mⁿ(k)|，|Mⁿ(k-1)|为Mⁿ(k-1)模型个数，|Mⁿ(k)|为Mⁿ(k)模型个数；

C-3.将旧的航迹当前运动模型集以及C-2激活的候选运动模型群组合，作为新的航迹当前运动模型集，以交互式多模型滤波器作为状态估计器估计目标状态，并分别计算旧当前运动模型群、新激活候选群的群模型概率，且由群模型切换判决法判决是否在下一时刻删除旧当前运动模型群和新激活候选群；

步骤C-3的具体过程为：C-3-1.借助旧的当前运动模型集

和修正的当前模型转移马尔科夫矩阵{μ_i|j(k-1)}，基于新的当前运动模型集Mⁿ(k)运行VS-IMM，估计目标状态以及各模型概率；

所述VS-IMM估计目标状态以及各模型概率流程如下：

C-3-1-1.k-1时刻运动模型集为Mⁿ(k-1)，k时刻的运动模型集为Mⁿ(k)；

C-3-1-2.滤波器的重初始化：

根据k-1时刻各子模型的状态估计来计算k时刻各子滤波器M_j(k)的初始状态估计和初始协方差矩阵：

其中，

为k-1时刻Mⁿ(k-1)模型集模型i对应的状态估计，

μ_i|j(k-1)为k时刻模型为j的情况下k-1时刻模型为i的条件概率，P_i(k-1)为k-1时刻Mⁿ(k-1)模型集模型i对应的状态估计协方差矩阵；

C-3-1-3.各子模型独立滤波，并计算模型似然值，似然值为：

其中，v_j(k)为量测新息，S_j(k)为新息协方差矩阵；

C-3-1-4.计算模型概率：模型j的概率更新为：

其中，

C-3-1-5.计算融合结果和协方差：

其中，

P_j(k)和μ_j(k)分别为k时刻模型j下的状态估计、协方差估计及模型j在模型集Mⁿ(k)中的模型概率；

C-3-2.分别计算旧当前运动模型集

新激活候选群

的群模型概率；当满足

时，候选模型群

在下一时刻终止；当满足

时，当前起作用的模型群

在下一时刻终止；其中，t₁和t₂为模型终止判决阈值，

为新激活候选群

的模型概率和，

为旧当前运动模型集

的模型概率和。

Images