CN110208791B - 一种纯角度跟踪伪线性滤波方法 - Google Patents

Abstract

本发明涉及一种纯角度跟踪伪线性滤波方法，该方法对每个跟踪周期依次执行如下步骤：从观测雷达处获取量测信息，若当前周期为起始周期，则通过在真实目标状态周围形成高斯分布，生成目标状态估计；根据获取的量测信息，利用状态方程计算状态一步预测及状态预测协方差；利用拟线性量测方程计算量测一步预测和量测预测协方差；根据状态的一步预测得到方位角预测值和预测量测矩阵，进而计算被估计的状态和量测之间的互协方差；计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪。本发明提供的纯角度跟踪伪线性滤波方法能够有效改善目标跟踪性能。

Description

一种纯角度跟踪伪线性滤波方法

技术领域

本发明涉及空间目标跟踪技术领域，尤其涉及一种纯角度跟踪伪线性滤波方法。

背景技术

雷达一般是通过距离、方位角以及多普勒速度等量测信息来定位和跟踪目标。而纯角度跟踪是仅通过使用移动传感器测量移动目标的时变方位角来估计目标运动轨迹的方法，即量测信息只有方位角这一种参量。纯角度跟踪通常需要传感器平台做超前于目标运动状态的机动运动，从而使得目标跟踪轨迹可以被观测。

纯角度跟踪问题的主要挑战是方位角量测与目标状态向量(目标位置和速度)之间是非线性关系。因此传统的卡尔曼滤波器已经不再适用于纯角度跟踪问题，于是解决非线性问题的各种滤波器开始应用于纯角度跟踪问题中。目前，一种重要的方法是通过数学运算构造伪量测，将非线性量测方程转换为伪线性量测方程，从而将卡尔曼滤波器应用于纯角度跟踪中，即伪线性卡尔曼滤波(pseudo-linear Kalman filter，PLKF)。PLKF经过构造伪量测方程导致了量测矩阵和量测噪声相关，因此其的主要缺点是存在状态估计偏差，这会严重恶化其跟踪性能，最小均方根误差(RMSE)性能非常不好。偏差补偿(Biascompensation)伪线性卡尔曼滤波方法(BC-PLKF)在PLKF基础上分析PLKF的偏差，并推导出一种状态偏差补偿方法，得到了一种闭合形式的降阶偏差伪线性估计方法。该方法与PLKF相比，其最小均方根误差改善很大，但是当量测噪声变大时，BC-PLKF的RMSE性能仍然相对比较差。选择方位角测量(the selective-angle-measurement，SAM)工具变量(instrumental variable，IV)卡尔曼滤波方法(SAM-IVKF)将IV估计方法应用于BC-PLKF中，形成一种新的混合估计方法，与PLKF、BC-PLKF相比，SAM-IVKF有最好的RMSE估计性能，但是同时也增加了计算复杂度。

因此，针对以上不足，需要提供一种性能更佳的纯角度跟踪伪线性滤波方法。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是解决现有技术中纯角度跟踪方法往往存在状态估计偏差大、跟踪性能差等问题。

(二)技术方案

为了解决上述技术问题，本发明提供了一种纯角度跟踪伪线性滤波方法，对每个跟踪周期依次执行如下步骤：

S1、从观测雷达处获取量测信息，若当前周期为起始周期，则通过在真实目标状态周围形成高斯分布，生成目标状态估计；

S2、根据获取的量测信息，利用状态方程计算状态一步预测及状态预测协方差；其中状态方程通过纯角度跟踪问题的状态空间模型得到；

S3、利用拟线性量测方程计算量测一步预测和量测预测协方差；其中拟线性量测方程通过构造伪量测得到；

S4、根据状态的一步预测得到方位角预测值和预测量测矩阵，进而计算被估计的状态和量测之间的互协方差；

S5、利用状态一步预测、状态预测协方差、量测一步预测、量测预测协方差，以及状态和量测之间的互协方差，计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪。

优选地，所述步骤S3中利用拟线性量测方程计算量测预测协方差时，采用如下步骤计算量测预测协方差中的交叉项：

将方位角量测展开为真实方位角加量测噪声的形式；将量测噪声从量测矩阵中分离；

根据三角函数公式，得到真实方位角与状态相关的表达式；

