CN105954742A - 一种球坐标系下带多普勒观测的雷达目标跟踪方法 - Google Patents

Abstract

本发明涉及一种球坐标系下带多普勒观测的雷达目标跟踪方法，该方法包括以下步骤：伪量测构造步骤，通过当前时刻k雷达获得的距离量测r_m(k)和多普勒量测的乘积构造伪量测(转换多普勒)；量测转换步骤，将球坐标系量测转换到直角坐标系；无偏一二阶矩计算步骤，计算转换位置量测误差和转换多普勒量测误差的无偏一二阶矩；伪状态空间构造及利用CDMKF提取伪状态信息，通过当前时刻转换多普勒η(k)及其导数构造伪状态空间，并提取伪状态信息；笛卡尔状态信息提取步骤，提取目标的笛卡尔状态信息；以及静态融合步骤，静态融合伪状态信息提取步骤的伪状态信息和笛卡尔状态信息提取步骤所提取的笛卡尔状态信息。

Description

一种球坐标系下带多普勒观测的雷达目标跟踪方法

技术领域

本发明涉及雷达目标跟踪，尤其涉及球坐标系带多普勒观测的雷达目标跟踪方法。

背景技术

在目标跟踪领域，目标动态模型通常在笛卡尔坐标系中进行建模，而量测一般却是在极坐标系中得到，这样一来，目标跟踪就成为一个非线性估计问题。解决这一问题的一类常用方法就是转换量测卡尔曼滤波器，即先将传感器量测通过坐标变换表示成笛卡尔坐标系下量测的伪线性形式，然后估计转换量测误差的前两阶矩并基于卡尔曼滤波完成目标跟踪。这一方法已经得到广泛的研究，差别仅在于转换量测误差偏差和协方差求法的不同，但是它们都仅仅考虑了传感器的位置量测。实际采用的雷达，尤其是多普勒雷达，往往还可以提供多普勒量测。理论计算与实践已经证明，充分利用多普勒量测信息可以有效地提高目标的跟踪精度。

解决带多普勒量测的目标跟踪问题目前有两种思路，第一种便是利用非线性滤波方法(如EKF/UKF/PF等)直接同时处理位置量测和多普勒量测，但由于此时多普勒量测是目标运动状态的强非线性函数，传统的EKF/UKF算法稳定性得不到保证，时常出现滤波精度恶化甚至滤波发散问题，而PF计算量很大，不能满足工程实时性要求；另一种便是通过距离和多普勒的乘积构造伪量测，包括利用伪量测及其各阶微分定义伪状态空间的方法先降低多普勒的强非线性，然后结合线性滤波和非线性滤波方法利用伪量测和位置量测更新目标状态，代表性的方法有序贯扩展卡尔曼滤波器(SEKF)、序贯无敏滤波器(SUKF)和静态融合转换量测卡尔曼滤波器(SF-CMKF)。基于第二种思路的这些方法由于更合理的利用了多普勒信息，相对于前者具有更好的跟踪精度和稳定性。

但在量测误差较大时，滤波误差在SEKF和SUKF迭代过程中反馈累计扩大，会造成滤波器性能不稳定。而文献G.Zhou,M.Pelletier,T.Kirubarajan and T.Quan,“Statically fused converted position and doppler measurement kalman filters,”IEEE Transactions on Aerospace and Electronic Systems,Vol.50,no.1,pp.300-318,2014提出的SF-CMKF，首先构造了一个转换多普勒量测卡尔曼滤波器(CDMKF)从伪量测中线性的提取伪状态信息，同时利用转换位置量测卡尔曼滤波器(CPMKF)提取目标笛卡尔状态，然后联合两者的输出在最小均方误差(MMSE)准则下估计出目标最终状态，将动态非线性估计问题转换成一个动态线性估计问题和一个静态非线性融合问题，有效避免了利用非线性迭代滤波技术处理多普勒量测。但该文献中采用的量测转换是有偏的，而且是在二维极坐标系下讨论的。

发明内容

本发明鉴于背景技术的以上问题提出，用于解决背景技术中存在的问题，至少是提供一种有益的选择。

为实现以上目的，本发明公开了一种球坐标系下带多普勒观测的雷达目标跟踪方法，包括以下步骤：伪量测构造步骤，通过当前时刻k雷达获得的距离量测r_m(k)和多普勒量测的乘积构造伪量测；量测转换步骤，将球坐标系下的量测无偏地转换到直角坐标系下；根据所述伪量测和转换后位置量测，在所述直角坐标系下计算转换位置量测误差的无偏一二阶矩和转换多普勒量测误差的无偏一二阶矩的步骤；笛卡尔状态信息提取步骤，利用转换位置量测及其转换位置量测误差的无偏一二阶矩提取目标的笛卡尔状态信息；伪状态信息提取步骤，利用伪量测的真值(转换多普勒)及其导数构造伪状态空间，并利用伪量测和笛卡尔状态信息提取伪状态信息；静态融合步骤，对所述伪状态信息提取步骤所提取的伪状态信息和所述笛卡尔状态信息提取步骤所提取的目标的笛卡尔状态信息进行静态融合。

根据一种实施方式，在所述伪状态信息步骤中，利用转换多普勒量测卡尔曼滤波器提取伪状态信息；以及在所述笛卡尔状态信息提取步骤中，由转换位置量测卡尔曼滤波器提取目标的笛卡尔状态信息。

