CN110501696A

CN110501696A - 一种基于多普勒量测自适应处理的雷达目标跟踪方法

Info

Publication number: CN110501696A
Application number: CN201910573952.5A
Authority: CN
Inventors: 程婷; 彭瀚; 李茜; 檀倩倩; 苏洋
Original assignee: University of Electronic Science and Technology of China
Current assignee: University of Electronic Science and Technology of China
Priority date: 2019-06-28
Filing date: 2019-06-28
Publication date: 2019-11-26
Anticipated expiration: 2039-06-28
Also published as: CN110501696B

Abstract

本发明属于多普勒雷达目标跟踪领域，特别涉及一种基于多普勒量测自适应处理的雷达目标跟踪方法。本发明基于BLUE‑KF的位置量测滤波方法和自适应伪信息的选取，实现可处理多普勒量测的序贯滤波。其中，基于BLUE‑KF方法的位置量测滤波方法实现了基于预测值量测转换的无偏估计；同时利用基于预测值的转换伪量测，获得伪状态的无偏估计。在此基础上，基于伪量测误差协方差与伪状态估计误差协方差的差异度变化率自适应地选取转换伪量测或伪状态估计进行序贯滤波。本发明在系统能够同时获得目标位置量测和多普勒量测的情况下，实现对目标非线性量测的自适应处理和目标的有效跟踪，且具有较高的跟踪精度。

Description

一种基于多普勒量测自适应处理的雷达目标跟踪方法

技术领域

本发明属于多普勒雷达目标跟踪领域，特别涉及一种基于多普勒量测自适应处理的雷达目标跟踪方法。

背景技术

在雷达目标跟踪中，目标的状态方程一般是建立在直角坐标系下的，而量测值一般是在极坐标系下获得的。这样，目标跟踪就成为一个非线性估计问题。为了解决这一问题，许多研究都集中在混合坐标系中的非线性量测跟踪问题上，包括扩展卡尔曼滤波器(Extend Kalman Filter，EKF)，不敏卡尔曼滤波器(Unscented Kalman Filter,UKF)和粒子滤波器(Particle Filter,PF)。由于EKF中的泰勒级数展开近似，不敏卡尔曼滤波器的确定性采样方法，这些方法在强非线性度时滤波效果不佳，甚至发散。粒子滤波方法虽然比其他非线性滤波器具有更好的性能，但由于其滤波方法涉及到大量粒子的计算，其复杂的运算量使得粒子滤波的实现较为困难。

除上述方法外，可以利用基于MMSE准则的最佳线性无偏估计(Best linearunbiasedEstimation,BLUE)滤波器解决量测非线性问题,文献[Zhao Z,Li X R,Jilkov VP.Best linear unbiased filtering with nonlinear measurements for targettracking.IEEE Transactions on Aerospace and Electronic Systems,2004,40(4):1324-1336.]推导了基于位置量测信息的BLUE方法。当目标的多普勒量测信息可以同时获取时,文献(WeiWang,DanLi,LipingJiang.BestlinearunbiasedestimationalgorithmwithDopplermeasurementsinsphericalcoordinates.系统工程与电子技术(英文版),2016,27(1):128-139.)提出可处理多普勒量测的最佳线性无偏估计滤波器的递归形式。

