CN114488116A - 一种基于两部两坐标外源雷达系统的3d目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于两部两坐标外源雷达系统的3D目标跟踪方法。利用两部两坐标外源雷达对高度近似恒定的目标进行3D跟踪，结合外源雷达自身的特点，提出了一种将目标高度与水平状态进行解耦估计的方法。首先，将目标高度区间划分为多个高度子区间，然后利用量测值递推计算目标在各个高度子区间的权重，最后通过加权融合各个高度子区间的方式估计目标高度；在水平面的状态估计器中，进一步考虑了目标在水平面机动的场景，利用交互式多模型拓展卡尔曼滤波器，并结合目标高度的估计值更新目标的水平状态。本发明方法利用两部两坐标外源雷达实现了对高度近似恒定目标的精确3D跟踪，并消除了2D跟踪情况下因未考虑目标高度而引起的误差。

Description

一种基于两部两坐标外源雷达系统的3D目标跟踪方法

技术领域

本发明属于雷达数据处理领域，具体涉及一种基于两部两坐标外源雷达系统的3D目标跟踪方法。

背景技术

外辐射源雷达是一种利用第三方发射的电磁信号探测目标的双/多基地雷达系统，该体制雷达本身并不发射信号，而是被动地接收目标反射的电磁信号，对目标进行探测。从已有的公开文献来看，多数外源雷达只能提供目标的双基地距离、双基地速度及方位角量测，这些量测不足以估计目标的3D位置。然而，目标3D位置信息的估计对外源雷达性能提升和应用大有裨益：一是获取高度信息后可消除2D跟踪情况下由于未考虑目标高度而引起的偏差，为后续的数据融合等创造条件；二是非合作目标的3D位置信息可用于空中飞行器的交通管制，特别是高度信息将对目标态势评估，实现威胁告警提供帮助。已有外源雷达文献当中关于3D目标跟踪结果方面的报道较少，且大多数根据多站双基距离联合定位目标3D位置，至少需要3个收发对同时检测到目标，这会大幅增加对探测区域重复覆盖的要求。事实上，对于通航或民航飞行器，除了起飞和降落阶段外，其飞行的高度基本保持在某一恒定值，这使得利用一部或两部两坐标雷达对这种高度近似恒定的目标进行3D跟踪变得可行。

针对高度保持恒定且在水平面匀速直线运动的目标，目前常用的方法之一是高度参数化拓展卡尔曼滤波(height-parametrized extended Kalman filter,HP-EKF)方法，利用单部两坐标雷达进行3D目标跟踪。另一种方法是利用两部两坐标空中交通管制雷达对目标进行3D跟踪。该方法利用前N场量测值，结合目标直线运动的条件来估计目标3D位置的最大似然估计，利用该估计值和启发式协方差初始化滤波器。滤波器初始化成功后，其后续量测仍利用HP-EKF处理。

以上利用单部或两部两坐标雷达来对目标进行3D跟踪的算法均是针对单基地结构的雷达设计，且均假定目标在水平面做匀速(或近似匀速)直线运动。因此，在设置各高度子区间滤波器的3D跟踪初始状态时一般利用坐标转换的方式，或利用目标在水平面做匀速直线运动的假设，通过前N场量测估计目标的3D初始状态。然而，外源雷达由于其特殊的双基地几何结构，其量测方程具有更强的非线性，转化误差不能被忽略。除此之外，实际情况中，目标在水平面可能存在机动，这使得上述两种滤波器的初始化方式不能直接用于实际的外源雷达系统。不仅如此，目标在水平面做匀速直线运动的假设前提极大的限制了已有算法的适用范围，难以应对复杂多变的实际情况。

针对上述问题，结合外源雷达自身特性，本发明提出了一种两部两坐标外源雷达3D目标跟踪方法。与上述已有算法仅考虑目标在水平面做匀速直线运动不同的是，本文进一步考虑了目标在水平面机动的情景。在估计目标3D状态时，本发明提出了两个并行的估计器分别估计目标高度和目标在水平面的状态：首先，高度估计器对目标高度状态进行更新，然后估计结果被送至另一估计器，参与目标在水平面的状态估计；在估计目标水平面的状态时，为了应对量测非线性及目标机动问题，采用了交互式多模型拓展卡尔曼滤波(Interacting Multiple Model-EKF,IMM-EKF)的方式更新目标状态。在本发明中，由于采用了单独的估计器来估计目标高度，从而避免了跟踪过程中目标动态方程失配对高度估计的影响。

发明内容

针对外源雷达3D目标跟踪问题，本发明提出了一种基于两部两坐标外源雷达系统的3D目标跟踪方法。

本发明两部两坐标外源雷达系统采用的技术方案如下：

包括：第一部外源雷达、第二部外源雷达、上位机；

所述第一部外源雷达由第一部外源雷达的接收站、第一部外源雷达的发射站构成；

所述第二部外源雷达由第二部外源雷达的接收站、第二部外源雷达的发射站构成；

所述上位机分别与所述的第一部外源雷达、第二部外源雷达依次连接；

所述第一部外源雷达在实时采集目标回波信号后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第一部外源雷达实时得到的目标双基地距离、第一部外源雷达实时得到的目标方位角、第一部外源雷达实时得到的目标双基地速度；

所述第二部外源雷达在实时采集目标回波后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第二部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标双基地速度；

所述上位机通过所述3D目标跟踪方法实现3D目标跟踪。

本发明3D目标跟踪方法采用的技术方案如下：

对于飞行高度几乎恒定的目标，对其高度与水平位置、速度进行解耦估计，结合外源雷达双基地几何结构的特点，通过两个并行估计器来实现3D目标跟踪，包含以下步骤：

