CN108802721A - 一种任意直线约束下目标跟踪方法 - Google Patents

Abstract

本发明涉及一种任意直线约束下目标跟踪方法，包括：从观测雷达处获取目标位置量测信息构建量测向量，量测向量包括目标相对观测雷达坐标系原点的距离量测和方位角量测；利用目标过往时刻状态向量对目标当前时刻状态向量进行状态增广，得到增广后的状态向量及其对应的状态方程，增广状态包括k时刻及之前d个连续时刻的状态，d表示增广部分的时间跨度；根据目标运动的直线轨迹形状构造伪量测描述任意直线约束关系，并将伪量测增广到量测向量中，得到增广后的量测方程；采用非线性滤波方法，利用增广后的状态方程和量测方程进行滤波。本发明构造伪量测，利用直线轨迹形状信息提高滤波精度，为任意直线约束下的目标跟踪问题提供了一条新的解决途径。

Description

一种任意直线约束下目标跟踪方法

技术领域

本发明涉及目标跟踪技术领域，尤其涉及一种任意直线约束下目标跟踪方法。

背景技术

目标跟踪就是系统对来自目标的量测(距离、角度、多普勒速度等)进行处理，以便保持对目标现时状态(位置、速度、加速度等)的估计。约束状态估计是在目标状态服从等式或不等式约束条件的情况下，对其进行状态估计的方法。在很多实际目标跟踪场景中，目标运动轨迹不完全由目标本身的运动速度决定，而是受其所处物理环境或是其自身运动特点影响或限制，并不是无约束的自由运动。合理利用这些实际场景中蕴含的约束信息可以有效的提高状态估计性能和滤波精度。

在实际跟踪场景中存在这样一种约束条件，其来源于所有直线轨迹共同的形状特征而非某一条特定轨迹，我们称之为任意直线约束。举例来讲，在地面目标跟踪问题中，跟踪在道路上行驶的车辆时，地图提供的道路位置信息常常被用作目标运动的约束先验信息。然而在一些情况下，无法获取地图信息，或者无法保证地图信息是准确可靠的。假如地图本身是未及时更新的，或存在制图误差的，那么地图上关于道路位置，方向的具体信息都是不可信的。此时将其作为约束先验信息引入滤波器中会导致滤波性能恶化甚至发散。

除了地面目标外，在对舰船或空中目标跟踪时，这种情况更加普遍，这是由于舰船和飞机本身的特点，导致其有很大可能在相当长一段时间内保持直线运动。然而关于其航行路线的具体信息一般是难以精确获得的，并且目标真实运动轨迹不一定完全与航线重合。因此在这类约束目标跟踪中，可利用的约束先验信息只有目标轨迹形状特征，而这种形状特征是无法通过传统的约束建模方法来描述的，且不同于先验信息不完整的线性等式约束条件。因此，探寻准确高效的任意直线约束建模和状态估计方法有重要的意义。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是解决现有技术没有针对任意直线约束建模和状态估计方法的问题。

(二)技术方案

为了解决上述技术问题，本发明提供了一种任意直线约束下目标跟踪方法，包括以下步骤：

S1、从观测雷达处获取目标位置量测信息构建量测向量，量测向量包括目标相对观测雷达坐标系原点的距离量测和方位角量测

S2、利用目标过往时刻的状态向量对目标当前时刻的状态向量x_k及目标运动模型的状态方程进行状态增广，得到增广后的状态向量及其对应的状态方程，增广状态包括k时刻及之前d个连续时刻的状态，d表示增广部分的时间跨度；

S3、根据目标运动的直线轨迹形状构造伪量测描述任意直线约束关系，并将伪量测增广到量测向量中，得到增广后的量测方程；

S4、采用非线性滤波方法，利用增广后的状态方程和量测方程进行滤波，得到约束状态估计结果，实现目标跟踪。

优选地，所述步骤S2中，笛卡尔坐标系下目标运动模型的状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k；

其中，x_k是k时刻状态向量，下标为对应时刻，包含k时刻沿x、y方向位置分量x_k、y_k以及速度分量x_k+1是k+1时刻状态向量，Φ_k是状态转移矩阵，v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其过程噪声协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