本发明的实施方式在球坐标系下提出了一种静态融合无偏转换量测卡尔曼滤波方法，该方法首先构造了一个转换多普勒量测卡尔曼滤波器(CDMKF)从伪量测中线性的提取伪状态信息，同时利用转换位置量测卡尔曼滤波器(CPMKF)提取目标笛卡尔状态，然后联合两者的输出在最小均方误差(MMSE)准则下估计出目标最终状态。可以提高其跟踪精度。

附图说明

结合附图，可以更好地理解本发明，但是附图仅仅是示例性的，不是对本发明的限制。

图1示出了球坐标下目标跟踪示意图。

图2示出了一种本发明一种实施方式的雷达目标跟踪方法的示例流程。

图3示出了仿真情况下的RMSE位置误差图。

图4示出了仿真情况下的RMSE速度误差图。

具体实施方式

下面结合附图具体说明本发明的实施方式。所说明的实施方式仅是示例性的，不是对本发明的限制。在所做的说明中，各实施方式可以互相参照。

在陈述本发明的步骤之前，先介绍一下球坐标系下带多普勒量测的目标跟踪的基本数学模型。

带多普勒量测的目标跟踪模型在笛卡尔坐标系中用离散时间状态方程表示为

X(k+1)＝ΦX(k)+ΓV(k) (40)

其中，为目标运动状态，x(k)，y(k)和z(k)分别为目标在x，y和z方向上的三个位置分量，和为相应的速度分量，Φ，Γ分别为状态转移矩阵和过程噪声增益矩阵，V(k)是均值为0，方差为Q(k)的高斯过程噪声。

图1示出了球坐标下目标跟踪示意图。如图1所示，量测方程可表示为

Z_m(k)＝f[X(k)]+W(k) (41)

其中

$Z_m\left(k\right)={\[r_m\left(k\right),{\mathrm{\β}}_m\left(k\right),{\mathrm{\ϵ}}_m\left(k\right),{\stackrel{\·}{r}}_m\left(k\right)\]}^T,f\[X\left(k\right)\]={\[r\left(k\right),\mathrm{\β}\left(k\right),\mathrm{\ϵ}\left(k\right),\stackrel{\·}{r}\left(k\right)\]}^T---\left(42\right)$

$\left\{\begin{array}{c}r\left(k\right)=\sqrt{x^2\left(k\right)+y^2\left(k\right)+z^2\left(k\right)}\\ \mathrm{\β}\left(k\right)={\tan }^{-1}\left(y\right(k)/x(k\left)\right)\\ \mathrm{\ϵ}\left(k\right)={\tan }^{-1}\left(z\right(k)/\sqrt{x^2\left(k\right)+y^2\left(k\right)})\end{array}\right.---\left(43\right)$

$\stackrel{\·}{r}\left(k\right)=\frac{x\left(k\right)\stackrel{\·}{x}\left(k\right)+y\left(k\right)\stackrel{\·}{y}\left(k\right)+z\left(k\right)\stackrel{\·}{z}\left(k\right)}{\sqrt{x^2\left(k\right)+y^2\left(k\right)+z^2\left(k\right)}},W\left(k\right)={\[\tilde{r}\left(k\right),\widetilde{\mathrm{\β}}\left(k\right),\widetilde{\mathrm{\ϵ}}\left(k\right),\widetilde{\stackrel{\·}{r}}\left(k\right)\]}^T---\left(44\right)$

r_m(k)，β_m(k)，ε_m(k)和分别为径向距离、方位角、俯仰角和多普勒量测，r(k)，β(k)，ε(k)和为相应的真值，和为相应的均值为0的高斯量测噪声，方差分别为和且和互不相关，和互不相关，和的相关系数为ρ。

球坐标系中带多普勒量测的雷达目标跟踪的目的，就是根据k时刻雷达对于目标的量测r_m(k)，β_m(k)，ε_m(k)和以及先验的量测偏差信息均值为0、方差分别为和的高斯白噪声和和的相关系数ρ，估计出目标当前时刻的运动状态

本发明的实施方式的基本步骤为：

步骤一S101：通过当前时刻k雷达获得的距离量测r_m(k)和多普勒量测的乘积构造伪量测

${\mathrm{\η}}_c\left(k\right)=r_m\left(k\right){\stackrel{\·}{r}}_m\left(k\right)=\mathrm{\η}\left(k\right)+\widetilde{\mathrm{\η}}\left(k\right)---\left(45\right)$

其中是笛卡尔坐标系中伪量测η_c(k)的转换误差。

步骤二S102：进行量测转换，将球坐标系下的量测无偏地转换到直角坐标系下

$x_c^u\left(k\right)=e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{r}_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)=x\left(k\right)+\tilde{x}\left(k\right)---\left(46\right)$

$y_c^u\left(k\right)=e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{r}_m\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)=y\left(k\right)+\tilde{y}\left(k\right)---\left(47\right)$

$z_c^u\left(k\right)=e^{{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}{r}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right)=z\left(k\right)+\tilde{z}\left(k\right)---\left(48\right)$

其中，和分别为笛卡尔坐标系中x，y和z方向上的转换后位置量测，和分别是笛卡尔坐标系中相应的位置转换量测误差，r_m(k)，β_m(k)和ε_m(k)分别是当前时刻k雷达获得的距离量测、方位角量测和俯仰角量测。

步骤三S103：计算转换位置量测误差和转换多普勒量测误差的无偏一二阶矩。转换位置量测误差和转换多普勒量测误差的均值和方差依次为(为简化起见，部分变量的索引时刻k给予省略)

${\mathrm{\μ}}^x\left(k\right)=(e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}-e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}})r_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)---\left(49\right)$