步骤1：上位机对于需要探测的目标，定义目标所在的高度区间已知，将目标所在的高度区间平均划分为多个高度子区间，计算每个高度子区间的宽度，计算每个高度子区间的平均高度；

步骤2：上位机根据第一部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标双基地距离求出目标在各高度子区间的实时平面位置坐标，根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量，根据部外源雷达实时目标方位角量测向量以及目标在各高度子区间的实时平面位置坐标构造各高度子空间实时的代价函数，根据各高度子空间实时的代价函数递推计算目标在各高度子区间的实时权重，根据实时目标在各高度子区间的权重对各高度子区间的平均高度进行加权融合，得到目标实时的高度估计值和目标实时的高度方差；

步骤3：根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化；根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互；将上一时刻各个运动目标模型的输出值的交互结果输入至本时刻IMM-EKF估计器，经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值；将上一时刻各个运动目标模型的输出值的交互结果作为本时刻IMM-EKF估计器的输入，经过IMM-EKF估计器处理后，得到本时刻各滤波器状态的更新值；根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率；根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合，得到本时刻目标水平面的状态估计及本时刻目标水平面的的协方差矩阵；

作为优选，步骤1中所述目标所在的高度区间记为：

[h_min,h_max]

其中，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值；

步骤1所述每个高度子区间的宽度为：

Δh＝(h_min-h_max)/N_a

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值，Δh表示每个高度子区间的宽度；

步骤1所述每个高度子区间的平均高度为：

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，

表示第j个子区间的平均高度，Δh表示每个高度子区间的宽度；

作为优选，步骤2所述求出目标在各高度子区间的实时平面位置坐标为：

将第一部外源雷达的接收站位置坐标记为s_r,1＝[x_r,1 y_r,1 z_r,1]^T、第一部外源雷达的发射站位置坐标记为s_t,1＝[x_t,1 y_t,1 z_t,1]^T，其中上标“T”表示转置运算；

将第二部外源雷达的接收站位置坐标记为s_r,2＝[x_r,2 y_r,2 z_r,2]^T、第二部外源雷达的发射站位置坐标记为s_t,2＝[x_t,2 y_t,2 z_t,2]^T；

所述的s_r,1＝[x_r,1 y_r,1 z_r,1]^T、s_t,1＝[x_t,1 y_t,1 z_t,1]^T、s_r,2＝[x_r,2 y_r,2 z_r,2]^T、s_t,2＝[x_t,2 y_t,2 z_t,2]^T均为已知量；

步骤2所述第一部外源雷达k时刻实时采集的目标双基地距离r_b,1(k)与k时刻的目标真实位置q(k)之间的关系为：

r_b,1(k)＝||q(k)-s_t,1||+||q(k)-s_r,1||+σ_r1

其中，||·||表示二阶范数，s_t,1为第一部雷达发射站位置坐标，s_r,1为第一部雷达发射站位置坐标，σ_r1为第一部外源雷达双基地距离量测噪声，r_b,1(k)为第一部外源雷达k时刻实时采集的目标双基地距离，q(k)＝[x(k)y(k)h(k)]^T为k时刻的目标真实位置；

步骤2所述第二部外源雷达k时实时得到的目标双基地距离r_b,2(k)与目标真实位置q(k)之间的关系：

r_b,2(k)＝||q(k)-s_t,2||+||q(k)-s_r,2||+σ_r2

其中，s_t,2为第二部雷达发射站位置坐标，s_r,2为第二部雷达发射站位置坐标，σ_r2为第二部外源雷达双基地距离量测噪声，r_b,2(k)为第二部雷达k时刻实时采集的目标双基地距离，q(k)为k时刻的目标真实位置；

表示第j个高度子区间的平均高度，N_a表示目标高度区间的数量；

将k时刻目标在第j个高度子区间的高度设为该子区间的平均高度，即令

将

分别代入双基地距离r_b,1(k)与双基地距离r_b,2(k)的表达式中，然后联立双基地距离r_b,1(k)的表达式与双基地距离r_b,2(k)的表达式，利用最大似然法估计目标k时刻在第j个高度子区间的位置坐标

和目标k时刻在第j个高度子区间对应的估计协方差C_j(k)；

步骤2所述根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量：

其中，ψ_b,1(k)为第一部外源雷达在k时刻实时得到的目标方位角，ψ_b,2(k)为第二部外源雷达在k时刻实时得到的目标方位角；

步骤2所述两部外源雷达实时目标方位角量测向量z_ψ(k)与待求解的k时刻的目标真实位置q(k)＝[x(k) y(k) h(k)]^T之间的关系为：

其中，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，σ_ψ1为第一部外源雷达方位角量测噪声，σ_ψ2为第二部外源雷达方位角量测噪声；

步骤2所述构造各高度子空间实时的代价函数为：

Dist(k,j)＝u_ψ,j(k)^TS_ψ,j(k)^-1u_ψ,j(k)

其中，u_ψ,j(k)为k时刻第j个高度子区间方位角新息，S_ψ,j(k)为k时刻第j个高度子区间方位角新息协方差矩阵，u_ψ,j(k)按下式计算

其中，z_ψ(k)为两部外源雷达k时刻的目标方位量测向量，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，

为目标k时刻在第j个高度子区间的X方向位置坐标估计值,

为目标k时刻在第j个高度子区间的Y方向位置坐标估计值；

所述S_ψ,j(k)按下式计算：

S_ψ,j(k)＝H_ψ(k)C_j(k)H_ψ(k)^T+R_ψ(k)

其中，H_ψ(k)表示k时刻两部外源雷达方位角量测向量z_ψ(k)对应的雅克比矩阵，C_j(k)为目标在k时刻的平面位置估计值对应的协方差矩阵，R_ψ(k)表示k时刻方位角量测向量z_ψ(k)的量测噪声协方差矩阵；