采用近匀速模型(NCV)作为线运动目标跟踪中的目标运动模型，其状态转移矩阵Φ_k和噪声分布矩阵Γ_k分别为：

对应的状态向量为T为观测雷达采样间隔。

优选地，所述步骤S2中对目标当前时刻的状态向量x_k进行增广，增广后的状态向量为：

其中，

对增广状态对应的状态方程中的状态转移矩阵和噪声分布矩阵做相应的增广，得到如下公式：

其中，I和0分别代表与状态转移矩阵Φ_k维数相同的单位矩阵和零矩阵；

增广后的过程噪声协方差矩阵为：

优选地，所述步骤S3中构造伪量测描述任意直线约束关系，得到笛卡尔坐标系下的伪量测为：

优选地，所述步骤S3中将伪量测增广到量测向量中，得到增广后的量测方程为：

对应的量测噪声协方差矩阵为：

其中，是k时刻量测向量，是表征量测向量与状态向量间关系的函数；是量测噪声向量；和分别是距离量测和方位角量测对应的量测噪声，和是对应的量测噪声方差。

优选地，所述步骤S4中采用的非线性滤波方法包括转换量测卡尔曼滤波方法、无迹卡尔曼滤波方法、扩展卡尔曼滤波方法、容积卡尔曼滤波方法或粒子滤波方法其中之一。

优选地，所述步骤S4中采用的非线性滤波方法为容积卡尔曼滤波方法，所述步骤S4包括以下步骤：

判断k，若k＝1或k＝2，进行滤波初始化，计算状态初值及初始状态协方差；若k>2则跳过此步骤，直接进行下一步骤；

利用增广的状态方程计算状态一步预测及一步预测协方差；

利用增广的量测方程计算预测量测和预测量测协方差；

计算预测的状态与预测的量测间的交互协方差；

根据交互协方差计算滤波增益；

根据滤波增益更新约束状态估计和协方差。

优选地，所述步骤S4中进行滤波初始化时，采用两点差分法，利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为：

其中，和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是将观测雷达极坐标下的目标位置量测通过无偏量测转换得到的笛卡尔坐标系量测，转换公式为：

其中，是转换后的k时刻量测向量；是转换后得到的沿x，y方向的笛卡尔坐标量测；μ_θ是去偏系数，由方位角测量噪声方差求得：

转换量测对应的量测噪声协方差矩阵为：

其中，R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

优选地，所述步骤S4中，从k＝3时刻开始滤波，根据k-1时刻的滤波结果对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差矩阵：

对计算得到的一步预测协方差矩阵进行Cholesky分解：

计算基于选择的2n_a个容积采样点：

根据容积采样点，计算预测量测均值：

计算预测量测对应的预测量测协方差矩阵：

计算预测的状态与预测的量测间的交互协方差：

计算滤波增益：

更新约束状态估计结果及协方差：

计算状态估计：

计算状态估计协方差：

其中，表示增广滤波结果，n_a＝n(d+1)为增广状态向量维数，n为增广前单个时刻状态对应的状态维数，i＝0,1,...,2n_a，ξ_i是容积采样点：

其中，代表矩阵的第i列。

优选地，所述步骤S4得到增广滤波结果中包含k时刻的约束滤波估计结果以及d个连续时刻的平滑结果，其中k-d时刻的平滑结果经历d次平滑，估计精度随平滑次数增加而升高；

对实时性和效率要求较高的场合，设置d＝1，输出约束滤波结果作为k时刻的约束状态估计结果；

对估计精度要求较高的场合，设置d>1，输出约束平滑结果作为k-d时刻的约束状态估计结果。

(三)有益效果

本发明的上述技术方案具有如下优点：本发明提供了一种任意直线约束下目标跟踪方法，提出了基于状态增广的任意直线约束建模方法，根据所有直线轨迹共同满足的约束关系，构造出最小伪量测集，并针对所构造的伪量测提出了相应的有效滤波方法，且约束状态估计结果精度有明显提升，本发明合理利用了目标运动轨迹中的形状特征信息，避免了信息浪费，提高了估计精度，为任意直线约束下的目标跟踪问题提供了一条新的解决途径，可用于地面目标、舰船或空中目标跟踪。

