CN106952290B - 一种用于三维空间跟踪转弯机动目标的方法及系统 - Google Patents

Abstract

本发明提供了一种用于三维空间跟踪转弯机动目标的方法及系统，旨在解决现有技术中难以对三维空间中转弯率变化的机动目标进行有效跟踪的问题。本发明所提供的方法或系统通过预测、转弯率和转向角估计、更新、新目标生成及目标状态提取这些模块，将转弯率和转向角估计与序贯贝叶斯滤波器相结合，确定出各目标在当前时刻的状态估计与误差估计，从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。

Description

一种用于三维空间跟踪转弯机动目标的方法及系统

技术领域

本发明属于多传感器信息融合技术领域，尤其涉及一种用于三维空间跟踪转弯机动目标的方法及系统。

背景技术

贝叶斯滤波技术主要用于协助解决测量数据具有不确定性情况下的多传感器信息的融合与处理，它是一种强大的统计方法工具。然而，目前的多目标贝叶斯滤波方法难以对三维空间中转弯率变化的机动目标进行有效跟踪。

发明内容

本发明提供了一种用于三维空间跟踪转弯机动目标的方法及系统，旨在解决现有技术中难以对三维空间中转弯率变化的机动目标进行有效跟踪的问题。

为解决上述技术问题，本发明是这样实现的，本发明提供了一种用于三维空间跟踪转弯机动目标的方法，所述方法包括：

步骤1、根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；

所述步骤1具体包括：

以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1；

步骤2、根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角；

步骤3、根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；

根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；

步骤4、利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角；

所述步骤4具体包括：

利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且j＝1,…,M，ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M；

步骤5、从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

进一步地，所述步骤2具体包括：

步骤A、设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,jz_i,j]^T，转换公式为其中，方位角高低角

步骤B、利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

步骤C、根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

进一步地，所述步骤3具体包括：

步骤D、由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

步骤E、利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。

本发明还提供了一种用于跟踪三维空间转弯机动目标的系统，所述系统包括：

预测模块，用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；

所述预测模块具体用于：

以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1；

转弯率与转向角估计模块，用于根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角；

更新模块，用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；

根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；

新目标生成模块，用于利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角；

所述新目标生成模块具体用于：

利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且j＝1,…,M，ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M；

目标状态提取模块，用于从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

进一步地，所述转弯率与转向角估计模块具体用于：

设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,j z_i,j]^T，转换公式为其中，方位角高低角

利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

进一步地，所述更新模块具体用于：

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。

本发明与现有技术相比，有益效果在于：

本发明提供的用于三维空间跟踪转弯机动目标的方法或系统，通过前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差对当前时刻各目标的参数进行预测，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；通过前一时刻各目标的相关参数等和当前时刻的测量对当前时刻各目标的参数进行估计，得到当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角；根据当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角以及估计得到的当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角等参数，得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；然后利用当前时刻的各测量生成新生目标的参数，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角与当前时刻各已存在目标的更新边缘分布、存在概率、转弯率和转向角分别进行合并，从而得到当前时刻各目标的边缘分布、存在概率、转弯率和转向角；最后通过对当前时刻各目标的边缘分布、存在概率、转弯率和转向角的处理，确定出各个目标在当前时刻的状态估计与误差估计，从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。

附图说明

图1是本发明实施例提供的一种用于跟踪三维空间转弯机动目标的方法流程图；

图2是本发明实施例提供的一种用于跟踪三维空间转弯机动目标的系统示意图；

图3是本发明实施例提供的传感器在70个扫描周期的测量数据示意图；

图4是本发明实施例提供的根据跳变马尔科夫系统模型高斯混合概率假设密度滤波方法处理得到的结果示意图；

图5是本发明实施例提供的利用本发明提供的一种用于三维空间跟踪转弯机动目标方法处理得到的结果示意图；

图6是本发明实施例提供的利用本发明一种用于三维空间跟踪转弯机动目标方法以及跳变马尔科夫系统模型高斯混合概率假设密度滤波方法在经过100次实验得到的平均OSPA距离示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

作为本发明的第一个实施例，如图1所示，本发明提供的一种本发明提供了一种用于三维空间跟踪转弯机动目标的方法，该方法包括以下步骤：

步骤S101：根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角。

所述步骤S101具体包括：

以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1。

步骤S102：根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角。

步骤S103：根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角。