针对位置量测的非线性问题，转换位置量测卡尔曼滤波器(CPMKF)也是一种有效的方法，其中传感器中的测量(极坐标或球面坐标)通过已知的坐标转换转换为笛卡尔坐标中的量测，并计算转换误差的均值和协方差矩阵。目前已有不少学者对此问题进行了研究，提出了一些改进方法，去偏量测转换卡尔曼滤波(CMKF-D)(D.Lerro andY.Barshalom.Tracking with debiased consistent converted measurements versusEKF.IEEE Transactions on Aerospace andElectronic Systems,29(3):1015–1022,1993.)采用相减去偏的方式处理量测转换偏差，在后来的研究中发现，量测转换的均值与真实值之间存在乘性因子的偏差，因此构建了无偏转换量测卡尔曼滤波(UCMKF)算法(M.Longbin,Song Xiaoquan,Y.,and Y.Barshalom.Unbiased convertedmeasurementsfor tracking.IEEE Transactions on Aerospace andElectronic Systems,34(3):1023–1027,1998.)，UCMKF算法在计算转换误差的均值和协方差过程中存在兼容性的问题，修正的无偏转换量测卡尔曼滤波(MUCMKF)算法为解决这一问题，对其转换误差的统计特性重新做出推导。但是MUCMKF也是在量测值的条件下推导转换量测误差的协方差矩阵，导致协方差矩阵和量测误差是相关的。文献(P.Willett S.Bordonaro andY.Barshalom.Decorrelated unbiasedconverted measurement kalman filter.IEEETransactions on Aerospaceand Electronic Systems,50(2):1431–1444,2014.)中提出了去相关的无偏量测转换卡尔曼滤波方法，该方法基于预测位置信息的推导量测转换误差的统计特性以避免上述的量测值相关性问题。

上述量测转换算法都仅仅考虑了雷达的位置量测。当同时存在多普勒量测时，如何基于位置量测转换方法进一步充分利用多普勒信息以提高目标跟踪精度受到广泛关注。为减小多普勒量测模型在直角坐标系下的强非线性，学者们通过将斜距和多普勒量测相乘构造了伪量测。段战胜等(见文献：DuanZhansheng,HanChongzhao and X.Rong Li,Sequential Nonlinear Tracking Filter with Range-rateMeasurements in SphericalCoordinates,7th International Conference on Information Fusion,Stockholm,2004,131-138.)将去偏量测转换算法推广为包含多普勒量测且斜距误差和多普勒误差相关的序贯滤波算法，序贯滤波可以认为是直接利用伪量测的可处理多普勒量测的滤波方法。另一种方法是静态融合算法(Zhou Gongjian,Pelletier Michel,KirubarajanThiagalingan,Statically fused converted position and dopplermeasurement kalman filters.IEEE Transactions on Aerospace and ElectronicSystems,2014,50(1):300-316.)，该方法中，首先利用位置量测转换卡尔曼滤波器估计目标位置状态，再由多普勒量测转换卡尔曼滤波器估计伪状态，最后利用伪状态估计结果更新位置量测转换卡尔曼滤波器的输出，获得最终的状态估计结果。

可见，序贯滤波方法和静态融合方法分别利用伪量测和伪状态估计结果对基于位置量测转换的状态估计结果进行更新。理论上，伪状态估计是由滤波器输出的估计值，应比伪量测更加准确。但是仿真发现伪状态估计需要一定的收敛时间，利用伪状态估计对位置状态估计结果进行更新时需要较长收敛时间，期间所获得的最终状态估计精度比利用伪量测更新结果的精度差。此外，上述算法都是以量测值为条件下推导量测转换误差的均值和协方差矩阵，导致了状态估计结果和量测误差是相关的。

发明内容

针对上述存在问题或不足，为解决雷达跟踪系统中不能同时获得目标位置量测和多普勒量测的目标跟踪问题，本发明考虑跟踪系统如何合理地处理多普勒信息，提供了一种基于多普勒量测自适应处理的雷达目标跟踪方法。

具体技术方案如下：

设k-1时刻雷达获取的序贯滤波状态估计为协方差为P(k-1)。雷达采样周期为T，k时刻获取的量测数据分别为距离量测r_m(k)、方位角量测α_m(k)和多普勒量测带下标m字符表示量测数据,其量测噪声和是零均值加性高斯白噪声，距离量测、方位角量测和多普勒量测的方差分别为和距离量测和多普勒量测噪声相关系数为ρ。本发明k-1时刻到k时刻的滤波步骤如下：

步骤1：采用最佳线性无偏估计算法的卡尔曼滤波形式(BLUE-KF)计算基于位置量测的状态估计结果。

步骤1.1按照下式计算目标位置状态预测