${\mathrm{\μ}}^y\left(k\right)=(e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}-e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}})r_m\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)---\left(50\right)$

${\mathrm{\μ}}^z\left(k\right)=(e^{{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}-e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2})r_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right)---\left(51\right)$

${\mathrm{\μ}}^{\mathrm{\η}}\left(k\right)={\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(52\right)$

$\begin{array}{c}{R}^{xx}\left(k\right)=\left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)(1+e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\cos (2{\mathrm{\β}}_m\left(k\right)\left)\right)(1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\\ \cos \left(2{\mathrm{\ϵ}}_m\right(k\left)\right))/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\cos }^2{\mathrm{\β}}_m\left(k\right){\cos }^2{\mathrm{\ϵ}}_m\left(k\right)\end{array}---\left(53\right)$

$\begin{array}{c}{R}^{yy}\left(k\right)=\left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)(1-e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\cos (2{\mathrm{\β}}_m\left(k\right)\left)\right)(1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\\ \cos \left(2{\mathrm{\ϵ}}_m\right(k\left)\right))/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\sin }^2{\mathrm{\β}}_m\left(k\right){\cos }^2{\mathrm{\ϵ}}_m\left(k\right)\end{array}---\left(54\right)$

$R^{zz}\left(k\right)=\left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)(1-e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}cos(2{\mathrm{\ϵ}}_m\left(k\right)\left)\right)/2-e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\sin }^2{\mathrm{\ϵ}}_m\left(k\right)---\left(55\right)$

$\begin{array}{c}{R}^{xy}\left(k\right)=R^{xy}\left(k\right)=\left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\sin \left(2{\mathrm{\β}}_m\right(k\left)\right)\\ (1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\cos (2{\mathrm{\ϵ}}_m\left(k\right)\left)\right)/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\cos }^2{\mathrm{\ϵ}}_m\left(k\right)\end{array}---\left(56\right)$

$\begin{array}{c}{R}^{xz}\left(k\right)=R^{zx}\left(k\right)=e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{\mathrm{cos\β}}_m\left(k\right)\sin \left(2{\mathrm{\ϵ}}_m\right(k\left)\right)\\ \left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)/2-e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\end{array}---\left(57\right)$

$\begin{array}{c}{R}^{yz}\left(k\right)=R^{zy}\left(k\right)=e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{\mathrm{sin\β}}_m\left(k\right)\sin \left(2{\mathrm{\ϵ}}_m\right(k\left)\right)\\ \left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)/2e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\end{array}---\left(58\right)$

$R^{\mathrm{\η}\mathrm{\η}}\left(k\right)=r_m^2\left(k\right){{\mathrm{\σ}}_{\stackrel{\·}{r}}}^2+{\mathrm{\σ}}_r^2{\stackrel{\·}{r}}_m^2\left(k\right)+(1+5{\mathrm{\ρ}}^2){\mathrm{\σ}}_r^2{{\mathrm{\σ}}_{\stackrel{\·}{r}}}^2+2r_m\left(k\right){\stackrel{\·}{r}}_m\left(k\right){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(59\right)$

$R^{x\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}x}\left(k\right)=e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(60\right)$

$R^{y\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}y}\left(k\right)=e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(61\right)$