步骤2所述根据各高度子空间实时的代价函数递推计算实时目标在各高度子区间的权重，具体为：

其中，w_j(k)k时刻目标在第j个高度子区间的权重，p(z(k)|j)表示k时刻目标在第j个高度子区间的概率，N_a表示目标高度区间的数量；

在高斯条件下，p(z(k)|j)的表达式如下

其中，S_ψ,j(k)为k时刻方位角新息协方差矩阵，Dist(k,j)为k时刻第j个高度子空间代价函数；

步骤2所述目标k时刻的高度估计值为：

其中，w_j(k)为k时刻目标在第j个高度子区间的权重，

为第j高度子区间的平均高度，N_a表示目标高度区间的数量；

步骤2所述目标k时刻的高度方差为：

其中，w_j(k)为目标k时刻在第j个高度子区间的权重，

为第j高度子区间的平均高度，

为k时刻目标高度的估计值，N_a表示目标高度区间的数量；

作为优选，步骤3所述根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化，具体为：

将步骤2中目标高度初始时刻的估计值

代入初始时刻第一部外源雷达的双基地距离r_b,1(1)与双基地距离r_b,2(1)的表达式中，然后联立双基地距离r_b,1(1)的表达式与双基地距离r_b,2(1)的表达式，利用最大似然法估计目标初始时刻的平面位置坐标

和目标初始时刻平面位置的估计协方差C_x(1)，然后对IMM-EFK内的所有N个滤波器的初始状态和对应的协方差进行相同的初始化操作，该初始状态为：

初始协方差矩阵为

其中，v_x,max为目标X方向速度的最大值，v_y,max为目标Y方向速度的最大值，且v_x,max与v_y,max均为已知量，初始时刻模型m转移到模型n的混合转移概率设为u^mn(1)，为已知量；

步骤3所述根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互为：

式中：

其中，

表示k-1时刻滤波器m的目标状态输出值、P^m(k-1|k-1)表示示k-1时刻滤波器m的状态输出值相应的协方差矩阵，

表示示k-1时刻模型n的状态交互值，P^on(k-1|k-1)表示示k-1时刻模型n的协方差交互值，u^mn(k-1)表示示k-1时刻模型m转移到模型n的混合转移概率，π^mn表示模型m转移到模型n的先验转移概率，将示k-1时刻各个运动目标模型的输出值进行交互，作为k时刻的输入；

步骤3所述经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值为：

将第一部外源雷达实时得到的k时刻目标双基地距离、第一部外源雷达实时得到的k时刻目标方位角、第一部外源雷达实时得到的k时刻目标双基地速度与第二部外源雷达实时得到的k时刻目标双基地距离、第二部外源雷达实时得到的k时刻目标方位角、第二部外源雷达实时得到的k时刻目标双基地速度组成量测向量z(k)，将目标在k时刻X方向的真实位置x(k)，Y方向的真实位置y(k)，X方向的真实速度x(k)，Y方向的真实速度y(k)，记为向量：

x(k)＝[x(k) y(k) v_x(k) v_y(k)]^T；

状态更新过程如下具体处理过程如下：

P^m(k|k-1)＝F^m(k-1)P^om(k-1|k-1)(F^m(k-1))^T+Q^m(k-1)

K^m(k)＝P^m(k|k-1)H^m(k)^T[H^m(k)P^m(k|k-1)H^m(k)^T+R(k)]

P^m(k|k)＝P^m(k|k-1)-K^m(k)H^m(k)P^m(k|k-1)

至此，得到k时刻目标在第m个滤波器的状态估计值

和相应的协方差矩阵P^m(k|k)，状态更新过程中，F^m(k-1)表示k时刻在第m个滤波器内目标状态转移矩阵，Q^m(k-1)表示k时刻在第m个滤波器内目标的运动噪声协方差矩阵，

表示上一时刻模型m的状态交互值，P^om(k-1|k-1)表示上一时刻模型m的协方差交互值,R(k)为k时刻量测向量z(k)对应的量测噪声协方差矩阵，H^m(k)为量测方程k时刻在第m个滤波器内对应的雅克比矩阵。

步骤3所述根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率为：

对于第m个模型，其概率按下式进行计算：

式中，Λ^m(k)为k时刻第m个滤波器新息的似然函数，C^m(k-1)为步骤3.1中计算得到的中间结果；

步骤3所述根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合为：

其中，

表示k时刻在第m个滤波器的状态估计值，P^m(k|k)表示k时刻在第m个滤波器的状态估计值对应的协方差矩阵。

本发明所提出的两部两坐标外源雷达的3D目标跟踪方法的优势在于：结合了外源雷达本身的双基地几何特点，利用两部两坐标外源雷达实现了对高度近似恒定目标的精确3D跟踪，并消除了2D跟踪情况下因未考虑目标高度而引起的误差。此外，所提方法进一步考虑了目标在水平面机动的情况，无需先验已知目标的运动模型，因此能更好地应对复杂的实际情况。

附图说明

图1：为本发明实施原理流程图；

图2：为本发明实施例的目标跟踪场景；

图3：为实施例中采用不同方法跟踪所得的目标水平航迹；

图4：为实施例中采用不同方法跟踪所得的目标高度RMSE；

图5：为实施例中采用不同方法跟踪所得的目标X方向位置RMSE；

图6：为实施例中采用不同方法跟踪所得的目标Y方向位置RMSE；

图7：为实施例中采用不同方法跟踪所得的目标X方向速度RMSE；

图8：为实施例中采用不同方法跟踪所得的目标Y方向速度RMSE；

具体实施方式

为了便于本领域的普通技术人员理解和实施本发明，下面结合附图及具体实施例对本发明做进一步说明，应当理解，此处所描述的实施示例仅用于解释和说明本发明，并不用于限定本发明。