步骤S104：利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角。

步骤S104具体包括：

利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且j＝1,…,M，ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M，具体合并过程如下：

将当前时刻各新生目标的边缘分布与当前时刻各已存在目标的边缘分布进行合并，生成合并后的当前时刻各目标的边缘分布为

将当前时刻各新生目标的存在概率与当前时刻各已存在目标的存在概率进行合并，生成合并后的当前时刻各目标的存在概率为

将当前时刻各新生目标的转弯率与当前时刻各已存在目标的转弯率进行合并，生成合并后的当前时刻各目标的转弯率为

将当前时刻各新生目标的转向角与当前时刻各已存在目标的转向角进行合并，生成合并后的当前时刻各目标的转向角为

步骤S105：从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

步骤S102具体包括以下步骤：

步骤A、设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,jz_i,j]^T，转换公式为其中，方位角高低角

步骤B、利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

步骤C、根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

步骤S103具体包括以下步骤：

步骤D、由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

步骤E、利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。

为了能够产生仿真数据，在本实施例的仿真实验中设定了部分参数的具体数据大小，包括幸存概率p_S,k＝1.0、检测概率p_D,k＝0.95、杂波密度λ_c,k＝1.95×10^-8m^-3、过程噪声的标准差σ_v＝1ms^-2、观测噪声的标准差σ_w＝1m和传感器的扫描周期T＝1s。同时，考虑三维空间[-400m，400m]×[-400m，400m]×[-400m，400m]中运动的目标，目标的状态由位置和速度构成，表示为其中，x、y和z分别表示位置分量，和分别表示速度分量，上标T表示向量的转置；过程噪声方差矩阵为，其中，Δt_k＝t_k-t_k-1为当前时刻与前一时刻的时间差，σ_v为过程噪声标准差；观测噪声方差矩阵σ_w为观测噪声的标准差；参数δ取为δ＝2.5，最大转弯率和最小转弯率分别取为ω_max＝8度/秒和ω_min＝-8度/秒。

综上所述，本发明第一个实施例所提供的用于三维空间跟踪转弯机动目标的方法，通过前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差对当前时刻各目标的参数进行预测，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；通过前一时刻各目标的相关参数等和当前时刻的测量对当前时刻各目标的参数进行估计，估计得到当前时刻的各目标对应于各测量的边缘分布、存在概率、转弯率和转向角；根据当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角以及估计得到的当前时刻的各目标对应于各测量的边缘分布、存在概率、转弯率和转向角等参数，得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；然后利用当前时刻的各测量生成新生目标的参数，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角与当前时刻各已存在目标的更新边缘分布、存在概率、转弯率和转向角分别进行合并，从而得到当前时刻各目标的边缘分布、存在概率、转弯率和转向角；最后通过对当前时刻各目标的边缘分布、存在概率、转弯率和转向角的处理，确定出各个目标在当前时刻的状态估计与误差估计，从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。该方法将转弯率估计与序贯贝叶斯滤波器相结合，在保证数据处理的实时性的同时，有效地解决了三维空间转弯机动目标的跟踪问题，具有很强的实用性。

作为本发明的第二个实施例，如图2所示，本发明还提供了一种用于三维空间跟踪转弯机动目标的系统，该系统包括：

预测模块10，用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角。

转弯率与转向角估计模块20，用于根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角。

更新模块30，用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角。

新目标生成模块40，用于利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角。

目标状态提取模块50，用于从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

上述预测模块10具体用于：以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数，该δ为预先给定的已知常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1。

上述转弯率与转向角估计模块20具体用于：设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,j z_i,j]^T，转换公式为其中，方位角高低角

利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

上述更新模块30具体用于：由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。

上述新目标生成模块40具体用于：利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且j＝1,…,M，ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M。