附图说明

图1示出了本发明实施例四中任意直线约束下目标跟踪方法中量测向量的构成；

图2示出了本发明实施例五仿真实验中构造的笛卡尔坐标系下沿直线匀速运动的目标轨迹；

图3示出了利用一种无约束非线性滤波方法(标准转换量测卡尔曼滤波)和本发明实施例五的方法分别得到的位置均方根误差对比结果图；

图4示出了利用一种无约束非线性滤波方法(标准转换量测卡尔曼滤波)和本发明实施例五的方法分别得到的速度均方根误差对比结果图；

图5示出了利用一种无约束非线性平滑方法(增广状态转换量测卡尔曼滤波方法)和本发明实施例五的方法分别得到的位置均方根误差对比结果图；

图6示出了利用一种无约束非线性平滑方法(增广状态转换量测卡尔曼滤波方法)和本发明实施例五的方法分别得到的速度均方根误差对比结果图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

本发明实施例提供的一种任意直线约束下目标跟踪方法，包括以下步骤：

S1、从观测雷达处获取目标位置量测信息构建量测向量，量测向量包括目标相对观测雷达坐标系原点的距离量测和方位角量测

跟踪过程中从观测雷达处获得目标位置量测信息为现有技术，观测雷达如何获得这些信息，以及本发明实施方式的方法如何从雷达处获得这些信息，本领域技术人员可以采取现有技术中的各种方式来实现，无论采用哪种方法都在本发明的保护范围内。

S2、利用目标过往时刻的状态向量x_k及对目标当前时刻的状态向量目标运动模型的状态方程进行状态增广，得到增广后的状态向量及其对应的状态方程，增广状态包括k时刻及k时刻之前d个连续时刻(过往时刻)的状态，d表示增广部分的时间跨度。增广状态中包括d+1个连续时刻的状态，上标“a”代表增广后的向量、矩阵和函数。

S3、根据目标运动的直线轨迹形状构造伪量测描述任意直线约束关系，并将伪量测增广到量测向量中，得到增广后的量测方程。

S4、采用非线性滤波方法，利用增广后的状态方程和量测方程进行滤波，得到k时刻的增广滤波结果增广滤波结果中包含有约束状态估计结果，实现目标跟踪。

由于量测与状态之间是非线性关系，因此在滤波过程中需要采用非线性滤波方法。优选地，常用的非线性滤波方法包括转换量测卡尔曼滤波方法、无迹卡尔曼滤波方法、扩展卡尔曼滤波方法、容积卡尔曼滤波方法以及粒子滤波方法等。

本发明提供了一种任意直线约束下目标跟踪方法，根据直线轨迹的特征构造伪量测，利用k时刻和k-1时刻目标位置和速度的状态分量给出约束关系的完整描述。而该约束并不限定为某条具体直线，其方向和位置都可以是任意的，约束条件中包含的先验信息全部源于目标真实轨迹形状特征，不涉及任何近似也不受噪声影响，引入滤波系统可以有效提高估计精度。本方法为任意直线约束下的目标跟踪问题提供了一条新的解决途径，合理利用了目标运动轨迹中的形状特征信息，避免了信息浪费，提高了估计精度。

实施例二

本实施例二与实施例一基本相同，相同之处不再赘述，不同之处在于：

本实施例步骤S2中，建立笛卡尔坐标系下目标运动模型，目标运动模型的状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k

其中，x_k是k时刻状态向量，下标为对应时刻，包含k时刻沿x、y方向位置分量x_k、y_k，和速度分量x_k+1是k+1时刻状态向量；Φ_k是状态转移矩阵；v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其过程噪声协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵。

优选地，所述步骤S1中采用近匀速模型(NCV)作为线运动目标跟踪中的目标运动模型，即以直线运动目标跟踪中常用的近匀速模型(NCV)举例，其状态转移矩阵Φ_k和噪声分布矩阵Γ_k分别为：

此时对应的状态向量为T为观测雷达采样间隔。

进一步优选地，对状态向量x_k进行增广，用d增广部分的时间跨度，增广后的状态向量为：