其中，F(k-1)为k-1时刻的状态转移矩阵，为k-1时刻的状态估计。G(k-1)为噪声驱动矩阵，为过程噪声的期望值，分别为在x和y方向上的预测位置，分别为在x和y方向上的预测速度。

预测误差协方差表示为：

P(k,k-1)＝F(k-1)P(k-1)F^T(k-1)+G(k-1)Q(k-1)G^T(k-1) (2)

其中，(·)^T为矩阵的转置运算；Q(k-1)为过程噪声协方差矩阵。

步骤1.2：计算预测位置量测值Z^p(k,k-1)

其中，为偏差补偿因子矩阵，式中，是方位角量测误差，其误差标准差为σ_α；H(k)为量测矩阵。

步骤1.3：计算新息自相关矩阵S(k)

其中，矩阵中的每一项按如下方式计算，为了简化公式，将新息自相关矩阵中的元素时标(k)省略。例如，s₁₁即为s₁₁(k)，其内部元素中时标也作相同省略。

其中，λ₂,λ₃,λ₄为其余补偿因子：为两个方向上的位置预测误差。因此及是预测误差协方差矩阵P(k,k-1)中对应位置的元素。

步骤1.3：计算基于位置量测的状态更新向量

其中，Z^c,p(k)为补偿后的转换位置量测，其计算方法为：

C(k)为卡尔曼增益，具体如下，由于需要对预测误差协方差矩阵P(k,k-1)中的元素进行索引，因此这里将P(k,k-1)等效表示为

其中，S^-1(k)为新息自相关矩阵的逆矩阵。为预测误差协方差矩阵的第1列，为预测误差协方差矩阵的第3列。

步骤1.4:计算位置状态估计误差协方差矩阵P^p

P^p(k)＝P(k,k-1)-C(k)[Λ(k)S(k)Λ^T(k)]C^T(k) (8)

将Λ(k)S(k)Λ^T(k)定义矩阵D(k)，即

步骤1.5:计算位置量测误差协方差矩阵R^p

R^p(k)＝D(k)-H(k)P(k,k-1)H^T(k) (9)

上述位置量测误差协方差矩阵会在之后步骤中的序贯滤波部分使用。

步骤2：获得伪量测的量测转换结果：

步骤2.1：计算去偏后的转换伪量测η^c(k)

r_m(k)为距离量测和为多普勒量测,带下标m字符表示量测数据,距离量测和多普勒量测的方差分别为和距离量测和多普勒量测噪声相关系数为ρ。

步骤2.2：基于预测值计算伪量测转换误差协方差R^η(k)和位置量测与伪量测转换误差互协方差R^pη(k)；其中R^pη(k)＝[R^xη(k) R^yη(k)]^T。R^xη(k)是伪量测与目标在x方向上位置量测的量测误差协方差，R^yη(k)是伪量测与目标在y方向上位置量测的量测误差协方差。为简化公式，这里同样将内部参数的时标(k)省略，式中的目标状态预测参数将用下标t表示，如预测距离r(k,k-1)简化为r_t具体如下：

其中，r_t,α_t和分别表示目标预测距离，预测方位角及预测的径向速度，通过式(1)中的一步预测状态获得。和分别表示距离预测误差方差，方位角预测误差方差和径向速度预测误差方差，通过式(2)中预测误差协方差P(k,k-1)获得。

步骤3：基于去偏伪量测的卡尔曼滤波

设k-1时刻的伪状态估计向量为估计误差协方差为P^η(k-1)。其中伪状态估计向量的具体形式定义为：

其中，伪状态估计向量由伪状态估计的零阶导和其一阶导数构成。

步骤3.1：计算伪状态估计向量的预测值

其中，F^η为伪状态转移矩阵；Γ_s为过程噪声驱动矩阵，u(k-1)为噪声项。具体形式如下

其中，q为过程噪声强度。

步骤3.2：计算伪状态预测误差协方差

其中，Γ_x为噪声输入矩阵，Q_x,Q_s为过程噪声协方差矩阵。其计算方法如下