下面结合图1至图8介绍本发明的具体实施方式为一种基于双外源雷达系统的3D目标跟踪方法。

本发明双外源雷达系统采用的技术方案如下：

包括：第一部外源雷达、第二部外源雷达、上位机；

所述第一部外源雷达由第一部外源雷达的接收站、第一部外源雷达的发射站构成；

所述第二部外源雷达由第二部外源雷达的接收站、第二部外源雷达的发射站构成；

所述上位机分别与所述的第一部外源雷达、第二部外源雷达依次连接；

所述第一部外源雷达在实时采集目标回波信号后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第一部外源雷达实时得到的目标双基地距离、第一部外源雷达实时得到的目标方位角、第一部外源雷达实时得到的目标双基地速度；

所述第二部外源雷达在实时采集目标回波后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第二部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标双基地速度；

所述上位机通过所述3D目标跟踪方法实现3D目标跟踪。

仿真场景如图2所示，采用两发一收的外源雷达对探测区域进行探测，接收站Rx同时接收来自发射站Tx1、Tx2发送的数字电视广播信号及目标反射回波。以接收站Rx为坐标原点，两发射站的坐标分别为Tx1＝[5850,-8250,10]m、Tx2＝[-11550,130,92]m。假设量测噪声为零均值高斯白噪声，双基距离、双基速度、方位角的量测噪声标准差分别为

σ_α＝1.5°，系统的数据更新周期为T＝1s。需要注意的是，在仿真实验结果图中，为了使图中内容表达清晰，画图间隔为5s。

仿真中，考虑单目标的场景，目标初始位置坐标为

x₀＝[4km,3km,2.5km,30m/s,20m/s,0]^T

数据长度为200s，蒙特卡洛仿真次数为100次，目标按DCWNA模型运动。由于假定目标高度近似恒定，因此在所有时间内，高度方向的噪声功率谱密度取一个极小值，设为q_z＝0.0001m²/s³。目标在水平面机动，不同时间段水平面的噪声功率谱密度为：0～50s，q_x＝q_y＝0.1m²/s³；50～100s，q_x＝q_y＝5m²/s³；100～150s，q_x＝q_y＝8m²/s³；150～200s，q_x＝q_y＝0.1m²/s³。所提算法在进行IMM-EKF滤波时，所使用的机动模型集同样由DCWNA模型产生，模型集中共有5中不同噪声量级的运动模型，其噪声功率谱密度设置如下

q_x＝q_y＝[0.01 0.1 1 5 10]^T(m²/s³)

由于HP-EKF仅能处理匀速直线运动的目标，因此其运动模型中噪声功率谱密度设置为q_x＝q_y＝0.1m²/s³，q_z＝0.0001m²/s³。在所提算法与HP-EKF算法中，目标飞行高度区间为0～3000m，将该区间划分为100个子区间，即N_a＝100。另一种用于对比的处理方式是忽略目标高度，即令目标高度h＝0，通过EKF滤波对目标进行2D跟踪。

本发明3D目标跟踪方法采用的技术方案如下：

步骤1：上位机对于需要探测的目标，定义目标所在的高度区间已知，将目标所在的高度区间平均划分为多个高度子区间，计算每个高度子区间的宽度，计算每个高度子区间的平均高度；

作为优选，步骤1中所述目标所在的高度区间记为：

[h_min,h_max]

其中，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值；

步骤1所述每个高度子区间的宽度为：

Δh＝(h_min-h_max)/N_a

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值，Δh表示每个高度子区间的宽度；

步骤1所述每个高度子区间的平均高度为：

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，

表示第j个子区间的平均高度，Δh表示每个高度子区间的宽度；

步骤2：上位机根据第一部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标双基地距离求出目标在各高度子区间的实时平面位置坐标，根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量，根据部外源雷达实时目标方位角量测向量以及目标在各高度子区间的实时平面位置坐标构造各高度子空间实时的代价函数，根据各高度子空间实时的代价函数递推计算目标在各高度子区间的实时权重，根据实时目标在各高度子区间的权重对各高度子区间的平均高度进行加权融合，得到目标实时的高度估计值和目标实时的高度方差；

步骤2所述求出目标在各高度子区间的实时平面位置坐标为：

将第一部外源雷达的接收站位置坐标记为s_r,1＝[x_r,1 y_r,1 z_r,1]^T、第一部外源雷达的发射站位置坐标记为s_t,1＝[x_t,1 y_t,1 z_t,1]^T，其中上标“T”表示转置运算；

将第二部外源雷达的接收站位置坐标记为s_r,2＝[x_r,2 y_r,2 z_r,2]^T、第二部外源雷达的发射站位置坐标记为s_t,2＝[x_t,2 y_t,2 z_t,2]^T；

所述的s_r,1＝[x_r,1 y_r,1 z_r,1]^T、s_t,1＝[x_t,1 y_t,1 z_t,1]^T、s_r,2＝[x_r,2 y_r,2 z_r,2]^T、s_t,2＝[x_t,2 y_t,2 z_t,2]^T均为已知量；

步骤2所述第一部外源雷达k时刻实时采集的目标双基地距离r_b,1(k)与k时刻的目标真实位置q(k)之间的关系为：

r_b,1(k)＝||q(k)-s_t,1||+||q(k)-s_r,1||+σ_r1

其中，||·||表示二阶范数，s_t,1为第一部雷达发射站位置坐标，s_r,1为第一部雷达发射站位置坐标，σ_r1为第一部外源雷达双基地距离量测噪声，r_b,1(k)为第一部外源雷达k时刻实时采集的目标双基地距离，q(k)＝[x(k) y(k) h(k)]^T为k时刻的目标真实位置；

步骤2所述第二部外源雷达k时实时得到的目标双基地距离r_b,2(k)与目标真实位置q(k)之间的关系：