其中，即增广状态中包含了总共d+1个连续时刻的状态。

对增广状态对应的状态方程中的状态转移矩阵和噪声分布矩阵进行相应增广，得到如下公式：

其中，I和0分别代表与状态转移矩阵Φ_k维数相同的单位矩阵和零矩阵；

增广后的过程噪声协方差矩阵为

实施例三

本实施例三与实施例二基本相同，相同之处不再赘述，不同之处在于：

本实施例步骤S3利用增广状态来构造伪量测描述任意直线约束关系，得到笛卡尔坐标系下的伪量测为：

可以看到只需要利用k时刻和k-1时刻目标位置和速度的状态分量就可以给出约束关系的完整描述。事实上，只要在d≥1的情况下都可以通过上面的公式得到约束关系的描述。

优选地，将伪量测增广到量测向量中，得到增广后的量测方程为：

对应的量测噪声协方差矩阵为：

其中，是k时刻量测向量，是表征量测向量与状态向量间关系的函数；是量测噪声向量；和分别是距离量测和方位角量测对应的量测噪声，和是对应的量测噪声方差。由于假设位置量测之间是不相关的，因此互协方差R_k,rθ＝0。伪量测是不受噪声影响的，与状态向量之间是非线性关系。由于伪量测是常数，因此互协方差方差和均为零。伪量测和位置量测之间也不存在相关性，所以协方差矩阵中两者之间的互协方差为零，包括和

实施例四

本实施例四与实施例三基本相同，相同之处不再赘述，不同之处在于：

如图1所示，本实施例步骤S4中采用的非线性滤波方法为容积卡尔曼滤波，按照容积卡尔曼滤波器的一般流程进行滤波，步骤S4包括以下步骤：

判断k，若k＝1或k＝2，进行滤波初始化，计算状态初值及初始状态协方差；若k>2则跳过此步骤，直接进行下一步骤；

利用增广的状态方程计算状态一步预测及一步预测协方差；

利用增广的量测方程计算预测量测和预测量测协方差；

计算预测的状态与预测的量测间的交互协方差；

根据交互协方差计算滤波增益；

根据得到的滤波增益更新约束状态估计和协方差。

优选地，步骤S4中进行滤波初始化时，采用两点差分法，即利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测值得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为：

其中，和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是将观测雷达极坐标位置量测通过无偏量测转换得到的笛卡尔坐标系量测，转换公式为：

其中，是转换后的k时刻量测向量，是转换后得到的沿x，y方向的笛卡尔坐标量测；为从观测雷达处获取的距离，方位角量测；μ_θ是去偏系数，可由方位角测量噪声方差求得：

转换量测对应的量测噪声协方差矩阵为：

其中，R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

上标“c”代表与转换量测相关的向量、矩阵和函数。

从k＝3时刻开始滤波，首先根据k-1时刻的约束状态估计(滤波结果)对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差：

对协方差矩阵进行Cholesky(平方根法)分解：

计算基于选择的2n_a个容积采样点：

根据容积采样点，计算预测量测均值：

计算预测量测对应的协方差矩阵：

计算量测和状态向量的交互协方差：

计算滤波增益：

最后更新状态估计及协方差：

计算状态估计：

计算状态估计协方差：

其中，表示增广滤波结果，n_a＝n(d+1)为增广状态向量维数，n为增广前单个时刻状态对应的状态维数，i＝0,1,...,2n_a，ξ_i是容积采样点：

代表矩阵的第i列。

在增广滤波结果中包含了k时刻的约束滤波结果可以看到在容积卡尔曼滤波器中一共需要选取2n(d+1)个容积采样点，采样点数量随d增大而增大，进而导致计算负担增大。然而事实上，该方法适用于任意d≥1的场景。

优选地，在对实时性和效率要求较高的场合，可以设置d＝1，并输出约束滤波结果作为k时刻的状态估计结果。

而增广滤波结果中同时还包含了d个连续时刻的平滑结果，其中经历d次平滑，并且估计精度随平滑次数增加而升高，因此在对估计精度要求较高而对实时性要求不高的场景，可以设置相对较大的固定延迟d，并输出约束平滑结果作为k-d时刻的状态估计结果。

实施例五