Qs(k)＝diag(2q²,2q²)

其中，分别为目标在x方向和y上的位置估计和速度估计，它们是基于位置量测的估计结果中的元素；P^xx(k),是基于位置量测估计误差协方差矩阵P^p(k)中的元素。和P^p(k)从步骤1的中获得，P^p(k)的形式如下

步骤3.3：计算多普勒伪状态估计

其中，H^η＝[1 0]是伪状态的观测矩阵，K^η(k)是伪状态滤波增益，按下式计算获得

K^η(k)＝P^η(k,k-1)[H^η(k)]^TS^η(k)^-1 (18)

伪状态的新息协方差矩阵S^η(k)按下式计算

S^η(k)＝H^ηP^η(k,k-1)[H^η]^T+R^η(k) (19)

其中，协方差阵R^η由式(13)获得

步骤3.4：计算多普勒伪状态估计误差协方差

P^η(k)＝[I-K^η(k)H^η]P^η(k,k-1) (20)

其中，I是与伪状态维度相同的单位矩阵。

步骤4：在切换点之前采用去偏伪量测作为伪信息，在切换点之后采用伪状态估计作为伪信息。

步骤4.1：计算k时刻的伪量测误差协方差R^η(k)与伪状态估计误差协方差P^η(k)中的第一行第一列的元素P^η(1,1)的差异度变化率Δ^η(k)。为了方便表示，在此定义P^η(1,1)为标量P^η(k)。

Δ^η(k)＝|P^η(k)-R^η(k)|-|P^η(k-1)-R^η(k-1)| (21)

步骤4.2：根据给定的门限Δ_th，若差异度变化率Δ^η(k)满足如下(22)约束，将式(17)获得的多普勒伪状态估计作为伪信息，输入给序贯滤波器；否则，将去偏伪量测η^c(k)作为伪信息输入。

Δ^η(k)≤Δ_th (22)

步骤5：序贯滤波

把式(5)和式(8)计算出的位置状态估计结果P^p(k)视为预测值，将上述步骤中获得的伪信息作为量测，采用序贯滤波方法进行滤波处理。最终得到k当前时刻的序贯滤波状态估计和估计误差协方差P(k)。

序贯滤波方法的具体滤波过程参考文献(ZhanshengDuan,Chongzhao Han andX.Rong Li,Sequential Nonlinear Tracking Filter with Range-rate Measurementsin Spherical Coordinates,7th International Conference on Information Fusion,Stockholm,2004,131-138.)

从上述步骤可见，本发明基于BLUE-KF的位置量测滤波方法和自适应伪信息的选取，实现可处理多普勒量测的序贯滤波。其中，基于BLUE-KF方法的位置量测滤波方法实现了基于预测值量测转换的无偏估计；同时利用基于预测值的转换伪量测，获得伪状态的无偏估计。在此基础上，基于伪量测误差协方差与伪状态估计误差协方差的差异度变化率自适应地选取转换伪量测或伪状态估计进行序贯滤波。

本发明中包含的原理主要分为三个方面，分别是基于最佳线性无偏估计器卡尔曼形式的滤波方法、预测值的量测转换方法和伪信息自适应选择方法，下面分别对各原理进行详细说明。

最佳线性无偏估计器的卡尔曼形式

在最小均方误差的标准中，最小均方误差线性估计器(Best linear unbiasedestimator,BLUE)是所有的线性无偏估计器中最优的。在文献中获得BLUE(LMMSE,Linearminimum mean square error)滤波器步骤：

预测：

P(k,k-1)＝F(k-1)P(k-1)F^T(k-1)+G(k-1)Q(k-1)G^T(k-1) (24)

更新：

P(k)＝P(k,k-1)-K(k)S(k)K^T(k) (27)

其中，K(k)为滤波增益定义如下由于需要对预测误差协方差矩阵P(k,k-1)中元素进行索引，因此这里将P(k,k-1)中的时间标识进行调整，等效表示为