r_b,2(k)＝||q(k)-s_t,2||+||q(k)-s_r,2||+σ_r2

其中，s_t,2为第二部雷达发射站位置坐标，s_r,2为第二部雷达发射站位置坐标，σ_r2为第二部外源雷达双基地距离量测噪声，r_b,2(k)为第二部雷达k时刻实时采集的目标双基地距离，q(k)为k时刻的目标真实位置；

表示第j个高度子区间的平均高度，N_a表示目标高度区间的数量；

将k时刻目标在第j个高度子区间的高度设为该子区间的平均高度，即令

将

分别代入双基地距离r_b,1(k)与双基地距离r_b,2(k)的表达式中，然后联立双基地距离r_b,1(k)的表达式与双基地距离r_b,2(k)的表达式，利用最大似然法估计目标k时刻在第j个高度子区间的位置坐标

和目标k时刻在第j个高度子区间对应的估计协方差C_j(k)；

步骤2所述根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量：

其中，ψ_b,1(k)为第一部外源雷达在k时刻实时得到的目标方位角，ψ_b,2(k)为第二部外源雷达在k时刻实时得到的目标方位角；

步骤2所述两部外源雷达实时目标方位角量测向量z_ψ(k)与待求解的k时刻的目标真实位置q(k)＝[x(k) y(k) h(k)]^T之间的关系为：

其中，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，σ_ψ1为第一部外源雷达方位角量测噪声，σ_ψ2为第二部外源雷达方位角量测噪声；

步骤2所述构造各高度子空间实时的代价函数为：

Dist(k,j)＝u_ψ,j(k)^TS_ψ,j(k)^-1u_ψ,j(k)

其中，u_ψ,j(k)为k时刻第j个高度子区间方位角新息，S_ψ,j(k)为k时刻第j个高度子区间方位角新息协方差矩阵，u_ψ,j(k)按下式计算

其中，z_ψ(k)为两部外源雷达k时刻的目标方位量测向量，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，

为目标k时刻在第j个高度子区间的X方向位置坐标估计值,

为目标k时刻在第j个高度子区间的Y方向位置坐标估计值；

所述S_ψ,j(k)按下式计算：

S_ψ,j(k)＝H_ψ(k)C_j(k)H_ψ(k)^T+R_ψ(k)

其中，H_ψ(k)表示k时刻两部外源雷达方位角量测向量z_ψ(k)对应的雅克比矩阵，C_j(k)为目标在k时刻的平面位置估计值对应的协方差矩阵，R_ψ(k)表示k时刻方位角量测向量z_ψ(k)的量测噪声协方差矩阵；

步骤2所述根据各高度子空间实时的代价函数递推计算实时目标在各高度子区间的权重，具体为：

其中，w_j(k)k时刻目标在第j个高度子区间的权重，p(z(k)|j)表示k时刻目标在第j个高度子区间的概率，N_a表示目标高度区间的数量；

在高斯条件下，p(z(k)|j)的表达式如下

其中，S_ψ,j(k)为k时刻方位角新息协方差矩阵，Dist(k,j)为k时刻第j个高度子空间代价函数；

步骤2所述目标k时刻的高度估计值为：

其中，w_j(k)为k时刻目标在第j个高度子区间的权重，

为第j高度子区间的平均高度，N_a表示目标高度区间的数量；

步骤2所述目标k时刻的高度方差为：

其中，w_j(k)为目标k时刻在第j个高度子区间的权重，

为第j高度子区间的平均高度，

为k时刻目标高度的估计值，N_a表示目标高度区间的数量；

步骤3：根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化；根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互；将上一时刻各个运动目标模型的输出值的交互结果输入至本时刻IMM-EKF估计器，经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值；将上一时刻各个运动目标模型的输出值的交互结果作为本时刻IMM-EKF估计器的输入，经过IMM-EKF估计器处理后，得到本时刻各滤波器状态的更新值；根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率；根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合，得到本时刻目标水平面的状态估计及本时刻目标水平面的的协方差矩阵；

步骤3所述根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化，具体为：

将步骤2中目标高度初始时刻的估计值

代入初始时刻第一部外源雷达的双基地距离r_b,1(1)与双基地距离r_b,2(1)的表达式中，然后联立双基地距离r_b,1(1)的表达式与双基地距离r_b,2(1)的表达式，利用最大似然法估计目标初始时刻的平面位置坐标

和目标初始时刻平面位置的估计协方差C_x(1)，然后对IMM-EFK内的所有N个滤波器的初始状态和对应的协方差进行相同的初始化操作，该初始状态为：

初始协方差矩阵为：

其中，v_x,max为目标X方向速度的最大值，v_y,max为目标Y方向速度的最大值，且v_x,max与v_y,max均为已知量，初始时刻模型m转移到模型n的混合转移概率设为u^mn(1)，为已知量；

步骤3所述根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互为：

式中

其中，

表示k-1时刻滤波器m的目标状态输出值、P^m(k-1|k-1)表示示k-1时刻滤波器m的状态输出值相应的协方差矩阵，

表示示k-1时刻模型n的状态交互值，P^on(k-1|k-1)表示示k-1时刻模型n的协方差交互值，u^mn(k-1)表示示k-1时刻模型m转移到模型n的混合转移概率，π^mn表示模型m转移到模型n的先验转移概率，将示k-1时刻各个运动目标模型的输出值进行交互，作为k时刻的输入；

步骤3所述经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值为：

将第一部外源雷达实时得到的k时刻目标双基地距离、第一部外源雷达实时得到的k时刻目标方位角、第一部外源雷达实时得到的k时刻目标双基地速度与第二部外源雷达实时得到的k时刻目标双基地距离、第二部外源雷达实时得到的k时刻目标方位角、第二部外源雷达实时得到的k时刻目标双基地速度组成量测向量z(k)，将目标在k时刻X方向的真实位置x(k)，Y方向的真实位置y(k)，X方向的真实速度x(k)，Y方向的真实速度y(k)，记为向量：