需要说明的是，在本实施例中，如图3所示，为实验中通过传感器得到的在70个扫描周期的仿真观测数据。为了处理仿真数据，本实施例中对相关参数进行了预设，取幸存概率p_S,k＝0.6、检测概率p_D,k＝0.95、杂波密度λ_c,k＝1.95×10^-8m^-3、观测噪声标准差σ_w＝1m、过程噪声标准差σ_v＝1ms^-2、第一阈值为10^-3、第二阈值为0.5、新生目标的权重w_γ＝0.1，新生目标的存在概率p_γ＝0.1，新生目标的协方差为图4为根据跳变马尔科夫系统模型高斯混合概率假设密度滤波方法对图3中的数据处理得到的结果示意图。图5所示为利用本发明提供的一种用于三维空间跟踪转弯机动目标方法对图4得到的结果进行处理后的示意图。图6所示为利用本发明所提供的方法以及跳变马尔科夫系统模型高斯混合概率假设密度滤波方法在经过100次蒙特卡罗(Monte Carlo)实验得到的，平均最优亚模式分配(Optimal Sub Pattern Assignment，OSPA)距离示意图。通过实验结果进行比较可以得出，本发明所提供的方法或系统可以获得较为精确和可靠的目标状态估计、且其OSPA距离处于合理范围内。

综上所述，本发明第二个实施例提供的用于三维空间跟踪转弯机动目标的系统，通过预测模块、转弯率和转向角估计模块、更新模块、新目标生成模块及目标状态提取模块，将转弯率和转向角估计与序贯贝叶斯滤波器相结合，确定出各个目标在当前时刻的状态估计与误差估计，从而实现了对三维空间中转弯率变化的机动目标进行有效跟踪。在保证数据处理的实时性的同时，有效地解决了三维空间转弯机动目标的跟踪问题，具有很强的实用性。

以上所述仅为本发明的较佳实施例而已，并不用以限制发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种用于三维空间跟踪转弯机动目标的方法，其特征在于，所述方法包括以下步骤：

步骤1、根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；

所述步骤1具体包括：

以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1；

步骤2、根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角；

步骤3、根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；

根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；

步骤4、利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角；

所述步骤4具体包括：

利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M；

步骤5、从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

2.如权利要求1所述的方法，其特征在于，所述步骤2具体包括：

步骤A、设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,jz_i,j]^T，转换公式为其中，方位角高低角

步骤B、利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

步骤C、根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

3.如权利要求2所述的方法，其特征在于，所述步骤3具体包括：

步骤D、由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

步骤E、利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

步骤F、取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。

4.一种用于跟踪三维空间转弯机动目标的系统，其特征在于，所述系统包括：

预测模块，用于根据前一时刻各目标的边缘分布、存在概率、转弯率和转向角，以及当前时刻与前一时刻的时间差，得到当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角；

所述预测模块具体用于：

以k-1表示前一时刻，k表示当前时刻，t_k-1表示前一时刻的时间，t_k表示当前时刻的时间，以N(x；m_i,k-1,P_i,k-1)、ρ_i,k-1、ω_i,k-1和θ_i,k-1分别表示k-1时刻目标i的边缘分布、存在概率、转弯率和转向角；其中，i为索引号，且i＝1,2,…N_k-1，N表示高斯分布，x表示目标的状态向量，m_i,k-1和P_i,k-1分别表示k-1时刻目标i的状态均值和协方差，N_k-1表示k-1时刻目标的总数；

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)、存在概率ρ_i,k-1、转弯率ω_i,k-1和转向角θ_i,k-1，预测得到当前时刻目标i的预测边缘分布和预测存在概率分别为N(x；m_i,k|k-1,P_i,k|k-1)和ρ_i,k|k-1；其中， 和分别表示目标i位置的x分量、y分量和z分量，和分别表示目标i速度的x分量、y分量和z分量；其中，m_i,k|k-1＝F_i,k|k-1m_i,k-1，P_i,k|k-1＝Q_i,k-1+F_i,k|k-1P_i,k-1(F_i,k|k-1)^T，ρ_i,k|k-1＝p_S,k(t_k-t_k-1)ρ_i,k-1，F_i,k|k-1表示状态转移矩阵，且F_i,k|k-1＝A+B·C·D·B^-1·(I-A)；其中， I表示单位矩阵，Δt_k＝t_k-t_k-1表示k时刻与k-1时刻的时间差，方位角高低角Q_i,k-1表示k-1时刻目标i的过程噪声协方差矩阵，p_S,k(t_k-t_k-1)表示目标的幸存概率，且T表示采样周期，δ表示预设的常数；

k时刻目标i的预测转弯率和预测转向角分别为ω_i,k|k-1＝ω_i,k-1和θ_i,k|k-1＝θ_i,k-1；

转弯率与转向角估计模块，用于根据前一时刻各目标的边缘分布和存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，估计当前时刻各目标对应于各测量的转弯率与转向角；