用状态的一步预测得到的状态一步预测值和状态预测误差代替真实的状态，求解交叉项的近似值。

优选地，所述步骤S2中，所述纯角度跟踪问题的状态空间模型表达式为：

x_k＝Fx_k-1+w_k-1；

其中，状态x_k＝[p_x,k,p_y,k,v_x,k,v_y,k]^T表示离散时刻k∈{1,2,3,...}的运动目标未知的位置变量和速度变量，位移r_k＝[r_x,k,r_y,k]^T表示k时刻的做机动运动的传感器位置变量；F表示状态x_k的转移矩阵；过程噪声w_k～N(0,Q)表示均值为0、方差矩阵已知为Q的白色高斯随机变量；量测噪声n_k～N(0,σ_k ²)表示均值为0、方差为

的白色高斯随机变量。

优选地，根据匀速运动模型，所述状态转移矩阵F和所述过程噪声协方差矩阵Q的表达式分别为：

其中，T代表采样间隔，q_x和q_y分别代表x方向和y方向的过程噪声功率谱密度。

优选地，所述步骤S2中计算状态一步预测和状态预测协方差时，状态一步预测值的表达式为：

状态预测误差为：

状态预测协方差为：

优选地，所述步骤S3中，构造伪量测

则拟线性量测方程表达式为：

z_k＝H_kx_k+η_k；

其中，

为量测矩阵，伪线性噪声η_k＝-||d_k||sin n_k，并且，

d_k＝Cx_k-r_k,

d_k表示k时刻的从传感器位置指向目标位置的距离向量；C表示转移矩阵，||·||表示欧几里得范数；伪线性噪声η_k均值为0，方差表达式为：

优选地，所述步骤S3中计算量测一步预测和量测预测协方差时，量测预测值的表达式为：

量测预测误差为：

量测预测协方差为：

其中，量测矩阵H_k和伪线性噪声η_k均为量测噪声n_k的函数，交叉项

和

均不为零；

所述步骤S3中，将方位角量测展开为真实方位角加量测噪声的形式；将量测噪声从量测矩阵中分离后，量测矩阵的表达式为：

H_k＝cos n_kH_1,k+sin n_kH_2,k；

其中，

H_1,k＝[sinθ_k -cosθ_k 0 0]；

H_2,k＝[cosθ_k sinθ_k 0 0]；

根据三角函数公式，得到真实方位角与状态相关的表达式为：

其中，x_k(1)、x_k(2)分别表示4*1维状态向量x_k的第一项和第二项；

用状态的一步预测得到的状态一步预测值和状态预测误差代替真实的状态时，得到状态的近似形式的表达为：

优选地，所述步骤S4中计算被估计的状态x_k和量测z_k之间的协方差时，协方差的表达式为：

优选地，所述步骤S4中根据状态的一步预测得到方位角预测值和预测量测矩阵时，方位角预测值

的表达式为：

将以真实方位角θ_k为基础的量测矩阵H_1,k和H_2,k重写为预测量测矩阵：

进而得到状态和量测之间的互协方差的表达式为：

优选地，所述步骤S5中计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪时，根据线性MMSE估计公式完成状态更新：

k时刻更新的状态相关协方差为：

(三)有益效果

本发明的上述技术方案具有如下优点：本发明提供了一种纯角度跟踪伪线性滤波方法，该方法通过构造伪量测，使得非线性量测方程转变为拟线性量测方程求解，量测矩阵与量测噪声相关，求解量测预测协方差时，其中的交叉项不为零，而是需要单独计算其数值，进而有效提高状态估计的准确性和一致性。

本发明还通过三角公式展开运算，以及将方位角量测展开为真实方位角加量测噪声的形式，将量测噪声从量测矩阵中分离开来，使得与量测噪声相关的量测矩阵表示成量测噪声与真实方位角构成的矩阵相乘的形式，从而解除了其相关性，求得结果。

附图说明

图1示出了本发明实施例中纯角度跟踪伪线性滤波方法步骤示意图；

图2示出了2D平面中由单个移动传感器收集量测信息的纯角度跟踪问题示意图；

图3示出了传感器运动轨迹；