x(k)＝[x(k) y(k) v_x(k) v_y(k)]^T；

状态更新过程如下具体处理过程如下：

P^m(k|k-1)＝F^m(k-1)P^om(k-1|k-1)(F^m(k-1))^T+Q^m(k-1)

K^m(k)＝P^m(k|k-1)H^m(k)^T[H^m(k)P^m(k|k-1)H^m(k)^T+R(k)]

P^m(k|k)＝P^m(k|k-1)-K^m(k)H^m(k)P^m(k|k-1)

至此，得到k时刻目标在第m个滤波器的状态估计值

和相应的协方差矩阵P^m(k|k)，状态更新过程中，F^m(k-1)表示k时刻在第m个滤波器内目标状态转移矩阵，Q^m(k-1)表示k时刻在第m个滤波器内目标的运动噪声协方差矩阵，

表示上一时刻模型m的状态交互值，P^om(k-1|k-1)表示上一时刻模型m的协方差交互值,R(k)为k时刻量测向量z(k)对应的量测噪声协方差矩阵，H^m(k)为量测方程k时刻在第m个滤波器内对应的雅克比矩阵。

步骤3所述根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率为：

对于第m个模型，其概率按下式进行计算：

式中，Λ^m(k)为k时刻第m个滤波器新息的似然函数，C^m(k-1)为步骤3.1中计算得到的中间结果；

步骤3所述根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合为：

其中，

表示k时刻在第m个滤波器的状态估计值，P^m(k|k)表示k时刻在第m个滤波器的状态估计值对应的协方差矩阵。

图3给出了某次蒙特卡洛仿真中，目标在水平面的航迹跟踪结果对比。图4给出了本发明所提方法的高度估计均方根误差(root mean square error,RMSE)与HP-EKF算法的高度估计RMSE以及高度估计的后验克拉美罗下界(posterior CRLB,PCRLB)随时间变化的对比图。由图可知，当目标整体距接收站较近时，由于目标高度对目标水平面位置的估计影响较大，直接忽略目标高度会使2D目标跟踪结果与目标真实轨迹之间产生较大的偏差。而HP-EKF方法开始时可以很好的跟踪目标，这是由于前50s内，其运动模型与目标实际运动模型适配，高度估计能收敛到真实值附近；50s后，HP-EKF方法的运动模型失配使得各个高度子区间的权值偏离实际情况，导致高度估计收敛到错误的高度，同时，水平面的状态估计也与目标真实轨迹偏离。相比之下，本发明所提方法的3D跟踪结果不受目标在水平面机动的影响。在15km范围内，本发明所提方法估计的目标高度RMSE约为200m；其水平面的跟踪结果始终与目标真实轨迹吻合，很好地消除了目标高度对其水平状态估计的影响。

图5、图6、图7、图8分别给出了本发明所提方法、HP-EKF算法、2D跟踪(h＝0)在X方向、Y方向的位置估计RMSE和速度估计RMSE及其对应的估计精度下界。由图5、图6可知，相较于本发明所提方法，2D跟踪(h＝0)的位置估计在X方向造成了约150～250m左右的偏差，在Y方向造成了约200～400m左右的偏差。HP-EKF算法在前50s与目标运动模型匹配，能够很好的跟踪目标，50s后其运动模型失配，导致计算各子区间高度的权值时出现混乱，其高度收敛到错误值，导致其水平面的估计精度同步下降；虽然最后50s HP-EKF算法的运动模型再次适配，但已无法及时纠正目标的状态估计，这说明HP-EKF算法无法很好的应对目标在水平面机动的情况。而本发明所提方法的性能并未受到目标机动性的影响，位置估计RMSE始终保持收敛，且接近PCRLB；同样，图7、图8中，本发明所提方法的速度估计精度几乎与速度估计的PCRLB重合，几乎不受目标机动的影响，这说明本发明所提方法能够很好的应对目标在水平面机动的场景。

此外，在同样的计算配置环境下，本发明所提方法单次蒙特卡洛仿真的平均耗时约为0.6143s，略低于HP-EKF算法的平均耗时0.6502s。这说明本发明所提方法在不增加计算量的前提下，可以很好的跟踪在水平面机动的目标，应对复杂的实际情况。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (4)

1.一种基于两部两坐标外源雷达系统的3D目标跟踪方法，其特征在于，所述两部两坐标外源雷达系统包括：第一部外源雷达、第二部外源雷达、上位机；

所述第一部外源雷达由第一部外源雷达的接收站、第一部外源雷达的发射站构成；

所述第二部外源雷达由第二部外源雷达的接收站、第二部外源雷达的发射站构成；

所述上位机分别与所述的第一部外源雷达、第二部外源雷达依次连接；

所述第一部外源雷达在实时采集目标回波信号后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第一部外源雷达实时得到的目标双基地距离、第一部外源雷达实时得到的目标方位角、第一部外源雷达实时得到的目标双基地速度；

所述第二部外源雷达在实时采集目标回波后传输至上位机，经过上位机对目标回波信号进行参考信号重构，杂波抑制，二维互相关，目标检测，角度估计处理后，得到第二部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标双基地速度；

所述上位机通过所述3D目标跟踪方法实现3D目标跟踪；

所述3D目标跟踪方法，包括以下步骤：

步骤1：上位机对于需要探测的目标，定义目标所在的高度区间已知，将目标所在的高度区间平均划分为多个高度子区间，计算每个高度子区间的宽度，计算每个高度子区间的平均高度；