其中，这里用代表的第一列的元素组成的矩阵。

以上为BLUE滤波算法步骤，其中的参数含义在前文中已有说明。下面对BLUE滤波算法的卡尔曼形式进行整理。首先，根据式(26)和式(25)有：

其中，Z^c,p(k)为转换后的量测值：

且：

C(k)＝K(k)Λ^-1(k)

(32)

另外，根据式(28)，有：

因此：

类似于卡尔曼滤波中的：

C(k)＝P(k,k-1)H(k)S^-1(k)

(35)

其中，D(k)＝Λ(k)S(k)Λ^T(k)。

另外，结合式(27)，可以得到：

P(k)＝P(k,k-1)-K(k)Λ^-1(k)

×(Λ(k)S(k)Λ^T(k))

×(K(k)Λ^-1(k))^T

(36)

即P(k)＝P(k,k-1)-C(k)D(k)C^T(k)，与卡尔曼滤波的估计误差协方差更新形式相符合。基于此，根据卡尔曼滤波中新息协方差的计算方法D(k)＝P(k,k-1)+R^p(k)，从而可以得到，BLUE的卡尔曼形式中，转换量测误差的协方差矩阵为：

R^p(k)＝D(k)-H(k)P(k,k-1)H^T(k)

＝Λ(k)S(k)Λ^T(k)-H(k)P(k,k-1)H^T(k)

(37)

其中：

根据以上推导可看出，BLUE算法能够被整理为Kalman滤波器的形式。BLUE可以视为采用无偏量测转换方法和基于预测估计值计算转换误差协方差矩阵的量测转换卡尔曼滤波器。

基于预测值的多普勒量测转换方法：

为了减弱多普勒量测和目标运动状态之间的强非线性程度，可以采用如下的伪量测转换方程

其中，为笛卡尔坐标系中伪量测的转换误差。

对上式求均值可得

可见传统量测是有偏的，对其进行无偏处理可得无偏转换伪量测η^c(k)

多普勒伪量测无偏转换误差：

其中，η(k)为伪量测真实值，有

对多普勒伪量测转换测量误差基于预测值求取均值及其协方差阵，同时求取位置和伪量测的互协方差，可得到如下结果：

去偏量测误差均值:

其中，r_t(k)为目标的预测距离，为预测的径向速度

多普勒量测无偏转换量测误差协方差R^η(k)和多普勒伪量测与位置量测互协方差R^xη(k),R^yη(k)如下：为简化公式的结果，这里同样将内部参数的时标(k)省略，如预测距离r_t(k)简化为r_t具体如下。

(44)-(46)中涉及到预测距离、预测方位角、预测多普勒径向速度以及它们的预测误差方差信息，它们的计算方法及原理如下：

令直角坐标系下，状态预测位置预测协方差矩阵P(k,k-1)中的具体元素表示为：

通过非线性变换获得距离、方位角和多普勒速度的预测值方式如下：

其中距离，方位角及多普勒速度的预测误差方差通过位置预测协方差矩阵P(k,k-1)的雅克比变换矩阵获得。

自适应选取伪信息方法

利用转换多普勒卡尔曼滤波器可以获得伪状态估计结果，理论上，伪状态估计由滤波器输出应该比去偏伪量测更准确。但是，在仿真中发现利用伪状态估计更新位置状态估计的RMSE曲线需要一定的时间完成收敛，该RMSE和利用伪量测更新位置状态估计的RMSE对比如说明书附图图2所示。可以看出在两条曲线“交叉点”之后，伪状态估计的精度比去偏伪量测的精度更好，而在此“交叉点”前，去偏伪量测的精度比伪状态估计的精度好。然而，在实际中伪信息的真实值是未知的，这样的RMSE曲线无法在实时获得。因此，依靠两者的RMSE曲线来确定交叉点出现的时刻的方法并不适用。