更新模块，用于根据估计得到的当前时刻各目标对应于各测量的转弯率与转向角，前一时刻各目标的边缘分布，当前时刻各目标的预测存在概率，当前时刻与前一时刻的时间差，以及当前时刻的测量集合，得到当前时刻各目标对应于各测量的边缘分布和存在概率；

根据所述当前时刻各目标的预测边缘分布、预测存在概率、预测转弯率和预测转向角，所述当前时刻各目标对应于各测量的边缘分布、存在概率、转弯率和转向角得到当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角；

新目标生成模块，用于利用当前时刻的各测量生成新生目标的边缘分布，为所述新生目标指定存在概率、转弯率和转向角；同时，将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布、存在概率、转弯率和转向角；

所述新目标生成模块具体用于：

利用当前时刻的M个测量生成当前时刻新生目标的边缘分布为当前时刻各新生目标指定存在概率为转弯率为和转向角为其中，表示第j个新生目标的边缘分布的协方差，为第j个新生目标的边缘分布的均值，由当前时刻的第j个测量生成，并且ρ_γ表示为当前时刻各新生目标所指定的存在概率，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；

将当前时刻新生目标的边缘分布、存在概率、转弯率和转向角，分别与所述的当前时刻各已存在目标的边缘分布、存在概率、转弯率和转向角进行合并，生成合并后的当前时刻各目标的边缘分布为存在概率为转弯率为和转向角为其中N_k＝N_k-1+M；

目标状态提取模块，用于从所述合并后的当前时刻各目标中，将存在概率小于第一阈值的目标裁减掉，并且将裁减后余下目标的边缘分布、存在概率、转弯率和转向角作为下一时刻递归滤波的输入；同时，从裁减后余下目标的边缘分布中提取存在概率大于第二阈值的边缘分布作为当前时刻的输出，并且将各个输出边缘分布的均值与方差分别作为当前时刻目标的状态估计与误差估计。

5.如权利要求4所述的系统，其特征在于，所述转弯率与转向角估计模块具体用于：

设测量其中，和分别表示测量y_j,k的x分量、y分量和z分量，上标T表示矩阵或向量的转置；利用m_i,k-1和y_j,k通过转换得到向量[x_i,j y_i,j z_i,j]^T，转换公式为其中，方位角高低角

利用转换后的向量[x_i,j y_i,j z_i,j]^T得到转弯率ω^i,j和转向角θ^i,j，其中，

根据所述转弯率ω^i,j、最大转弯率ω_max和最小转弯率ω_min得到目标i对应于测量y_j,k的转弯率其中，ω_max和ω_min是两个已知的参数；由所述转向角θ^i,j，得到目标i对应于测量y_j,k的转向角

在完成对M个测量的处理后，得到各目标对应于各测量的转弯率为和转向角为其中，i＝1,…,N_k-1，j＝1,…,M。

6.如权利要求5所述的系统，其特征在于，所述更新模块具体用于：

由k-1时刻目标i的边缘分布N(x；m_i,k-1,P_i,k-1)，以及所述的转弯率和转向角得到k时刻目标i对应于测量y_j,k的预测边缘分布为其中，i＝1,…,N_k-1，j＝1,…,M，表示状态向量的均值，且 表示状态向量的方差，且其中，为状态转移矩阵，且其中， I表示单位矩阵，Δt_k＝t_k-t_k-1为k时刻与k-1时刻的时间差，方位角高低角

利用贝叶斯规则对测量y_j,k进行处理，得到目标i对应于测量y_j,k的存在概率滤波增益均值向量协方差矩阵其中，H_k表示观测矩阵，R_k表示观测噪声方差矩阵，p_D,k表示目标的检测概率，λ_c,k表示杂波密度，I表示单位矩阵；

在完成对M个测量的处理后，得到各目标对应于各测量的边缘分布为和存在概率为其中，i＝1,…,N_k-1，j＝1,…,M；

取其中则k时刻已存在目标i的边缘分布取为存在概率为转弯率为和转向角为其中i＝1,…,N_k-1，当q＝M+1时，

处理完后，得到各已存在目标的边缘分布为N(x；m_i,k,P_i,k)、存在概率为ρ_i,k、转弯率为ω_i,k和转向角为θ_i,k，其中，i＝1,…,N_k-1，j＝1,…,M。