步骤2：上位机根据第一部外源雷达实时得到的目标双基地距离、第二部外源雷达实时得到的目标双基地距离求出目标在各高度子区间的实时平面位置坐标，根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量，根据部外源雷达实时目标方位角量测向量以及目标在各高度子区间的实时平面位置坐标构造各高度子空间实时的代价函数，根据各高度子空间实时的代价函数递推计算目标在各高度子区间的实时权重，根据实时目标在各高度子区间的权重对各高度子区间的平均高度进行加权融合，得到目标实时的高度估计值和目标实时的高度方差；

步骤3：根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化；根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互；将上一时刻各个运动目标模型的输出值的交互结果输入至本时刻IMM-EKF估计器，经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值；将上一时刻各个运动目标模型的输出值的交互结果作为本时刻IMM-EKF估计器的输入，经过IMM-EKF估计器处理后，得到本时刻各滤波器状态的更新值；根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率；根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合，得到本时刻目标水平面的状态估计及本时刻目标水平面的的协方差矩阵。

2.根据权利要求1所述的基于两部两坐标外源雷达系统的3D目标跟踪方法，其特征在于，步骤1中所述目标所在的高度区间记为：

[h_min,h_max]

其中，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值；

步骤1所述每个高度子区间的宽度为：

Δh＝(h_min-h_max)/N_a

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，h_max表示目标高度区间的最大值，Δh表示每个高度子区间的宽度；

步骤1所述每个高度子区间的平均高度为：

其中，N_a表示目标高度区间的数量，h_min表示目标高度区间的最小值，

表示第j个子区间的平均高度，Δh表示每个高度子区间的宽度。

3.根据权利要求1所述的基于两部两坐标外源雷达系统的3D目标跟踪方法，其特征在于，步骤2所述求出目标在各高度子区间的实时平面位置坐标为：

将第一部外源雷达的接收站位置坐标记为s_r,1＝[x_r,1 y_r,1 z_r,1]^T、第一部外源雷达的发射站位置坐标记为s_t,1＝[x_t,1 y_t,1 z_t,1]^T，其中上标“T”表示转置运算；

将第二部外源雷达的接收站位置坐标记为s_r,2＝[x_r,2 y_r,2 z_r,2]^T、第二部外源雷达的发射站位置坐标记为s_t,2＝[x_t,2 y_t,2 z_t,2]^T；

所述的s_r,1＝[x_r,1 y_r,1 z_r,1]^T、s_t,1＝[x_t,1 y_t,1 z_t,1]^T、s_r,2＝[x_r,2 y_r,2 z_r,2]^T、

s_t,2＝[x_t,2 y_t,2 z_t,2]^T均为已知量；

步骤2所述第一部外源雷达k时刻实时采集的目标双基地距离r_b,1(k)与k时刻的目标真实位置q(k)之间的关系为：

r_b,1(k)＝||q(k)-s_t,1||+||q(k)-s_r,1||+σ_r1

其中，||·||表示二阶范数，s_t,1为第一部雷达发射站位置坐标，s_r,1为第一部雷达发射站位置坐标，σ_r1为第一部外源雷达双基地距离量测噪声，r_b,1(k)为第一部外源雷达k时刻实时采集的目标双基地距离，q(k)＝[x(k) y(k) h(k)]^T为k时刻的目标真实位置；

步骤2所述第二部外源雷达k时实时得到的目标双基地距离r_b,2(k)与目标真实位置q(k)之间的关系：

r_b,2(k)＝||q(k)-s_t,2||+||q(k)-s_r,2||+σ_r2

其中，s_t,2为第二部雷达发射站位置坐标，s_r,2为第二部雷达发射站位置坐标，σ_r2为第二部外源雷达双基地距离量测噪声，r_b,2(k)为第二部雷达k时刻实时采集的目标双基地距离，q(k)为k时刻的目标真实位置；

j＝1,2,…,N_a表示第j个高度子区间的平均高度，N_a表示目标高度区间的数量；

将k时刻目标在第j个高度子区间的高度设为该子区间的平均高度，即令

将

分别代入双基地距离r_b,1(k)与双基地距离r_b,2(k)的表达式中，然后联立双基地距离r_b,1(k)的表达式与双基地距离r_b,2(k)的表达式，利用最大似然法估计目标k时刻在第j个高度子区间的位置坐标

和目标k时刻在第j个高度子区间对应的估计协方差C_j(k)；

步骤2所述根据第一部外源雷达实时得到的目标方位角、第二部外源雷达实时得到的目标方位角构建两部外源雷达实时目标方位角量测向量：

其中，ψ_b,1(k)为第一部外源雷达在k时刻实时得到的目标方位角，ψ_b,2(k)为第二部外源雷达在k时刻实时得到的目标方位角；

步骤2所述两部外源雷达实时目标方位角量测向量z_ψ(k)与待求解的k时刻的目标真实位置q(k)＝[x(k) y(k) h(k)]^T之间的关系为：

其中，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，σ_ψ1为第一部外源雷达方位角量测噪声，σ_ψ2为第二部外源雷达方位角量测噪声；

步骤2所述构造各高度子空间实时的代价函数为：

Dist(k,j)＝u_ψ,j(k)^TS_ψ,j(k)^-1u_ψ,j(k)

其中，u_ψ,j(k)为k时刻第j个高度子区间方位角新息，S_ψ,j(k)为k时刻第j个高度子区间方位角新息协方差矩阵，u_ψ,j(k)按下式计算

其中，z_ψ(k)为两部外源雷达k时刻的目标方位量测向量，x_r,1为第一部外源雷达接收站X方向位置坐标，y_r,1为第一部外源雷达接收站Y方向位置坐标，x_r,2为第二部外源雷达接收站X方向位置坐标，y_r,2为第二部外源雷达接收站Y方向位置坐标，