$R^{z\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}z}\left(k\right)=e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}{\mathrm{sin\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(62\right)$

其中，r_m(k)，β_m(k)和ε_m(k)和分别是当前时刻k雷达获得的距离量测、方位角量测、俯仰角量测和多普勒量测，σ_r，σ_β，σ_ε和分别是距离量测、方位角量测、俯仰角量测和多普勒量测的测量偏差。ρ是距离和多普勒量测之间的相关系数。R^xx(k)即指转换量测误差的方差，R^xy(k)即指转换量测误差和之间的互协方差，类似符号的含义可以类推。

步骤四S104：利用之前得到的转换位置量测及其转换位置量测误差的无偏一二阶矩提取由CPMKF提取目标的笛卡尔状态信息，其迭代过程如下

${\hat{X}}_p(k+1,k)={\mathrm{\Φ}}_p{\hat{X}}_p(k,k)---\left(63\right)$

$P_p(k+1,k)={\mathrm{\Φ}}_p{P}_p(k,k){\mathrm{\Φ}}_p^T+{\mathrm{\Γ}}_{p}Q\left(k\right){\mathrm{\Γ}}_p^T---\left(64\right)$

$K_p(k+1)=P_p(k+1,k)H_p^T{\[H_p{P}_p(k+1,k)H_p^T+R^p(k+1)\]}^{-1}---\left(65\right)$

${\hat{X}}_p(k+1,k+1)={\hat{X}}_p(k+1,k)+K_p(k+1)\[Z_c^p(k+1)-H_p{\hat{X}}_p(k+1,k)\]---\left(66\right)$

P_p(k+1,k+1)＝[I-K_p(k+1)H_p]P_p(k+1,k) (67)

其中

$R^p\left(k\right)=\left[\begin{array}{ccc}{R}^{xx}\left(k\right)& R^{xy}\left(k\right)& R^{xz}\left(k\right)\\ R^{yx}\left(k\right)& R^{jy}\left(k\right)& R^{yz}\left(k\right)\\ R^{zx}\left(k\right)& R^{zy}\left(k\right)& R^{zz}\left(k\right)\end{array}\right],Z_c^p\left(k\right)=\left[\begin{array}{c}{x}_c\left(k\right)-{\mathrm{\μ}}^x\left(k\right)\\ y_c\left(k\right)-{\mathrm{\μ}}^y\left(k\right)\\ z_c\left(k\right)-{\mathrm{\μ}}^z\left(k\right)\end{array}\right]---\left(68\right)$

步骤五S105：通过当前时刻转换多普勒η(k)及其导数构造伪状态空间，并利用CDMKF提取伪状态信息。

构造伪状态空间为

$\mathrm{\η}\left(k\right)=\left[\begin{array}{c}\mathrm{\η}\left(k\right)\\ \stackrel{\·}{\mathrm{\η}}\left(k\right)\end{array}\right]---\left(69\right)$

CDMKF的迭代过程如下

$\widehat{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}\widehat{\mathrm{\η}}(k,k)+Gu\left(k\right)---\left(70\right)$

$P_{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}{P}_{\mathrm{\η}}(k,k){\mathrm{\Φ}}_{\mathrm{\η}}^T+{\mathrm{\Γ}}_x{Q}_x\left(k\right){\mathrm{\Γ}}_x^T+{\mathrm{\Γ}}_s{Q}_s\left(k\right){\mathrm{\Γ}}_s^T---\left(71\right)$

$K_{\mathrm{\η}}(k+1)=P_{\mathrm{\η}}(k+1,k)H_{\mathrm{\η}}^T{\[H_{\mathrm{\η}}{P}_{\mathrm{\η}}(k+1,k)H_{\mathrm{\η}}^T+R^{\mathrm{\η}\mathrm{\η}}(k+1)\]}^{-1}---\left(72\right)$

$\widehat{\mathrm{\η}}(k+1,k+1)=\widehat{\mathrm{\η}}(k+1,k)+K_{\mathrm{\η}}(k+1)\[Z_c^{\mathrm{\η}}(k+1)-H_{\mathrm{\η}}\widehat{\mathrm{\η}}(k+1,k)\]---\left(73\right)$

P_η(k+1,k+1)＝[I-K_η(k+1)H_η]P_η(k+1,k) (74)

其中

${\mathrm{\Γ}}_x=\left[\begin{array}{cc}T& 3T^2/2\\ 0& 2T\end{array}\right],Q_s\left(k\right)=diag\[2q^2,2q^2,2q^2\]---\left(75\right)$

${\mathrm{\Φ}}_{\mathrm{\η}}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],G={\mathrm{\Γ}}_s=\left[\begin{array}{ccc}{T}^3/2& T^3/2& T^3/2\\ T^2& T^2& T^2\end{array}\right],u\left(k\right)=E\left(\left[\begin{array}{c}{v}_x^2\left(k\right)\\ v_y^2\left(k\right)\\ v_z^2\left(k\right)\end{array}\right]\right)=\left[\begin{array}{c}q\\ q\\ q\end{array}\right]---\left(76\right)$

$\begin{array}{c}{Q}_x\left(k\right)=q\left(\left[\begin{array}{cc}{\hat{x}}^2& \hat{x}\widehat{\stackrel{\·}{x}}\\ \widehat{\stackrel{\·}{x}}\hat{x}& {\widehat{\stackrel{\·}{x}}}^2\end{array}\right]+\left[\begin{array}{cc}{\hat{y}}^2& \hat{y}\widehat{\stackrel{\·}{y}}\\ \widehat{\stackrel{\·}{y}}\hat{y}& {\widehat{\stackrel{\·}{y}}}^2\end{array}\right]+\left[\begin{array}{cc}{\hat{z}}^2& \hat{z}\widehat{\stackrel{\·}{z}}\\ \widehat{\stackrel{\·}{z}}{\hat{z}}^2& {\widehat{\stackrel{\·}{z}}}^2\end{array}\right]\right)-\\ q\left(\left[\begin{array}{cc}{P}^{xx}& P^{x\stackrel{\·}{x}}\\ P^{\stackrel{\·}{x}x}& P^{\stackrel{\·}{x}\stackrel{\·}{x}}\end{array}\right]+\left[\begin{array}{cc}{P}^{yy}& P^{y\stackrel{\·}{y}}\\ P^{\stackrel{\·}{y}y}& P^{\stackrel{\·}{y}\stackrel{\·}{y}}\end{array}\right]+\left[\begin{array}{cc}{P}^{zz}& P^{z\stackrel{\·}{z}}\\ P^{\stackrel{\·}{z}z}& P^{\stackrel{\·}{z}\stackrel{\·}{z}}\end{array}\right]\right)\end{array}---\left(77\right)$

其中T是雷达扫描周期，q是笛卡尔坐标系中各个坐标轴方向的过程高斯白噪声的方差，式(77)中P_p(k,k)由步骤四中的CPMKF提供。

步骤六S106：静态融合伪状态信息和笛卡尔状态信息(为简化起见，部分变量的索引时刻k给予省略)。

1)计算伪状态估计和目标位置估计之间的互协方差

$\begin{array}{c}{P}_{p\mathrm{\η}}(k+1)=\[I-K_p(k+1)H_p\]{\mathrm{\Φ}}_p{P}_{p\mathrm{\η}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\η}}^T{\[I-K_{\mathrm{\η}}(k+1)H_{\mathrm{\η}}\]}^T+\[I-K_p(k+1)H_p\]\\ {\mathrm{\Γ}}_{p}Q\left(k\right){\left({\mathrm{\Γ}}_x{X}_{\mathrm{\Γ}}\right)}^T{\[I-K_{\mathrm{\η}}(k+1)H_{\mathrm{\η}}\]}^T+K_p(k+1)R^{p\mathrm{\η}}(k+1)K_{\mathrm{\η}}{(k+1)}^T\end{array}---\left(78\right)$

其中