图4示出了本发明实施例提供的纯角度跟踪伪线性滤波方法(PL-MMSE)以及PLKF、BC-PLKF、PCRLB和κ＝4的SAM-IVKF时间平均RMSE和BNorm性能比较；

图5示出了PL-MMSE、PLKF、BC-PLKF、PCRLB和κ＝3的SAM-IVKF时间平均RMSE和BNorm性能比较；

图6示出了PL-MMSE、PLKF、BC-PLKF、PCRLB和κ＝2的SAM-IVKF时间平均RMSE和BNorm性能比较；

图7示出了PL-MMSE、PLKF、BC-PLKF、PCRLB和κ＝4的SAM-IVKF的RMSE和BNorm性能比较；

图8示出了PL-MMSE、PLKF、BC-PLKF、PCRLB和κ＝3的SAM-IVKF的RMSE和BNorm性能比较；

图9示出了PL-MMSE、PLKF、BC-PLKF、PCRLB和κ＝2的SAM-IVKF的RMSE和BNorm性能比较；

图10示出了PL-MMSE、PLKF、BC-PLKF和κ＝4的SAM-IVKF的一致性检验图；

图11示出了PL-MMSE、PLKF、BC-PLKF和κ＝3的SAM-IVKF的一致性检验图；

图12示出了PL-MMSE、PLKF、BC-PLKF和κ＝2的SAM-IVKF的一致性检验图。

具体实施方式

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

如图1所示，本发明实施例提供的一种纯角度跟踪伪线性滤波方法，该方法包括对每个跟踪周期依次执行如下步骤：

S1、从观测雷达处获取量测信息，若当前周期为起始周期，则通过在真实目标状态周围形成高斯分布，生成目标状态估计。

设当前周期为第k周期，对于纯角度跟踪，量测信息只有方位角，即获取离散时刻k∈{1,2,3,...}的运动目标方位角量测优选地，若当前周期为起始周期，即k＝1时，尚没有上一周期目标状态估计，通过在真实目标状态x₁周围形成高斯分布，并限定其协方差为P₁₀＝ρ²diag(2.6²,2.6²,0.26²,0.26²)，从而生成目标状态估计

ρ表示初始化误差的大小。若k>1，那么获取量测信息后，直接进行下一步骤。

S2、根据获取的量测信息，利用状态方程计算状态一步预测及状态预测协方差；其中，状态方程通过纯角度跟踪问题的状态空间模型得到。

优选地，步骤S2中，所述纯角度跟踪问题的状态空间模型表达式为：

x_k＝Fx_k-1+w_k-1；

其中，状态x_k＝[p_x,k,p_y,k,v_x,k,v_y,k]^T表示离散时刻k∈{1,2,3,...}的运动目标未知的位置变量和速度变量，位移r_k＝[r_x,k,r_y,k]^T表示k时刻的做机动运动的传感器位置变量。F表示状态x_k的转移矩阵。过程噪声w_k～N(0,Q)表示均值为0、方差矩阵已知为Q的白色高斯随机变量；量测噪声n_k～N(0,σ_k ²)表示均值为0、方差为的白色高斯随机变量，其中，方差

可以是时变的，并且被假设为先验已知的。此外，n_k严格独立于w_k。

假设目标以几乎恒定的速度移动，即为匀速(constant velocity，CV)运动模型，因此状态转移矩阵F和过程噪声协方差矩阵Q的表达式为：

和

其中，T代表采样间隔，q_x和q_y分别代表x方向和y方向的过程噪声功率谱密度。

计算状态的一步状态预测，得到状态一步预测值表达式为：

状态预测误差为：

状态预测协方差为：

S3、利用拟线性量测方程计算量测一步预测和量测预测协方差；其中拟线性量测方程通过构造伪量测得到。优选地，为了能够将现有技术中的最小均方误差(minimum meansquared error，MMSE)估计框架应用于纯角度跟踪问题中，必须将非线性量测方程转化为线性或者拟线性方程。

构造伪量测

拟线性量测方程表达式为：

z_k＝H_kx_k+η_k；

其中，

为量测矩阵，伪线性噪声η_k＝-||d_k||sin n_k，并且，

d_k＝Cx_k-r_k,