本实施例五与实施例四基本相同，相同之处不再赘述，不同之处在于：

本实施例利用仿真数据进行蒙特卡洛实验。仿真试验中的目标沿直线匀速运动且真实运动状态服从线性等式约束，运动轨迹如图2所示。此时假设目标真实直线轨迹的具体先验信息是无法获得的或不可信的，因此无法采用传统约束估计方法来进行估计。

本实施例采用一种无约束非线性滤波方法，即未引入任何约束的转换量测卡尔曼滤波方法，和本发明提供的任意直线下目标跟踪方法进行对比。仿真中设置观测雷达采样间隔为1s，仿真目标200s的运动，重复进行200次蒙特卡洛实验。

图3示出了利用无约束非线性滤波方法(标准转换量测卡尔曼滤波)和本发明提供方法的位置滤波结果对应的均方根误差对比，图4示出了两种方法速度滤波结果对应的均方根误差对比。从中可以看到引入任意直线约束后，滤波误差明显减小，优化效果十分明显。这是由于约束中包含关于目标状态的有用信息，增大了滤波器可利用的信息量，从而提高了滤波精度。

图5和图6示出了d＝3时利用无约束非线性滤波方法(增广状态转换量测卡尔曼滤波)和本发明提供方法的位置、速度平滑结果分别对应的均方根误差对比。可以看到，引入任意直线约束后，平滑误差相对无约束平滑方法也明显减低，并且由于平滑方法一般要比滤波方法误差更低，约束平滑方法可以获得相当高的估计精度。

综上所述，本发明提供了一种任意直线约束下目标跟踪方法，不是某条具体直线的近似，而是基于所有直线轨迹共有的形状特征得到的，其中包含关于目标运动状态的有用先验信息，引入跟踪系统中会增加滤波器可利用的信息量，从而提高滤波精度。因此，本发明具有如下优点：

(1)提出了基于状态增广的任意直线约束建模方法，对于沿未知直线轨迹移动的目标，利用增广状态中当前时刻(k时刻)和前一时刻(k-1时刻)的位置和速度状态分量，根据所有直线轨迹共同满足的约束关系，构造出最小伪量测集，以最少数量的伪量测对约束关系进行完整描述，其具体形式为：

(2)针对所构造的伪量测提出了相应的有效滤波方法。将伪量测增广到向量中，从而将其中包含的与目标状态有关的约束先验信息引入跟踪系统，增加滤波器可利用的信息量，进而达到提升估计精度的目的。

(3)在同一贝叶斯框架下，同时得到k时刻约束滤波结果和k-1至k-d时刻约束平滑结果，经过固定延迟平滑得到的约束状态估计结果相对无约束平滑估计结果精度有明显提升。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种任意直线约束下目标跟踪方法，其特征在于，包括以下步骤：

S1、从观测雷达处获取目标位置量测信息构建量测向量，量测向量包括目标相对观测雷达坐标系原点的距离量测和方位角量测

S2、利用目标过往时刻状态向量对目标当前时刻的状态向量x_k进行状态增广，得到增广后的状态向量及其对应的状态方程，增广状态包括k时刻及之前d个连续时刻的状态，d表示增广部分的时间跨度；

S3、根据目标运动的直线轨迹形状构造伪量测描述任意直线约束关系，并将伪量测增广到量测向量中，得到增广后的量测方程；

S4、采用非线性滤波方法，利用增广后的状态方程和量测方程进行滤波，得到约束状态估计结果，实现目标跟踪。

2.根据权利要求1所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S2中，笛卡尔坐标系下目标运动模型的状态方程为：

x_k+1＝Φ_kx_k+Γ_kv_k；

其中，x_k是k时刻状态向量，下标为对应时刻，包含k时刻沿x、y方向位置分量x_k、y_k以及速度分量x_k+1是k+1时刻状态向量，Φ_k是状态转移矩阵，v_k是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其过程噪声协方差矩阵为cov(v_k)＝Q_k≥0；Γ_k是噪声分布矩阵；

采用近匀速模型(NCV)作为线运动目标跟踪中的目标运动模型，其状态转移矩阵Φ_k和噪声分布矩阵Γ_k分别为：

