[发明专利]一种快速多传感器集势概率假设密度滤波方法

权利要求书

1.一种快速多传感器集势概率假设密度滤波方法，其特征在于，该方法包括以下步骤：

先给出统一的符号定义：(·)_k表示k时刻的值，则(·)₀表示初始值，(·)_k|k-1表示k-1时刻对k时刻的预测值，(·)⁽ⁱ⁾表示与索引号为i的高斯分量有关的物理量，(·)^γ表示与传感器γ有关的物理量，(·)^l表示与目标航迹l有关的物理量；

再给出高斯分量的定义：表示k时刻索引号为i的高斯分量，其权重为均值为协方差为被标签标记；索引号i∈{1,2,...,I_k}，其中I_k表示k时刻高斯分量的数量；标签其中表示k时刻的标签数，被同一个标签标记的高斯分量认为来自同一个目标航迹；

S1，场景中总共有γ个传感器，传感器编号γ∈{1,2,...,γ}；设初始时刻k＝0，初始化势分布ρ₀(n)和概率假设密度D₀(x)，具体为：

初始化势分布为ρ₀(n)，其中n表示目标数为n且n∈{0,1,...,N_max}，N_max为最大可能目标数；ρ₀(n)可以根据情况选择合适分布，这里选择使用二项分布：

其中C表示组合数符号，r为目标出现概率；

初始化概率假设密度D₀(x)的形式为：

S2，已有上一时刻的概率假设密度D_k-1(x)和势分布ρ_k-1(n)，对当前时刻进行预测，得到预测概率假设密度D_k|k-1(x)和预测势分布ρ_k|k-1(n)；包括以下步骤：

上一时刻的概率假设密度D_k-1(x)的形式为：

对当前时刻的势分布进行预测为：

其中ρ_B(·)是新目标的势分布，p_S是目标存活概率，s和t表示两个整数；

对当前时刻的概率假设密度进行预测为：

D_k|k-1(x)＝D_S,k|k-1(x)+D_B,k|k-1(x)

其中D_S,k|k-1(x)表示存活目标的概率假设密度，D_B,k|k-1(x)表示新目标概率假设密度；

D_S,k|k-1(x)由下式给出：

其中是存活目标标签且和分别是存活目标高斯分量的均值和协方差，由下两式分别计算：

其中F和Q分别表示状态转移矩阵和过程噪声协方差矩阵，T表示矩阵转置；

D_B,k|k-1(x)由下式给出：

其中和分别是新目标高斯分量的权重，均值和协方差，I_B,k|k-1是新目标高斯分量数，新目标标签是新目标数；

则预测概率假设密度D_k|k-1(x)为：

其中I_k|k-1＝I_k-1+I_B,k|k-1，标签

S3，获得传感器γ当前时刻的量测集合其中是量测状态，是量测数量，通过各传感器的量测集合，计算各传感器的权重矩阵和代价矩阵；包括以下步骤：

S3.1传感器γ的代价矩阵用表示，矩阵第l行，第j列的元素表示航迹l与量测的关联代价，其中由下式计算：

其中：

表示标记为l的高斯分量索引号的集合；

H是量测矩阵，R是量测噪声协方差矩阵；

表示传感器γ的检测概率；

κ(·)表示杂波的强度函数；

S3.2传感器γ的权重矩阵由表示，其大小与相同，第l行，第j列的元素由下式计算：

S3.3计算量测的更新权重：

设置量测剔除阈值θ_z＝10^-3，如果则认为是杂波，将其所在列从代价矩阵中剔除；剔除后矩阵剩余列；

S3.4计算传感器γ代价矩阵的行列差：

从各传感器代价矩阵的行列差中取最大值为

如果Δ_k＞0，则将Δ_k+1个代价矩阵纵向拼接为新的代价矩阵：