为了能够在线获得“交叉点”，并选取出合适的伪信息，我们利用了两个参数，分别是与去偏伪量测η^c(k)对应的量测误差协方差R^η(k)，与伪状态估计对应的估计误差协方差P^η(k)，图3给出了单次蒙特卡洛仿真中两者的对比结果。

可以看出，当误差协方差的改善不明显时所对应的时刻便是“交叉点”。因此，定义估计对量测的改善度差异度为：

Δ^η(k)＝|P^η(k+1)-R^η(k+1)|-|P^η(k)-R^η(k)|≤Δ_th (51)

其中，Δ_th是给定的门限值，当二者的差异度变化率满足门限时，其时间索引k便为“交叉点”所对应的时刻，可认为切换点已被找到。基于上述结果，可以选取合适的伪信息作为量测进行滤波更新，即在切换点之前采用去偏伪量测作为伪信息，在切换点之后采用伪状态估计作为伪信息。

本发明为了解决目标的位置量测与运动状态之间的非线性关系，采用了最佳线性无偏估计理论(BLUE)，其中使用了BLUE算法的卡尔曼滤波形式(BLUE of Kalman Form,BLUE-KF)来获得目标位置状态估计，通过BLUE-KF滤波过程以避免量测转换方法中对量测转换误差统计特性繁琐的推导。其最终形式可以被视为采用相乘去偏方法和基于预测位置信息推导转换误差统计特性的量测转换卡尔曼滤波方法。

为解决多普勒量测所引入的非线性问题，本发明采用目标的距离和多普勒信息来构造伪量测，并构造了用于处理伪量测的转换多普勒量测卡尔曼滤波器(ConvertedDoppler Measurement Kalman Filter,CDMKF)来获得伪状态估计，针对伪状态估计与伪量测各自存在的问题进行分析，选取出最佳的量测输入给序贯滤波器，完成对目标位置状态的滤波更新。最终获得了一种基于多普勒量测自适应处理的雷达目标跟踪方法(Sequential Filtering based on BLUE with Adaptive Pseudo Information forTarget Tracking,BLUESQ-ADP)。

综上可见，本发明考虑跟踪系统如何合理地处理多普勒信息，在系统能够同时获得目标位置量测和多普勒量测的情况下，实现对目标非线性量测的自适应处理和目标的有效跟踪，且具有较高的跟踪精度。

附图说明

图1为本发明的目标跟踪流程图；

图2为实施例中两种伪信息(伪量测、伪状态估计)的RMSE对比曲线；

图3为实施例伪量测转换误差协方差与伪状态估计误差协方差曲线；

图4为实施例中场景1RMSE曲线；

图5为实施例中场景2RMSE曲线；

图6为实施例中场景3RMSE曲线；

图7为实施例中场景4RMSE曲线；

图8为实施例中场景5RMSE曲线；

图9为实施例中场景6RMSE曲线；

图10为实施例中场景7RMSE曲线；

具体实施方式

为了比较算法的跟踪性能，我们从运算时间、位置估计误差均方差RMSE两个方面来比较算法的复杂度和跟踪精度。

考虑对目标匀速直线运场景进行跟踪仿真。目标的初始位置坐标(x＝2km,y＝6km)，初始速度为(v_x＝50m/s,v_y＝30m/s)，雷达采样周期为1s，目标的量测值包括径向距离，方位角和径向速度。假设各量测噪声为高斯零均值白噪声，其噪声标准差定义为表1所示。过程噪声假定为高斯白噪声，其标准差为q＝0.5m/s。整个仿真的蒙特卡洛循环次数为500。

表1仿真场景参数

在上述7个场景下，选取(1)采用了基于多普勒伪状态的最佳线性无偏估计算法(BLUESF)，(2)基于量测值的量测转换序贯滤波算法(SEKF)，(3)基于量测值的静态融合量测转换卡尔曼滤波算法(SFCMKF)。

位置均方根误差曲线分别列于图4-10，其中，图4-8中对应于相关系数为0时的RMSE曲线。图4-6中，多普勒量测噪声标准差固定位0.01m/s；图7-10中多普勒量测噪声标准差为0.1m/s.在图9中相关系数为ρ＝0.9，图10中的相关系数为ρ＝-0.9。其余量测参数参照表中设置。