如图2所示，d_k表示k时刻的从传感器位置指向目标位置的距离向量。C是转移矩阵，||·||表示欧几里得范数，因此伪线性噪声η_k是均值为0，方差为下式的高斯变量，伪线性噪声η_k的方差表达式为：

计算量测预测，量测预测值的表达式为：

量测预测误差的表达式为：

所以量测预测协方差为：

其中，量测矩阵H_k和伪线性噪声η_k都是量测噪声n_k的函数，量测矩阵与量测噪声相关，因此H_k和η_k之间的相关性不能被忽略，也就是上式中的交叉项和不为零，通过引入不为零的交叉项，能够提高状态估计的准确性和一致性。

优选地，量测预测协方差中的交叉项按照下述步骤计算，通过：

(1)将方位角量测

展开为真实方位角θ_k加量测噪声n_k的形式，量测矩阵H_k的表达式为：

H_k＝[sin(θ_k+n_k) -cos(θ_k+n_k) 0 0]

＝[sinθ_kcos n_k+cosθ_ksin n_k -cosθ_k cos n_k+sinθ_ksin n_k 0 0]；

＝cos n_k[sinθ_k -cosθ_k 0 0]+sin n_k[cosθ_k sinθ_k 0 0]

令，H_1,k＝[sinθ_k -cosθ_k 0 0]，H_2,k＝[cosθ_k sinθ_k 0 0]；

然后可以将量测矩阵重写，将量测噪声η_k从量测矩阵H_k中分离出来，得到量测矩阵的表达式为：

H_k＝cos n_kH_1,k+sin n_kH_2,k；

在量测噪声被假设为零均值高斯噪声的情况下，有：

E[sin n_k]＝0；

E[cos n_ksin n_k]＝0；

(2)根据三角函数公式，得到真实方位角θ_k与状态相关的表达式：如图2所示，

和

其中x_k(1)、x_k(2)分别表示4*1维状态向量x_k的第一项和第二项。

(3)用状态的一步预测得到的状态一步预测值和状态预测误差代替真实的状态，求解交叉项

和

的近似值：

由于真实的状态x_k在实际中是不可得的，所以用状态的一步预测值加状态预测误差的近似形式作为代替，即

和

由于状态的一步预测值和传感器位置r_k与状态预测误差无关，因此交叉项

可以计算为：

其中，P_xx(1,1)、P_xx(2,2)分别表示4*4的矩阵P_xx的第一和第二对角线元素。

与

的计算方法类似，

也可以被重写为：

由于伪线性噪声η_k的方差为

其中距离向量d_k不可直接获得，为了解决这个问题，采用一种d_k的近似形式，即距离向量的一步预测值

作为替代。

然后，量测预测协方差可以重写为：

本发明所提供的方法通过构造伪量测的方式，即可解决量测矩阵与量测噪声相关的问题，实现近似求解。

S4、根据状态的一步预测得到方位角预测值和预测量测矩阵，进而计算被估计的状态和量测之间的互协方差。

优选地，计算被估计的状态x_k和量测z_k之间的互协方差的表达式为：

由于在实际中真实方位角θ_k是基于目标距离的真实值的变量，因此是不可获得的，上述结果不能直接使用。

进一步优选地，为了使这些结果能够被使用，利用从状态一步预测值

计算得到的方位角预测值替换未知的真实方位角θ_k，方位角预测值的表达式为：

然后，可以将以真实方位角θ_k为基础的量测矩阵H_1,k和H_2,k重写为预测量测矩阵：

因此，可以将状态和量测之间的互协方差重写为：

S5、利用状态一步预测、状态预测协方差、量测一步预测、量测预测协方差，以及状态和量测之间的互协方差，计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪。

运用上述计算结果，根据线性MMSE估计公式完成状态更新：

k时刻更新的状态相关协方差为：

进一步地，为验证本发明所提供的纯角度跟踪伪线性滤波方法效果，利用仿真数据进行蒙特卡洛实验。下面给出本发明所提供的方法(PL-MMSE)和现有技术中伪线性卡尔曼滤波方法(PLKF)、偏差补偿伪线性卡尔曼滤波方法(BC-PLKF)、选择方位角测量工具变量卡尔曼滤波方法(SAM-IVKF)、后验克拉美罗界方法(PCRLB)进行仿真试验的结果比较。