S3.5代价矩阵的关联矩阵用表示，其大小与相同，其中第l行，第j列的元素为a_l,j，a_l,j等于0或1，且每行每列最多只有1个a_l,j等于1；则关联矩阵的代价为其中表示此时的行数，表示中第l行，第j列的元素；

通过寻优算法寻找代价最小的关联矩阵然后求矩阵与的Hadamard积，得到最终的代价矩阵其中符号*表示Hadamard积运算；

S4，利用S3获得的代价矩阵通过航迹对各传感器的量测进行分组，然后用各航迹的量测分组组成全局量测划分，包括以下步骤：

S4.1用每个航迹对各传感器的量测进行分组，具体过程为：

对于航迹l，依次从传感器γ∈{1,2,...,γ}的量测集合中选择一个量测，且只有在传感器γ的代价矩阵中的量测可以被选择，不选择量测则表示传感器γ漏检该目标航迹l；完成后将所有选择的量测组成航迹l的第1个量测分组

S4.2重复S4.1的过程，找到航迹l所有可能的量测分组其中L^l是航迹l的量测分组数；

S4.3对所有的航迹进行S4.1～S4.2的过程，找到所有航迹的量测分组；

S4.4然后用各航迹的量测分组组成全局量测划分，具体过程为：

依次从航迹的量测分组中选择一个分组其中l^l∈{1,2,...,L^l}，将这些量测分组组成一个全局量测划分其中表示来自所有传感器的杂波组成的集合；用表示量测分组中来自传感器γ的量测数量，则ψ中来自传感器γ的量测数量表示为ψ中总的量测数量表示为

S4.5重复该过程，找到所有可能的量测划分，所有量测划分组成集合Ψ；

S5，通过S2获得的预测概率假设密度D_k|k-1(x)和预测势分布ρ_k|k-1(n)，以及S4获得的量测划分集合Ψ进行更新，获得更新的概率假设密度D_k(x)和更新势分布ρ_k(n)；包括以下步骤：

S5.1更新的势分布ρ_k(n)由下式计算：

其中：

是传感器γ的杂波的势分布；

P表示排列数符号，

表示量测分组的似然比，由下式计算：

其中：

c^γ(·)是杂波的空间分布函数；

S5.2索引号为i的高斯分量在量测分组中的归一化伪似然由下式计算：

S5.3更新的概率假设密度D_k(x)为：

D_k(x)＝D_E,k(x)+D_U,k(x)

其中D_E,k(x)和D_U,k(x)分别表示遗留概率假设密度和量测更新概率假设密度，分别为：

其中：

而α₀由下式计算：

表示全局量测划分ψ中量测分组更新的概率假设密度，由下式给出：

其中比例系数由下式计算：

量测分组更新的高斯分量权重由下式计算：

其中α_ψ由下式计算：

是量测更新的高斯分量均值，是更新协方差，分别由下式计算：

则量测更新概率假设密度D_U,k(x)为：

其中新标签由量测分组决定，是航迹l选择的量测分组，则

(5.4)重新整理D_U,k(x)中高斯分量的索引号，可将D_U,k(x)整理为：

其中为所有ψ中量测数量之和；

将D_E,k(x)重新整理为：

其中

最终，所有高斯分量组成当前时刻更新的概率假设密度D_k(x)：

其中I_k＝I_k|k-1+I_k|k-1·|Ψ|；

S6，对高斯分量进行剪枝与合并，估计目标数和目标状态，包括以下步骤：

S6.1设置高斯分量剔除阈值w_P＝10^-5，如果将权重对应的高斯分量删除；

S6.2如果具有相同标签的高斯分量之间的距离小于距离阈值d_M＝4m，将这些高斯分量合并；

S6.3目标数估计就是使更新势分布取最大值的整数，即：

S6.4航迹l的高斯分量权重和为：

将航迹高斯分量权重和按从大到小排序，取前条航迹，然后从每个航迹对应的高斯分量中选择权重最大的高斯分量，其均值就是目标状态的估计；

S7，重复S2～S6，直到不再需要跟踪为止。