实施结果分析：本发明可以实现有效的目标跟踪，并且跟踪精度比其余传统的量测转换序贯滤波方法高；由于考虑了自适应选取伪信息其跟踪精度也比基于最佳线性无偏估计的静态融合跟踪方法高。

从说明书附图中的RMSE曲线综合分析可看出：SFCMKF算法性能最差，由于其需要花费大量时间来收敛且该算法是基于量测值推导的转换误差统计特性，因此在跟踪精度及性能效果上并没有太大提升。收敛时间长的问题同样体现在BLUESF算法中，但是BLUESF算法采用了线性无偏估计技术处理位置量测，弥补了CMKF算法存在的缺陷；SEKF算法中存在的问题是量测转换误差的统计特性是基于量测值推导的，因此，SEKF算法的跟踪精度没有达到最优。本发明所提算法BLUESQ-ADP自适应地选取伪信息进行滤波估计解决了上述算法中存在的问题，并且具有较高的跟踪精度。

综上，本发明所提算法一种基于最佳线性无偏估计自适应处理多普勒量测的目标跟踪方法(BLUESQ-ADP)能够实现有效的目标跟踪，相比于其他可以处理多普勒量测的目标跟踪算法，本算法具有较高的跟踪精度。

Claims

1.一种基于多普勒量测自适应处理的雷达目标跟踪方法，具体技术方案如下：

设k-1时刻雷达获取的序贯滤波状态估计为协方差为P(k-1)，雷达采样周期为T，k时刻获取的量测数据分别为距离量测r_m(k)、方位角量测α_m(k)和多普勒量测带下标m字符表示量测数据,其量测噪声和是零均值加性高斯白噪声，距离量测、方位角量测和多普勒量测的方差分别为和距离量测和多普勒量测噪声相关系数为ρ；

k-1时刻到k时刻的滤波步骤如下：

步骤1：采用最佳线性无偏估计算法的卡尔曼滤波形式BLUE-KF计算基于位置量测的状态估计结果；

步骤1.1按照下式计算目标位置状态预测

其中，F(k-1)为k-1时刻的状态转移矩阵，为k-1时刻的状态估计，G(k-1)为噪声驱动矩阵，为过程噪声的期望值，分别为在x和y方向上的预测位置，分别为在x和y方向上的预测速度；

预测误差协方差表示为：

P(k,k-1)＝F(k-1)P(k-1)F^T(k-1)+G(k-1)Q(k-1)G^T(k-1) (2)

其中，(·)^T为矩阵的转置运算，Q(k-1)为过程噪声协方差矩阵；

步骤1.2：计算预测位置量测值Z^p(k,k-1)

其中，为偏差补偿因子矩阵，式中，是方位角量测误差，其误差标准差为σ_α；H(k)为量测矩阵；

步骤1.3：计算新息自相关矩阵S(k)

其中，矩阵中的每一项按如下方式计算，为了简化公式，将新息自相关矩阵中的元素时标(k)省略，其内部元素中时标也作相同省略；

其中，λ₂,λ₃,λ₄为其余补偿因子：为两个方向上的位置预测误差；因此及是预测误差协方差矩阵P(k,k-1)中对应位置的元素；

步骤1.3：计算基于位置量测的状态更新向量

其中，Z^c,p(k)为补偿后的转换位置量测，其计算方法为：

C(k)为卡尔曼增益，对预测误差协方差矩阵P(k,k-1)中的元素进行索引，将P(k,k-1)等效表示为

其中，S^-1(k)为新息自相关矩阵的逆矩阵，为预测误差协方差矩阵的第1列，为预测误差协方差矩阵的第3列；

步骤1.4:计算位置状态估计误差协方差矩阵P^p

P^p(k)＝P(k,k-1)-C(k)[Λ(k)S(k)Λ^T(k)]C^T(k) (8)