2)计算目标状态和伪观测状态(将伪状态η(k)当做目标最终状态的一种观测状态，伪状态是目标最终状态的一个数学函数)之间的协方差

$P_{XZ}=P_p{\stackrel{\·}{C}}^T-P_{p\mathrm{\η}}---\left(79\right)$

其中C是伪状态与目标状态之间的函数关系，定义如下

$\mathrm{\η}\left(k\right)=\left[\begin{array}{c}\mathrm{\η}\left(k\right)\\ \stackrel{\·}{\mathrm{\η}}\left(k\right)\end{array}\right]=C\[X\left(k\right)\]=\left[\begin{array}{c}x\left(k\right)\stackrel{\·}{x}\left(k\right)+y\left(k\right)\stackrel{\·}{y}\left(k\right)+z\left(k\right)\stackrel{\·}{z}\left(k\right)\\ {\stackrel{\·}{x}}^2\left(k\right)+{\stackrel{\·}{y}}^2\left(k\right)+{\stackrel{\·}{z}}^2\left(k\right)\end{array}\right]---\left(80\right)$

是函数C的Jacobin矩阵。

3)计算伪观测状态的方差

$P_{ZZ}=\stackrel{\·}{C}{P}_p{\stackrel{\·}{C}}^T+P_{\mathrm{\η}}+\frac{1}{2}\underset{i=1}{\overset{n_{\mathrm{\η}}}{\Σ}}\underset{j=1}{\overset{n_{\mathrm{\η}}}{\Σ}}{e}_i{e}_j^{T}tr\left({\stackrel{\·\·}{C}}^i{P}_p{\stackrel{\·\·}{C}}^j{P}_p\right)-\stackrel{\·}{C}{P}_{p\mathrm{\η}}-{\left(\stackrel{\·}{C}{P}_{p\mathrm{\η}}\right)}^T---\left(81\right)$

其中，e_i是笛卡尔坐标系中第i个n_η维偏置单位向量，是函数C的Jacobin矩阵，为函数C的第i个分量的Hessian矩阵。

4)计算目标的最终状态和状态估计方差

$\hat{X}={\hat{X}}_p+P_{XZ}{\left(P_{ZZ}\right)}^{-1}(\widehat{\mathrm{\η}}-\stackrel{\‾}{Z})---\left(82\right)$

P＝P_p-P_XZ(P_ZZ)^-1(P_XZ)^T (83)

其中

本发明相对于其他方法的优势在于，将球坐标系下的动态非线性估计问题转换成一个动态线性估计问题和一个静态非线性融合问题，有效避免了利用非线性迭代滤波技术处理多普勒量测；而且步骤三中的量测转换方法是无偏的，相对于现有球坐标系中加性有偏量测转换方法更精确。

为了验证球坐标下静态融合无偏转换量测卡尔曼滤波器的有效性，将本文算法(SF-UCMKF)与仅考虑位置量测的CPMKF算法、同时处理位置和多普勒量测的SEKF算法进行仿真比较。

考虑对一在三维空间里作常速运动的目标进行跟踪，目标初始位置(30km,30km,30km),初始速度为20m/s，方向为(60deg,60deg)，位于坐标原点的多普勒雷达以1s的采样周期提供目标径向距离、方位角、俯仰角和多普勒量测数据，其量测噪声的标准差分别为σ_r＝300m，σ_β＝0.3deg,σ_ε＝0.3deg和 和的相关系数ρ＝-0.9，三个坐标轴上的过程噪声标准差均为0.01m/s²。采用两点差分法对跟踪滤波器进行初始化，评价指标为位置、速度的均方根(RMSE)误差。

对上述条件做100步上的50次Monte-Carlo仿真结果如图3和图4所示。

从以上的仿真结果可以看出，SF-UCMKF和SEKF相比CPMKF初始误差和稳态误差都有很大幅度的减小，这说明多普勒量测的引入，可以显著改善跟踪滤波器的性能；SF-UCMKF又比SEKF具有更小的初始误差和稳态误差，其估计精度接近CRLB极限，这是因为SF-UCMKF同时利用两个动态最优线性滤波器(CPMKF和CDMKF)提取目标位置信息和伪状态信息，而将非线性信息的处理放在动态迭代之外的静态融合估计器中完成，这样避免了非线性误差在动态迭代过程中多次反馈累计，从而有效改善了跟踪滤波器的性能。

Claims (8)

1.一种球坐标系带多普勒观测的雷达目标跟踪方法，包括以下步骤：

伪量测构造步骤，通过当前时刻k雷达获得的距离量测r_m(k)和多普勒量测的乘积构造伪量测；

量测转换步骤，将球坐标系下的量测无偏地转换到直角坐标系下；

根据所述伪量测和转换位置量测，在所述直角坐标系下计算转换位置量测误差的无偏一二阶矩和转换多普勒量测误差的无偏一二阶矩的步骤；

笛卡尔状态信息提取步骤，利用转换位置量测及其转换位置量测误差的无偏一二阶矩提取目标的笛卡尔状态信息；

伪状态信息提取步骤，利用伪量测的真值及其导数构造伪状态空间，并利用伪量测和笛卡尔状态信息提取伪状态信息；

静态融合步骤，对所述伪状态信息提取步骤所提取的伪状态信息和所述笛卡尔状态信息提取步骤所提取的目标的笛卡尔状态信息进行静态融合。

2.根据权利要求1所述的方法，其特征在于，

在所述伪状态信息步骤中，利用转换多普勒量测卡尔曼滤波器提取伪状态信息；以及