仿真试验中目标从位置[30,42]^Tm开始以恒定速度[0,12]^Tm/s运动，传感器的轨迹是五个恒速直线，每条直线的末端位置为[60,0]^Tm，[0,7.5]^Tm，[60,15]^Tm，[0,22.5]^Tm，[60,30]^Tm，[0,37.5]^Tm。传感器轨迹如图3所示，传感器初始位置用五角星标记。传感器在采样间隔为T＝0.1s的规律时刻t_k＝kT,k∈{1,2,...,150}收集方位角量测值。量测噪声假定为独立同分布的，并且量测噪声具有已知的不随时间变化的方差，即

过程噪声的功率谱密度设置为q_x＝q_y＝0.2m²/s³。仿真模拟中，通过在真实目标状态x₁周围形成高斯分布，其协方差为P₁₀＝ρ²diag(2.6²,2.6²,0.26²,0.26²)，从而生成目标状态估计

ρ变量表示初始化误差的大小。仿真实验中ρ＝1。

图4-图6分别示出了各方法(PL-MMSE、PLKF、BC-PLKF、PCRLB以及方位角选择阈值分别为κ＝4、3和2的SAM-IVKF)的时间平均的目标位置和速度估计的RMSE性能和BNorm(Bias norm，偏差范数)性能与量测噪声的关系。其中图4中，图4(a)示出了量测噪声标准差与位置的时间平均RMSE关系，图4(b)示出了量测噪声标准差与速度的时间平均RMSE关系，图4(c)示出了量测噪声标准差与位置的时间平均BNorm关系，图4(d)示出了量测噪声标准差与速度的时间平均BNorm关系，图5、图6与图4类似，不再重复说明。

图7-图9分别显示了各方法(量测噪声标准差σ_θ＝7°，SAM-IVKF的方位角选择阈值分别为κ＝4、3和2)的目标位置和速度估计的RMSE和BNorm性能。其中，图7中，图7(a)示出了扫描序号与位置的RMSE关系，图7(b)示出了扫描序号与速度的RMSE关系，图7(c)示出了扫描序号与位置的BNorm关系，图7(d)示出了扫描序号与速度的BNorm关系，图8、图9与图7类似，不再重复说明。蒙特卡洛的次数为M＝10,000。

从图4-图9中可以看出，传统方法PLKF的RMSE和BNorm性能非常不好，但BC-PLKF，SAM-IVKF和本发明提出的新方法PL-MMSE对传统的PLKF具有显著的性能改进。BC-PLKF相比，新方法PL-MMSE表现出良好的RMSE性能改善。此外，PL-MMSE提供了与最佳选择的SAM-IVKF(在仿真中，κ＝4)相当的RMSE性能。并且与非最佳选择的SAM-IVKF(在仿真中，κ＝2,3)相比，PL-MMSE的RMSE性能甚至更优。另外，就BNorm来看，PL-MMSE的性能最优。

图10-图12分别显示了四种方法(PL-MMSE、PLKF、BC-PLKF、PCRLB以及方位角选择阈值分别为κ＝4、3和2的SAM-IVKF，量测噪声标准差σ_θ＝7°)的各种纯角度跟踪方法的NEES一致性性能。其中自由度为M·d＝400，取显著水平α＝0.025，则查询χ²分布表得到置信区间[3.46,4.57]。可以看出，传统方法PLKF的一致性非常不好，主要是因为状态估计存在偏差，但BC-PLKF，SAM-IVKF和本文中提出的新方法PL-MMSE对传统PLKF的一致性性能具有显著改进。κ＝4和κ＝3时，SAM-IVKF一致性还比较好，但是当κ＝2，SAM-IVKF的一致性已经变差。而本发明提出的新方法PL-MMSE一直都有着良好的一致性性能。

表1四种方法的平均运行时间比

为了比较各个纯角度跟踪方法的计算复杂性，在同一平台上运行各个方法，并且各个方法的平均运行时间比在表1中给出。为方便起见，平均运行时间的方法通过PLKF的平均运行时间进行归一化。从表1中可以看出，虽然PL-MMSE比PLKF和BC-PLKF运算复杂，但是比SAM-IVKF的计算复杂度小。

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

Claims (1)