将Λ(k)S(k)Λ^T(k)定义矩阵D(k)，即

步骤1.5:计算位置量测误差协方差矩阵R^p

R^p(k)＝D(k)-H(k)P(k,k-1)H^T(k) (9)

步骤2：获得伪量测的量测转换结果：

步骤2.1：计算去偏后的转换伪量测η^c(k)

r_m(k)为距离量测和为多普勒量测,带下标m字符表示量测数据,距离量测和多普勒量测的方差分别为和距离量测和多普勒量测噪声相关系数为ρ；

步骤2.2：基于预测值计算伪量测转换误差协方差R^η(k)和位置量测与伪量测转换误差互协方差R^pη(k)，其中R^pη(k)＝[R^xη(k)R^yη(k)]^T；R^xη(k)是伪量测与目标在x方向上位置量测的量测误差协方差，R^yη(k)是伪量测与目标在y方向上位置量测的量测误差协方差；简化公式，将内部参数的时标(k)省略，式中的目标状态预测参数将用下标t表示，如预测距离r(k,k-1)简化为r_t，具体如下：

其中，r_t,α_t和分别表示目标预测距离，预测方位角及预测的径向速度，通过式(1)中的一步预测状态获得；和分别表示距离预测误差方差，方位角预测误差方差和径向速度预测误差方差，通过式(2)中预测误差协方差P(k,k-1)获得；

步骤3：基于去偏伪量测的卡尔曼滤波

设k-1时刻的伪状态估计向量为估计误差协方差为P^η(k-1)；其中伪状态估计向量的具体形式定义为：

其中，伪状态估计向量由伪状态估计的零阶导和其一阶导数构成；

步骤3.1：计算伪状态估计向量的预测值

其中，F^η为伪状态转移矩阵；Γ_s为过程噪声驱动矩阵，u(k-1)为噪声项，具体形式如下

其中，q为过程噪声强度；

步骤3.2：计算伪状态预测误差协方差

其中，Γ_x为噪声输入矩阵，Q_x,Q_s为过程噪声协方差矩阵，其计算方法如下

Q_s(k)＝diag(2q²,2q²)

其中，分别为目标在x方向和y上的位置估计和速度估计，它们是基于位置量测的估计结果中的元素；P^xx(k),是基于位置量测估计误差协方差矩阵P^p(k)中的元素，和P^p(k)从步骤1的中获得，P^p(k)的形式如下：

步骤3.3：计算多普勒伪状态估计

其中，H^η＝[1 0]是伪状态的观测矩阵，K^η(k)是伪状态滤波增益，按下式计算获得：

K^η(k)＝P^η(k,k-1)[H^η(k)]^TS^η(k)^-1 (18)

伪状态的新息协方差矩阵S^η(k)按下式计算

S^η(k)＝H^ηP^η(k,k-1)[H^η]^T+R^η(k) (19)

其中，协方差阵R^η由式(13)获得；

步骤3.4：计算多普勒伪状态估计误差协方差

P^η(k)＝[I-K^η(k)H^η]P^η(k,k-1) (20)

其中，I是与伪状态维度相同的单位矩阵；

步骤4：在切换点之前采用去偏伪量测作为伪信息，在切换点之后采用伪状态估计作为伪信息；

步骤4.1：计算k时刻的伪量测误差协方差R^η(k)与伪状态估计误差协方差P^η(k)中的第一行第一列的元素P^η(1,1)的差异度变化率Δ^η(k)，定义P^η(1,1)为标量P^η(k)；

Δ^η(k)＝|P^η(k)-R^η(k)|-|P^η(k-1)-R^η(k-1)| (21)

Δ^η(k)≤Δ_th (22)

步骤5：序贯滤波：

把式(5)和式(8)计算出的位置状态估计结果P^p(k)视为预测值，将上述步骤中获得的伪信息作为量测，采用序贯滤波方法进行滤波处理，最终得到k当前时刻的序贯滤波状态估计和估计误差协方差P(k)。