在所述笛卡尔状态信息提取步骤中，由转换位置量测卡尔曼滤波器提取目标的笛卡尔状态信息。

3.根据权利要求2所述的方法，其特征在于，利用以下的公式构造所述伪量测

${\mathrm{\η}}_c\left(k\right)=r_m\left(k\right){\stackrel{\·}{r}}_m\left(k\right)=\mathrm{\η}\left(k\right)+\widetilde{\mathrm{\η}}\left(k\right)---\left(1\right)$

其中是笛卡尔坐标系中伪量测η_c(k)的转换误差。

4.根据权利要求3所述的方法，其特征在于，在所述量测转换步骤，利用以下的公式将球坐标系下的量测无偏地转换到直角坐标系下

$x_c^u\left(k\right)=e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{r}_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)=x\left(k\right)+\tilde{x}\left(k\right)---\left(2\right)$

$y_c^u\left(k\right)=e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{r}_m\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)=y\left(k\right)+\tilde{y}\left(k\right)---\left(3\right)$

$z_c^u\left(k\right)=e^{{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}{r}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right)=z\left(k\right)+\tilde{z}\left(k\right)---\left(4\right)$

其中，和分别为笛卡尔坐标系中x，y和z方向上的转换后位置量测，和分别是笛卡尔坐标系中相应的位置转换量测误差，r_m(k)，β_m(k)和ε_m(k)分别是当前时刻k雷达获得的距离量测、方位角量测和俯仰角量测。

5.根据权利要求4所述的方法，其特征在于，在计算转换位置量测误差和转换多普勒量测误差的无偏一二阶矩的步骤中，利用以下的公式依次计算位置转换量测误差和转换多普勒量测误差的均值和方差

${\mathrm{\μ}}^x\left(k\right)=(e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}-e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}})r_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)---\left(5\right)$

${\mathrm{\μ}}^y\left(k\right)=(e^{\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}-e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}})r_m\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)---\left(6\right)$

${\mathrm{\μ}}^z\left(k\right)=(e^{{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}-e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2})r_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right)---\left(7\right)$

${\mathrm{\μ}}^{\mathrm{\η}}\left(k\right)={\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(8\right)$

$\begin{array}{c}{R}^{xx}\left(k\right)=\left(r_m^2\left(k\right)+{\mathrm{\σ}}_r^2\right)\left(1+e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\cos \left(2{\mathrm{\β}}_m\left(k\right)\right)\right)(1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\\ \cos \left(2{\mathrm{\ϵ}}_m\left(k\right)\right))/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2(k\left){\cos }^2{\mathrm{\β}}_m\right(k\left){\cos }^2{\mathrm{\ϵ}}_m\right(k)\end{array}---\left(9\right)$

$\begin{array}{c}{R}^{yy}\left(k\right)=\left(r_m^2\left(k\right)+{\mathrm{\σ}}_r^2\right)\left(1-e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\cos \left(2{\mathrm{\β}}_m\left(k\right)\right)\right)(1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\\ \cos \left(2{\mathrm{\ϵ}}_m\left(k\right)\right))/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2(k\left){\sin }^2{\mathrm{\β}}_m\right(k\left){\cos }^2{\mathrm{\ϵ}}_m\right(k)\end{array}---\left(10\right)$

$R^{zz}\left(k\right)=\left(r_m^2\right(k)+{\mathrm{\σ}}_r^2)(1-e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}cos(2{\mathrm{\ϵ}}_m\left(k\right)\left)\right)/2-e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\sin }^2{\mathrm{\ϵ}}_m\left(k\right)---\left(11\right)$

$\begin{array}{c}{R}^{xy}\left(k\right)=R^{yx}\left(k\right)=\left(r_m^2\left(k\right)+{\mathrm{\σ}}_r^2\right)e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2}\sin \left(2{\mathrm{\β}}_m\left(k\right)\right)\\ \left(1+e^{-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}\cos \left(2{\mathrm{\ϵ}}_m\left(k\right)\right)\right)/4-e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\β}}_m\left(k\right){\cos }^2{\mathrm{\ϵ}}_m\left(k\right)\end{array}---\left(12\right)$

$\begin{array}{c}{R}^{xz}\left(k\right)=R^{zx}\left(k\right)=e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{\mathrm{cos\β}}_m\left(k\right)\sin \left(2{\mathrm{\ϵ}}_m\left(k\right)\right)\\ \left(r_m^2\left(k\right)+{\mathrm{\σ}}_r^2\right)/2-e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{cos\β}}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\end{array}---\left(13\right)$