为目标k时刻在第j个高度子区间的X方向位置坐标估计值,

为目标k时刻在第j个高度子区间的Y方向位置坐标估计值；

所述S_ψ,j(k)按下式计算：

S_ψ,j(k)＝H_ψ(k)C_j(k)H_ψ(k)^T+R_ψ(k)

其中，H_ψ(k)表示k时刻两部外源雷达方位角量测向量z_ψ(k)对应的雅克比矩阵，C_j(k)为目标在k时刻的平面位置估计值对应的协方差矩阵，R_ψ(k)表示k时刻方位角量测向量z_ψ(k)的量测噪声协方差矩阵；

步骤2所述根据各高度子空间实时的代价函数递推计算实时目标在各高度子区间的权重，具体为：

其中，w_j(k)k时刻目标在第_j个高度子区间的权重，p(z(k)|j)表示k时刻目标在第j个高度子区间的概率，N_a表示目标高度区间的数量；

在高斯条件下，p(z(k)|j)的表达式如下

其中，S_ψ,j(k)为k时刻方位角新息协方差矩阵，Dist(k,j)为k时刻第j个高度子空间代价函数；

步骤2所述目标k时刻的高度估计值为：

其中，w_j(k)为k时刻目标在第j个高度子区间的权重，

为第j高度子区间的平均高度，N_a表示目标高度区间的数量；

步骤2所述目标k时刻的高度方差为：

其中，w_j(k)为目标k时刻在第j个高度子区间的权重，

为第j高度子区间的平均高度，

为k时刻目标高度的估计值，N_a表示目标高度区间的数量。

4.根据权利要求1所述的基于两部两坐标外源雷达系统的3D目标跟踪方法，其特征在于，步骤3所述根据步骤2中目标高度初始时刻的估计值及两部雷达初始时刻的双基地距离量测值对IMM-EKF滤波器进行初始化，具体为：

将步骤2中目标高度初始时刻的估计值

代入初始时刻第一部外源雷达的双基地距离r_b,1(1)与双基地距离r_b,2(1)的表达式中，然后联立双基地距离r_b,1(1)的表达式与双基地距离r_b,2(1)的表达式，利用最大似然法估计目标初始时刻的平面位置坐标

和目标初始时刻平面位置的估计协方差C_x(1)，然后对IMM-EFK内的所有N个滤波器的初始状态和对应的协方差进行相同的初始化操作，该初始状态为：

初始协方差矩阵为

其中，v_x,max为目标X方向速度的最大值，v_y,max为目标Y方向速度的最大值，且v_x,max与v_y,max均为已知量，初始时刻模型m转移到模型n的混合转移概率设为u^mn(1)，为已知量；

步骤3所述根据上一时刻各个运动目标模型间的混合转移概率对各滤波器的输出结果进行交互为：

式中：

其中，

表示k-1时刻滤波器m的目标状态输出值、P^m(k-1|k-1)表示示k-1时刻滤波器m的状态输出值相应的协方差矩阵，

表示示k-1时刻模型n的状态交互值，P^on(k-1|k-1)表示示k-1时刻模型n的协方差交互值，u^mn(k-1)表示示k-1时刻模型m转移到模型n的混合转移概率，π^mn表示模型m转移到模型n的先验转移概率，将示k-1时刻各个运动目标模型的输出值进行交互，作为k时刻的输入；

步骤3所述经过IMM-EKF估计器处理后得到本时刻各滤波器状态的更新值为：

将第一部外源雷达实时得到的k时刻目标双基地距离、第一部外源雷达实时得到的k时刻目标方位角、第一部外源雷达实时得到的k时刻目标双基地速度与第二部外源雷达实时得到的k时刻目标双基地距离、第二部外源雷达实时得到的k时刻目标方位角、第二部外源雷达实时得到的k时刻目标双基地速度组成量测向量z(k)，将目标在k时刻X方向的真实位置x(k)，Y方向的真实位置y(k)，X方向的真实速度x(k)，Y方向的真实速度y(k)，记为向量：

x(k)＝[x(k) y(k) v_x(k) v_y(k)]^T；

状态更新过程如下具体处理过程如下：

P^m(k|k-1)＝F^m(k-1)P^om(k-1|k-1)(F^m(k-1))^T+Q^m(k-1)

K^m(k)＝P^m(k|k-1)H^m(k)^T[H^m(k)P^m(k|k-1)H^m(k)^T+R(k)]

P^m(k|k)＝P^m(k|k-1)-K^m(k)H^m(k)P^m(k|k-1)

至此，得到k时刻目标在第m个滤波器的状态估计值

和相应的协方差矩阵P^m(k|k)，状态更新过程中，F^m(k-1)表示k时刻在第m个滤波器内目标状态转移矩阵，Q^m(k-1)表示k时刻在第m个滤波器内目标的运动噪声协方差矩阵，

表示上一时刻模型m的状态交互值，P^om(k-1|k-1)表示上一时刻模型m的协方差交互值,R(k)为k时刻量测向量z(k)对应的量测噪声协方差矩阵，H^m(k)为量测方程k时刻在第m个滤波器内对应的雅克比矩阵；

步骤3所述根据上一时刻各个运动目标模型的输出值的交互结果计算出各滤波器新息的似然函数更新本时刻目标运动模型的概率为：

对于第m个模型，其概率按下式进行计算：

式中，Λ^m(k)为k时刻第m个滤波器新息的似然函数，C^m(k-1)为步骤3.1中计算得到的中间结果；

步骤3所述根据本时刻不同运动模型的概率对各滤波器的滤波结果进行加权融合为：

其中，

表示k时刻在第m个滤波器的状态估计值，P^m(k|k)表示k时刻在第m个滤波器的状态估计值对应的协方差矩阵。

Images