1.一种纯角度跟踪伪线性滤波方法，其特征在于，对每个跟踪周期依次执行如下步骤：

S1、从观测雷达处获取量测信息，若当前周期为起始周期，则通过在真实目标状态周围形成高斯分布，生成目标状态估计；

S2、根据获取的量测信息，利用状态方程计算状态一步预测及状态预测协方差；其中状态方程通过纯角度跟踪问题的状态空间模型得到；

S3、利用拟线性量测方程计算量测一步预测和量测预测协方差；其中拟线性量测方程通过构造伪量测得到；

S4、根据状态的一步预测得到方位角预测值和预测量测矩阵，进而计算被估计的状态和量测之间的互协方差；

S5、利用状态一步预测、状态预测协方差、量测一步预测、量测预测协方差，以及状态和量测之间的互协方差，计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪；

其中，所述步骤S2中，所述纯角度跟踪问题的状态空间模型表达式为：

x_k＝Fx_k-1+w_k-1；

其中，状态x_k＝[p_x,k,p_y,k,v_x,k,v_y,k]^T表示离散时刻k∈{1,2,3,...}的运动目标未知的位置变量和速度变量，位移r_k＝[r_x,k,r_y,k]^T表示k时刻的做机动运动的传感器位置变量；F表示状态x_k的转移矩阵；过程噪声w_k～N(0,Q)表示均值为0、方差矩阵已知为Q的白色高斯随机变量；量测噪声n_k～N(0,σ_k ²)表示均值为0、方差为的白色高斯随机变量；

根据匀速运动模型，所述状态转移矩阵F和所述过程噪声协方差矩阵Q的表达式分别为：

其中，T代表采样间隔，q_x和q_y分别代表x方向和y方向的过程噪声功率谱密度；

所述步骤S2中计算状态一步预测和状态预测协方差时，状态一步预测值的表达式为：

状态预测误差为：

状态预测协方差为：

所述步骤S3中利用拟线性量测方程计算量测预测协方差时，采用如下步骤计算量测预测协方差中的交叉项：

将方位角量测展开为真实方位角加量测噪声的形式；将量测噪声从量测矩阵中分离；

根据三角函数公式，得到真实方位角与状态相关的表达式；

用状态的一步预测得到的状态一步预测值和状态预测误差代替真实的状态，求解交叉项的近似值；

所述步骤S3中，构造伪量测

则拟线性量测方程表达式为：

z_k＝H_kx_k+η_k；

其中，

为量测矩阵，伪线性噪声η_k＝-||d_k||sin n_k，并且，

d_k表示k时刻的从传感器位置指向目标位置的距离向量；C表示转移矩阵，||·||表示欧几里得范数；伪线性噪声η_k均值为0，方差表达式为：

所述步骤S3中计算量测一步预测和量测预测协方差时，量测预测值的表达式为：

量测预测误差为：

量测预测协方差为：

其中，量测矩阵H_k和伪线性噪声η_k均为量测噪声n_k的函数，交叉项

和

均不为零；

所述步骤S3中，将方位角量测展开为真实方位角加量测噪声的形式；将量测噪声从量测矩阵中分离后，量测矩阵的表达式为：

H_k＝cos n_kH_1,k+sin n_kH_2,k；

其中，

H_1,k＝[sinθ_k -cosθ_k 0 0]；

H_2,k＝[cosθ_k sinθ_k 0 0]；

根据三角函数公式，得到真实方位角与状态相关的表达式为：

其中，x_k(1)、x_k(2)分别表示4*1维状态向量x_k的第一项和第二项；

用状态的一步预测得到的状态一步预测值和状态预测误差代替真实的状态时，得到状态的近似形式的表达为：

所述步骤S4中计算被估计的状态x_k和量测z_k之间的协方差时，协方差的表达式为：

所述步骤S4中根据状态的一步预测得到方位角预测值和预测量测矩阵时，方位角预测值

的表达式为：

将以真实方位角θ_k为基础的量测矩阵H_1,k和H_2,k重写为预测量测矩阵：

进而得到状态和量测之间的互协方差的表达式为：

所述步骤S5中计算更新状态估计和状态估计协方差，完成当前周期的目标跟踪时，根据线性MMSE估计公式完成状态更新：

k时刻更新的状态相关协方差为：

Images