$\begin{array}{c}{R}^{yz}\left(k\right)=R^{zy}\left(k\right)=e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-2{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{\mathrm{sin\β}}_m\left(k\right)\sin \left(2{\mathrm{\ϵ}}_m\left(k\right)\right)\\ \left(r_m^2\left(k\right)+{\mathrm{\σ}}_r^2\right)/2-e^{-\frac{{\mathrm{\σ}}_{\mathrm{\β}}^2}{2}-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{r}_m^2\left(k\right){\mathrm{sin\β}}_m\left(k\right){\mathrm{sin\ϵ}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\end{array}---\left(14\right)$

$R^{\mathrm{\η}\mathrm{\η}}\left(k\right)=r_m^2\left(k\right){{\mathrm{\σ}}_{\stackrel{\·}{r}}}^2+{\mathrm{\σ}}_r^2{\stackrel{\·}{r}}_m^2\left(k\right)+(1+5{\mathrm{\ρ}}^2){\mathrm{\σ}}_r^2{{\mathrm{\σ}}_{\stackrel{\·}{r}}}^2+2r_m\left(k\right){\stackrel{\·}{r}}_m\left(k\right){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(15\right)$

$R^{x\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}x}\left(k\right)=e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{\mathrm{cos\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(16\right)$

$R^{y\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}y}\left(k\right)=e^{\frac{-{\mathrm{\σ}}_{\mathrm{\β}}^2-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2}{2}}{\mathrm{sin\β}}_m\left(k\right){\mathrm{cos\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(17\right)$

$R^{z\mathrm{\η}}\left(k\right)=R^{\mathrm{\η}z}\left(k\right)=e^{-{\mathrm{\σ}}_{\mathrm{\ϵ}}^2/2}{\mathrm{sin\ϵ}}_m\left(k\right)\left({\stackrel{\·}{r}}_m^2\right(k){\mathrm{\σ}}_r^2+r_m(k\left){\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}\right)---\left(18\right)$

其中，r_m(k)，β_m(k)和ε_m(k)和分别是当前时刻k雷达获得的距离量测、方位角量测、俯仰角量测和多普勒量测，σ_r，σ_β，σ_ε和分别是距离量测、方位角量测、俯仰角量测和多普勒量测的测量偏差。ρ是距离和多普勒量测之间的相关系数，R^xx(k)即指转换量测误差的方差，R^xy(k)即指转换量测误差和之间的互协方差。

6.根据权利要求5所述的方法，其特征在于，在所述笛卡尔状态信息提取步骤中，由CPMKF提取目标的笛卡尔状态信息，其迭代过程如下

${\hat{X}}_p(k+1,k)={\mathrm{\Φ}}_p{\hat{X}}_p(k,k)---\left(19\right)$

$P_p(k+1,k)={\mathrm{\Φ}}_p{P}_p(k,k){\mathrm{\Φ}}_p^T+{\mathrm{\Γ}}_{p}Q\left(k\right){\mathrm{\Γ}}_p^T---\left(20\right)$

$K_p(k+1)=P_p(k+1,k)H_p^T{\[H_p{P}_p(k+1,k)H_p^T+R^p(k+1)\]}^{-1}---\left(21\right)$

${\hat{X}}_p(k+1,k+1)={\hat{X}}_p(k+1,k)+K_p(k+1)\[Z_c^p(k+1)-H_p{\hat{X}}_p(k+1,k)\]---\left(22\right)$

P_p(k+1,k+1)＝[I-K_p(k+1)H_p]P_p(k+1,k) (23)

其中

$R^p\left(k\right)=\left[\begin{array}{ccc}{R}^{xx}\left(k\right)& R^{xy}\left(k\right)& R^{xz}\left(k\right)\\ R^{yx}\left(k\right)& R^{yy}\left(k\right)& R^{yz}\left(k\right)\\ R^{zx}\left(k\right)& R^{zy}\left(k\right)& R^{zz}\left(k\right)\end{array}\right],Z_c^p\left(k\right)=\left[\begin{array}{c}{x}_c\left(k\right)-{\mathrm{\μ}}^x\left(k\right)\\ y_c\left(k\right)-{\mathrm{\μ}}^y\left(k\right)\\ z_c\left(k\right)-{\mathrm{\μ}}^z\left(k\right)\end{array}\right]---\left(24\right)$

其中

${\mathrm{\Φ}}_p=\left[\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],{\mathrm{\Γ}}_p=\left[\begin{array}{ccc}{T}^2/2& 0& 0\\ 0& T^2/2& 0\\ 0& 0& T^2/2\\ T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right],H_p=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

7.根据权利要求6所述的方法，其特征在于，在所述伪状态信息提取步骤中，如下地构造状态空间和利用转换多普勒量测卡尔曼滤波器提取伪状态信息：

构造伪状态空间为

$\mathrm{\η}\left(k\right)=\left[\begin{array}{c}\mathrm{\η}\left(k\right)\\ \stackrel{\·}{\mathrm{\η}}\left(k\right)\end{array}\right]---\left(25\right)$

转换多普勒量测卡尔曼滤波器的迭代过程如下

$\widehat{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}\widehat{\mathrm{\η}}(k,k)+Gu\left(k\right)---\left(26\right)$

$P_{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}{P}_{\mathrm{\η}}(k,k){\mathrm{\Φ}}_{\mathrm{\η}}^T+{\mathrm{\Γ}}_x{Q}_x\left(k\right){\mathrm{\Γ}}_x^T+{\mathrm{\Γ}}_s{Q}_s\left(k\right){\mathrm{\Γ}}_s^T---\left(27\right)$

$K_{\mathrm{\η}}(k+1)=P_{\mathrm{\η}}(k+1,k)H_{\mathrm{\η}}^T{\[H_{\mathrm{\η}}{P}_{\mathrm{\η}}(k+1,k)H_{\mathrm{\η}}^T+R^{\mathrm{\η}\mathrm{\η}}(k+1)\]}^{-1}---\left(28\right)$

$\widehat{\mathrm{\η}}(k+1,k+1)=\widehat{\mathrm{\η}}(k+1,k)+K_{\mathrm{\η}}(k+1)\[Z_c^{\mathrm{\η}}(k+1)-H_{\mathrm{\η}}\widehat{\mathrm{\η}}(k+1,k)\]---\left(29\right)$

P_η(k+1,k+1)＝[I-K_η(k+1)H_η]P_η(k+1,k) (30)

其中

${\mathrm{\Γ}}_x=\left[\begin{array}{cc}T& 3T^2/2\\ 0& 2T\end{array}\right],Q_s\left(k\right)=diag\[2q^2,2q^2,2q^2\]---\left(31\right)$

${\mathrm{\Φ}}_{\mathrm{\η}}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],G={\mathrm{\Γ}}_s=\left[\begin{array}{ccc}{T}^3/2& T^3/2& T^3/2\\ T^2& T^2& T^2\end{array}\right],u\left(k\right)=E\left(\left[\begin{array}{c}{v}_x^2\left(k\right)\\ v_y^2\left(k\right)\\ v_z^2\left(k\right)\end{array}\right]\right)=\left[\begin{array}{c}q\\ q\\ q\end{array}\right]---\left(32\right)$

$\begin{array}{c}{Q}_x\left(k\right)=q\left(\left[\begin{array}{cc}{\hat{x}}^2& \hat{x}\widehat{\stackrel{\·}{x}}\\ \widehat{\stackrel{\·}{x}}\hat{x}& {\widehat{\stackrel{\·}{x}}}^2\end{array}\right]+\left[\begin{array}{cc}{\hat{y}}^2& \hat{y}\widehat{\stackrel{\·}{y}}\\ \widehat{\stackrel{\·}{y}}\hat{y}& {\widehat{\stackrel{\·}{y}}}^2\end{array}\right]+\left[\begin{array}{cc}{\hat{z}}^2& \hat{z}\widehat{\stackrel{\·}{z}}\\ \widehat{\stackrel{\·}{z}}\hat{z}& {\widehat{\stackrel{\·}{z}}}^2\end{array}\right]\right)-\\ q\left(\left[\begin{array}{cc}{P}^{xx}& P^{x\stackrel{\·}{x}}\\ P^{\stackrel{\·}{x}x}& P^{\stackrel{\·}{x}\stackrel{\·}{x}}\end{array}\right]+\left[\begin{array}{cc}{P}^{yy}& P^{y\stackrel{\·}{y}}\\ P^{\stackrel{\·}{y}y}& P^{\stackrel{\·}{y}\stackrel{\·}{y}}\end{array}\right]+\left[\begin{array}{cc}{P}^{zz}& P^{z\stackrel{\·}{z}}\\ P^{\stackrel{\·}{z}z}& P^{\stackrel{\·}{z}\stackrel{\·}{z}}\end{array}\right]\right)\end{array}---\left(33\right)$

其中T是雷达扫描周期，q是笛卡尔坐标系中各个坐标轴方向的过程高斯白噪声的方差。

8.根据权利要求7所述的方法，其特征在于，在所述的静态融合步骤中，利用如下公式进行静态融合

$\begin{array}{c}{P}_{p\mathrm{\η}}(k+1)=\[I-K_p(k+1)H_p\]{\mathrm{\Φ}}_p{P}_{p\mathrm{\η}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\η}}^T{\[I-K_{\mathrm{\η}}(k+1)H_{\mathrm{\η}}\]}^T+\[I-K_p(k+1)H_p\]\\ {\mathrm{\Γ}}_{p}Q\left(k\right){\left({\mathrm{\Γ}}_x{X}_{\mathrm{\Γ}}\right)}^T{\[I-K_{\mathrm{\η}}\left(k+1\right)H_{\mathrm{\η}}\]}^T+K_p\left(k+1\right)R^{p\mathrm{\η}}\left(k+1\right)K_{\mathrm{\η}}{\left(k+1\right)}^T\end{array}---\left(34\right)$

其中

$P_{XZ}=P_p{\stackrel{\·}{C}}^T-P_{p\mathrm{\η}}---\left(35\right)$

其中C是伪状态与目标状态之间的函数关系，定义如下

$\mathrm{\η}\left(k\right)=\left[\begin{array}{c}\mathrm{\η}\left(k\right)\\ \stackrel{\·}{\mathrm{\η}}\left(k\right)\end{array}\right]=C\[X\left(k\right)\]=\left[\begin{array}{c}x\left(k\right)\stackrel{\·}{x}\left(k\right)+y\left(k\right)\stackrel{\·}{y}\left(k\right)+z\left(k\right)\stackrel{\·}{z}\left(k\right)\\ {\stackrel{\·}{x}}^2\left(k\right)+{\stackrel{\·}{y}}^2\left(k\right)+{\stackrel{\·}{z}}^2\left(k\right)\end{array}\right]---\left(36\right)$

是函数C的Jacobin矩阵，

$P_{ZZ}=\stackrel{\·}{C}{P}_p{\stackrel{\·}{C}}^T+P_{\mathrm{\η}}+\frac{1}{2}\underset{i=1}{\overset{n_{\mathrm{\η}}}{\Σ}}\underset{j=1}{\overset{n_{\mathrm{\η}}}{\Σ}}{e}_i{e}_j^{T}tr\left({\stackrel{\·\·}{C}}^i{P}_p{\stackrel{\·\·}{C}}^j{P}_p\right)-\stackrel{\·}{C}{P}_{p\mathrm{\η}}-{\left(\stackrel{\·}{C}{P}_{p\mathrm{\η}}\right)}^T---\left(37\right)$

其中，e_i是笛卡尔坐标系中第i个n_η维偏置单位向量，是函数C的Jacobin矩阵，为函数C的第i个分量的Hessian矩阵。

然后计算目标的最终状态和状态估计方差

$\hat{X}={\hat{X}}_p+P_{XZ}{\left(P_{ZZ}\right)}^{-1}(\widehat{\mathrm{\η}}-\stackrel{\‾}{Z})---\left(38\right)$

P＝P_p-P_XZ(P_ZZ)^-1(P_XZ)^T (39)

其中