对应的状态向量为T为观测雷达采样间隔。

3.根据权利要求2所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S2中对目标当前时刻的状态向量x_k进行增广，增广后的状态向量为：

其中，

对增广状态对应的状态方程中的状态转移矩阵和噪声分布矩阵做相应的增广，得到如下公式：

其中，I和0分别代表与状态转移矩阵Φ_k维数相同的单位矩阵和零矩阵；

增广后的过程噪声协方差矩阵为：

4.根据权利要求3所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S3中构造伪量测描述任意直线约束关系，得到笛卡尔坐标系下的伪量测为：

5.根据权利要求4所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S3中将伪量测增广到量测向量中，得到增广后的量测方程为：

对应的量测噪声协方差矩阵为：

其中，是k时刻量测向量，是表征量测向量与状态向量间关系的函数；是量测噪声向量；和分别是距离量测和方位角量测对应的量测噪声，和是对应的量测噪声方差。

6.根据权利要求1所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S4中采用的非线性滤波方法包括转换量测卡尔曼滤波方法、无迹卡尔曼滤波方法、扩展卡尔曼滤波方法、容积卡尔曼滤波方法或粒子滤波方法其中之一。

7.根据权利要求5所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S4中采用的非线性滤波方法为容积卡尔曼滤波方法，所述步骤S4包括以下步骤：

判断k，若k＝1或k＝2，进行滤波初始化，计算状态初值及初始状态协方差；若k>2则跳过此步骤，直接进行下一步骤；

利用增广的状态方程计算状态一步预测及一步预测协方差；

利用增广的量测方程计算预测量测和预测量测协方差；

计算预测的状态与预测的量测间的交互协方差；

根据交互协方差计算滤波增益；

根据滤波增益更新约束状态估计和协方差。

8.根据权利要求7所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S4中进行滤波初始化时，采用两点差分法，利用最初两个时刻k＝1、k＝2时刻的笛卡尔坐标系下位置量测得到k＝2时刻的状态估计：

其对应的初始状态协方差矩阵为：

其中，和是笛卡尔坐标下的目标沿x，y方向的位置量测信息，是将观测雷达极坐标下的目标位置量测通过无偏量测转换得到的笛卡尔坐标系量测，转换公式为：

其中，是转换后的k时刻量测向量；是转换后得到的沿x，y方向的笛卡尔坐标量测；μ_θ是去偏系数，由方位角测量噪声方差求得：

转换量测对应的量测噪声协方差矩阵为：

其中，R_k,xx，R_k,yy分别为x，y方向笛卡尔坐标量测对应的量测噪声方差，R_k,xy为x，y方向笛卡尔坐标量测对应的量测噪声之间的互协方差：

9.根据权利要求8所述的任意直线约束下目标跟踪方法，其特征在于，所述步骤S4中，从k＝3时刻开始滤波，根据k-1时刻的滤波结果对k时刻状态进行一步预测：

计算状态一步预测：

计算状态一步预测协方差矩阵：

对计算得到的一步预测协方差矩阵进行Cholesky分解：

计算基于选择的2n_a个容积采样点：

根据容积采样点，计算预测量测均值：

计算预测量测对应的预测量测协方差矩阵：

计算预测的状态与预测的量测间的交互协方差：

计算滤波增益：

更新约束状态估计结果及协方差：

计算状态估计：

计算状态估计协方差：

其中，表示增广滤波结果，n_a＝n(d+1)为增广状态向量维数，n为增广前单个时刻状态对应的状态维数，i＝0,1,...,2n_a，ξ_i是容积采样点：

其中，代表矩阵的第i列。

10.根据权利要求9所述的任意直线约束下目标跟踪方法，其特征在于：

所述步骤S4得到增广滤波结果中包含k时刻的约束滤波估计结果以及d个连续时刻的平滑结果，其中k-d时刻的平滑结果经历d次平滑，估计精度随平滑次数增加而升高；

对实时性和效率要求较高的场合，设置d＝1，输出约束滤波结果作为k时刻的约束状态估计结果；

对估计精度要求较高的场合，设置d>1，输出约束平滑结果作为k-d时刻的约束状态估计结果。