CN105785358A - 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法 - Google Patents

Abstract

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

Description

一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法

技术领域

本发明涉及雷达目标跟踪，尤其涉及方向余弦坐标系中的雷达目标跟踪。

背景技术

雷达目标跟踪就是根据目标当前状态和雷达量测，对目标运动状态进行估计和预测。在目标跟踪领域，雷达提供的量测除了位置(距离和角度)外，还有多普勒信息，理论与实践已经证明，充分利用多普勒量测信息可以有效地提高目标跟踪精度。

由于传统机械扫描雷达受限于扫描速度，而相控阵雷达中的方向图和扫描特征又可以很方便的在方向余弦坐标系(COS)下表示，相控阵雷达的应用越来越广泛。但相控阵雷达COS坐标系下目标跟踪的研究相对较少，考虑多普勒量测的跟踪算法就更少了。现有文献中Y.Kosuge,H.IwamaandY.Miyazaki,“Atrackingfilterforphasedarrayradarwithrangeratemeasurement,”Proceedingsof1991InternationalConferenceonIndustrialElectronics,ControlandInstrumentation,pp.2555-2560,1991直接利用扩展卡尔曼滤波器(EKF)对COS坐标系下的位置量测和多普勒量测进行跟踪滤波，但由于多普勒量测的强非线性特点，加之COS坐标系下第三个方向余弦是前两个方向余弦的强非线性函数，在大的量测误差情况下，EKF的近似误差在滤波迭代中容易累计扩大，从而使滤波器性能恶化，会出现精度下降和滤波发散问题。文献B.Zhang,H.QuandS.Li,“Anewmethodfortargettrackingwithdebiasedconsisitentconvertedmeasurementsindirectioncosines,”ChineseJournalofElectronics,Vol.19,no.3,pp.538-542,2010利用转换量测的方法对方向余弦坐标下的目标进行跟踪，但没有考虑包含目标速度信息的多普勒量测，属于次优的方案。

发明内容

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

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

根据本发明的一些实施方式，针将多普勒量测引入方向余弦坐标系下进行目标跟踪，在静态融合滤波器的框架下，推导了方向余弦坐标系下的量测转换和量测转换误差的无偏一二阶矩，并同时利用转换多普勒量测卡尔曼滤波器(CDMKF)和转换位置量测卡尔曼滤波器(CPMKF)线性的提取多普勒信息和目标笛卡尔状态信息，然后联合两者的输出在最小均方误差(MMSE)准则下估计出目标最终状态，从而能够提高目标跟踪的精度。

附图说明

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

图1示出了方向余弦坐标系和直角坐标系的关系。

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

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

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

具体实施方式

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

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

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

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

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

图1示出了方向余弦坐标系和直角坐标系的关系。如图1所示，量测方程可表示为

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

其中

$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(44\right)$

$r\left(k\right)=\sqrt{x^2\left(k\right)+y^2\left(k\right)+z^2\left(k\right)},\mathrm{\α}\left(k\right)=x\left(k\right)/r\left(k\right),\mathrm{\β}\left(k\right)=y\left(k\right)/r\left(k\right)---\left(45\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(46\right)$

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

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

图2示出了一种本发明一种实施方式的带多普勒量测的雷达目标跟踪方法的示例流程。如图2所示，首先在步骤一，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(47\right)$

其中是直角坐标系中伪量测η_c(k)的转换误差。伪量测的真值为转换多普勒。

下标m是measurement的首字母，表明是测量值；下标c是convert的首字母，表明是转换值，在方向余弦和直角坐标系中，伪量测的数学形式一样，为了统一后面转换量测(位置、多普勒)卡尔曼滤波器的数学形式，统一用c表明转换量测值；η(k)是转换多普勒，是伪量测对应的真值。

然后在步骤二S102进行量测转换，将方向余弦坐标系下的量测转换到直角坐标系下。在一种实施方式中，可以如下进行

$x_c\left(k\right)=r_m\left(k\right){\mathrm{\α}}_m\left(k\right)=x\left(k\right)+\tilde{x}\left(k\right)---\left(48\right)$

$y_c\left(k\right)=r_m\left(k\right){\mathrm{\β}}_m\left(k\right)=y\left(k\right)+\tilde{y}\left(k\right)---\left(49\right)$

$z_c\left(k\right)=r_m\left(k\right){\mathrm{\γ}}_m\left(k\right)=z\left(k\right)+\tilde{z}\left(k\right)---\left(50\right)$

其中，x_c(k)，y_c(k)和z_c(k)分别为直角坐标系中x，y和z方向上的转换位置量测，和分别是直角坐标系中相应的转换位置量测误差，r_m(k)，α_m(k)和β_m(k)分别是当前时刻k雷达获得的距离量测和两个方向余弦量测，其中第三个方向余弦γ_m(k)为

${\mathrm{\γ}}_m\left(k\right)=\sqrt{1-{\mathrm{\α}}_m^2\left(k\right)-{\mathrm{\β}}_m^2\left(k\right)}---\left(51\right)$

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

${\mathrm{\μ}}^x\left(k\right)=0,{\mathrm{\μ}}^y\left(k\right)=0,{\mathrm{\μ}}^z\left(k\right)=r_{m}E\[\widetilde{\mathrm{\γ}}\left(k\right)\],{\mathrm{\μ}}^{\mathrm{\η}}\left(k\right)=-{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(52\right)$

$R^{xx}={\mathrm{\α}}_m^2{\mathrm{\σ}}_r^2+r_m^2{\mathrm{\σ}}_{\mathrm{\α}}^2+{\mathrm{\σ}}_r^2{\mathrm{\σ}}_{\mathrm{\α}}^2---\left(53\right)$

$R^{yy}={\mathrm{\β}}_m^2{\mathrm{\σ}}_r^2+r_m^2{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_r^2{\mathrm{\σ}}_{\mathrm{\β}}^2---\left(54\right)$

$R^{zz}={\mathrm{\γ}}_m^2{\mathrm{\σ}}_r^2+(2r_m^2+{\mathrm{\σ}}_r^2)E\left({\widetilde{\mathrm{\γ}}}^2\right)-2{\mathrm{\σ}}_r^2{\mathrm{\γ}}_{m}E\left(\widetilde{\mathrm{\γ}}\right)-2r_m^2{E}^2\left(\widetilde{\mathrm{\γ}}\right)---\left(55\right)$

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

$R^{xy}=R^{yx}={\mathrm{\σ}}_r^2{\mathrm{\α}}_m{\mathrm{\β}}_m---\left(57\right)$

$R^{xz}=R^{zx}={\mathrm{\σ}}_r^2{\mathrm{\α}}_m({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)+(r_m^2+{\mathrm{\σ}}_r^2)E\left(\widetilde{\mathrm{\α}}\widetilde{\mathrm{\γ}}\right)---\left(58\right)$

$R^{yz}=R^{zy}={\mathrm{\σ}}_r^2{\mathrm{\β}}_m({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)+(r_m^2+{\mathrm{\σ}}_r^2)E\left(\widetilde{\mathrm{\β}}\widetilde{\mathrm{\γ}}\right)---\left(59\right)$

$R^{x\mathrm{\η}}=R^{\mathrm{\η}x}={\stackrel{\·}{r}}_m{\mathrm{\α}}_m{\mathrm{\σ}}_r^2+r_m{\mathrm{\α}}_m{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(60\right)$

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

$R^{z\mathrm{\η}}=R^{\mathrm{\η}z}={\stackrel{\·}{r}}_m{\mathrm{\γ}}_m{\mathrm{\σ}}_r^2+r_m{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)-{\stackrel{\·}{r}}_m{\mathrm{\σ}}_{\stackrel{\·}{r}}^{2}E\left(\widetilde{\mathrm{\γ}}\right)---\left(62\right)$

其中

$E\[\widetilde{\mathrm{\γ}}\left(k\right)\]=-\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}{\mathrm{\σ}}_{\mathrm{\α}}^2-\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}{\mathrm{\σ}}_{\mathrm{\β}}^2,E\left(\widetilde{\mathrm{\α}}\widetilde{\mathrm{\γ}}\right)={\mathrm{\γ}}_{\mathrm{\α}}^{\′}{\mathrm{\σ}}_{\mathrm{\α}}^2,E\left(\widetilde{\mathrm{\β}}\widetilde{\mathrm{\γ}}\right)={\mathrm{\γ}}_{\mathrm{\β}}^{\′}{\mathrm{\σ}}_{\mathrm{\β}}^2---\left(63\right)$

$E\left({\widetilde{\mathrm{\γ}}}^2\right)={\left({\mathrm{\γ}}_{\mathrm{\α}}^{\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\α}}^2+{\left({\mathrm{\γ}}_{\mathrm{\β}}^{\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\β}}^2+\frac{3}{4}{\left({\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\α}}^4+\frac{3}{4}{\left({\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\β}}^4+{\mathrm{\σ}}_{\mathrm{\α}}^2{\mathrm{\σ}}_{\mathrm{\β}}^2\[{\left({\mathrm{\γ}}_{\mathrm{\α}\mathrm{\β}}^{\′\′}\right)}^2+\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}\]---\left(64\right)$

${\mathrm{\γ}}_{\mathrm{\α}}^{\′}=-\frac{{\mathrm{\α}}_m\left(k\right)}{{\mathrm{\γ}}_m\left(k\right)},{\mathrm{\γ}}_{\mathrm{\β}}^{\′}=-\frac{{\mathrm{\β}}_m\left(k\right)}{{\mathrm{\γ}}_m\left(k\right)}---\left(65\right)$

${\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}=-\frac{1-{\mathrm{\β}}_m^2\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)},{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}=-\frac{1-{\mathrm{\α}}_m^2\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)},{\mathrm{\γ}}_{\mathrm{\α}\mathrm{\β}}^{\′\′}=-\frac{{\mathrm{\α}}_m\left(k\right){\mathrm{\β}}_m\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)}---\left(66\right)$

其中，r_m(k)，α_m(k)，β_m(k)和分别是当前时刻k雷达获得的距离量测、两个方向余弦量测和多普勒量测，σ_r，σ_α，σ_β和分别是距离量测、两个方向余弦和多普勒量测的测量偏差。ρ是距离和多普勒量测之间的相关系数。γ_m(k)同步骤二S102。

步骤四S104：提取目标的笛卡尔状态信息，在一种实施方式中，利用CPMKF来进行提取，其迭代过程如下

${\hat{X}}_p(k+1,k)={\mathrm{\Φ}}_p{\hat{X}}_p(k,k)---\left(67\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(68\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(69\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(70\right)$

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

其中

$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(72\right)$

该部分输出的笛卡尔状态信息即为和

$P_p\left(k,k\right)=\left[\begin{array}{cccccc}{P}^{xx}& P^{xy}& P^{xz}& P^{x\stackrel{\·}{x}}& P^{x\stackrel{\·}{y}}& P^{x\stackrel{\·}{z}}\\ P^{yx}& P^{yy}& P^{yz}& P^{y\stackrel{\·}{x}}& P^{y\stackrel{\·}{y}}& P^{y\stackrel{\·}{z}}\\ P^{zx}& P^{zy}& P^{zz}& P^{z\stackrel{\·}{x}}& P^{z\stackrel{\·}{y}}& P^{z\stackrel{\·}{z}}\\ P^{\stackrel{\·}{x}x}& P^{\stackrel{\·}{x}y}& P^{\stackrel{\·}{x}z}& P^{\stackrel{\·}{x}\stackrel{\·}{x}}& P^{\stackrel{\·}{x}\stackrel{\·}{y}}& P^{\stackrel{\·}{x}\stackrel{\·}{z}}\\ P^{\stackrel{\·}{y}x}& P^{\stackrel{\·}{y}y}& P^{\stackrel{\·}{y}z}& P^{\stackrel{\·}{y}\stackrel{\·}{x}}& P^{\stackrel{\·}{y}\stackrel{\·}{y}}& P^{\stackrel{\·}{y}\stackrel{\·}{z}}\\ P^{\stackrel{\·}{z}x}& P^{\stackrel{\·}{z}y}& P^{\stackrel{\·}{z}z}& P^{\stackrel{\·}{z}\stackrel{\·}{x}}& P^{\stackrel{\·}{z}\stackrel{\·}{y}}& P^{\stackrel{\·}{z}\stackrel{\·}{z}}\end{array}\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(73\right)$

CDMKF的迭代过程如下

$\widehat{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}\widehat{\mathrm{\η}}(k,k)+Gu\left(k\right)---\left(74\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(75\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(76\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(77\right)$

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

其中

${\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(79\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(80\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(81\right)$

其中T是雷达扫描周期，q是直角坐标系中各个坐标轴方向的过程高斯白噪声的方差，式(34)中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{\η}}\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(82\right)$

其中

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

$P_{XZ}=P_p{\stackrel{\·}{C}}^T-P_{p\mathrm{\η}}---\left(83\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(84\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(85\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(86\right)$

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

其中

本发明的一些实施方式相对于一些其他方法的优势在于，将COS坐标系下的动态非线性估计问题转换成一个动态线性估计问题和一个静态非线性融合问题，避免了利用非线性滤波方法EKF直接同时处理位置量测和多普勒量测可能出现的精度下降和滤波发散问题，从而精确稳健的估计COS坐标系下的运动目标状态。

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

仿真情况设定相控阵雷达位于坐标原点，以1s的扫描间隔给出目标的斜距、两个方向余弦和多普勒量测信息，量测的标准差分别为σ_r＝1000m，σ_α＝σ_β＝0.01和过程噪声方差为q＝0.01m/s²，目标做匀速运动，初始位置为(30km,30km,30km)，初始速度为(20m/s,20m/s,20m/s)。评价指标为位置、速度的相对均方根误差(RRMSE)，其定义为

对上述条件做500次蒙特卡洛实验的100次跟踪扫描Monte-Carlo仿真结果如图3和图4所示。

从仿真结果可以看出，带多普勒量测的三种滤波器(SEKF、UKF和SF-CMKFcos)的RMSE明显小于不带多普勒量测的CPMKF滤波器，这说明多普勒量测的引入，可以显著改善跟踪滤波器的性能；而SF-CMKFcos性能最优。这是因为大的量测误差导致了大的非线性近似误差，并且通过SEKF和UKF的动态非线性迭代引起了滤波器性能恶化。而SF-CMKFcos中采用两个线性最优滤波器(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)的转换误差，r_m(k)和分别为径向距离和多普勒量测，下标m表明是测量值；下标c表明是转换值，η(k)是转换多普勒，是伪量测对应的真值。

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

$x_c\left(k\right)=r_m\left(k\right){\mathrm{\α}}_m\left(k\right)=x\left(k\right)+\tilde{x}\left(k\right)---\left(2\right)$

$y_c\left(k\right)=r_m\left(k\right){\mathrm{\β}}_m\left(k\right)=y\left(k\right)+\tilde{y}\left(k\right)---\left(3\right)$

$z_c\left(k\right)=r_m\left(k\right){\mathrm{\γ}}_m\left(k\right)=z\left(k\right)+\tilde{z}\left(k\right)---\left(4\right)$

其中，r_m(k)，α_m(k)，β_m(k)和分别为径向距离、两个方向余弦和多普勒量测，x_c(k)，y_c(k)和z_c(k)分别为直角坐标系中x，y和z方向上的转换后的位置量测，和分别是直角坐标系中相应的转换位置量测误差，r_m(k)，α_m(k)和β_m(k)分别是当前时刻k雷达获得的距离量测和两个方向余弦量测，其中第三个方向余弦γ_m(k)为

${\mathrm{\γ}}_m\left(k\right)=\sqrt{1-{\mathrm{\α}}_m^2\left(k\right)-{\mathrm{\β}}_m^2\left(k\right)}---\left(5\right)$

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

其中转换位置量测误差和转换多普勒量测误差的均值和方差依次为

${\mathrm{\μ}}^x\left(k\right)=0,{\mathrm{\μ}}^y\left(k\right)=0,{\mathrm{\μ}}^z\left(k\right)=r_{m}E\[\widetilde{\mathrm{\γ}}\left(k\right)\],{\mathrm{\μ}}^{\mathrm{\η}}\left(k\right)=-{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(6\right)$

$R^{xx}={\mathrm{\α}}_m^2{\mathrm{\σ}}_r^2+r_m^2{\mathrm{\σ}}_{\mathrm{\α}}^2+{\mathrm{\σ}}_r^2{\mathrm{\σ}}_{\mathrm{\α}}^2---\left(7\right)$

$R^{yy}={\mathrm{\β}}_m^2{\mathrm{\σ}}_r^2+r_m^2{\mathrm{\σ}}_{\mathrm{\β}}^2+{\mathrm{\σ}}_r^2{\mathrm{\σ}}_{\mathrm{\β}}^2---\left(8\right)$

$R^{zz}={\mathrm{\γ}}_m^2{\mathrm{\σ}}_r^2+(2r_m^2+{\mathrm{\σ}}_r^2)E\left({\widetilde{\mathrm{\γ}}}^2\right)-2{\mathrm{\σ}}_r^2{\mathrm{\γ}}_{m}E\left(\widetilde{\mathrm{\γ}}\right)-2r_m^2{E}^2\left(\widetilde{\mathrm{\γ}}\right)---\left(9\right)$

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

$R^{xy}=R^{yx}={\mathrm{\σ}}_r^2{\mathrm{\α}}_m{\mathrm{\β}}_m---\left(11\right)$

$R^{xz}=R^{zx}={\mathrm{\σ}}_r^2{\mathrm{\α}}_m({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)+(r_m^2+{\mathrm{\σ}}_r^2)E\left(\widetilde{\mathrm{\α}}\widetilde{\mathrm{\γ}}\right)---\left(12\right)$

$R^{yz}=R^{zy}={\mathrm{\σ}}_r^2{\mathrm{\β}}_m({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)+(r_m^2+{\mathrm{\σ}}_r^2)E\left(\widetilde{\mathrm{\β}}\widetilde{\mathrm{\γ}}\right)---\left(13\right)$

$R^{x\mathrm{\η}}=R^{\mathrm{\η}x}={\stackrel{\·}{r}}_m{\mathrm{\α}}_m{\mathrm{\σ}}_r^2+r_m{\mathrm{\α}}_m{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}---\left(14\right)$

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

$R^{z\mathrm{\η}}=R^{\mathrm{\η}z}={\stackrel{\·}{r}}_m{\mathrm{\γ}}_m{\mathrm{\σ}}_r^2+r_m{\mathrm{\ρ\σ}}_r{\mathrm{\σ}}_{\stackrel{\·}{r}}({\mathrm{\γ}}_m-E(\widetilde{\mathrm{\γ}}\left)\right)-{\stackrel{\·}{r}}_m{\mathrm{\σ}}_{\stackrel{\·}{r}}^{2}E\left(\widetilde{\mathrm{\γ}}\right)---\left(16\right)$

其中

$E\[\widetilde{\mathrm{\γ}}\left(k\right)\]=-\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}{\mathrm{\σ}}_{\mathrm{\α}}^2-\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}{\mathrm{\σ}}_{\mathrm{\β}}^2,E\left(\widetilde{\mathrm{\α}}\widetilde{\mathrm{\γ}}\right)={\mathrm{\γ}}_{\mathrm{\α}}^{\′}{\mathrm{\σ}}_{\mathrm{\α}}^2,E\left(\widetilde{\mathrm{\β}}\widetilde{\mathrm{\γ}}\right)={\mathrm{\γ}}_{\mathrm{\β}}^{\′}{\mathrm{\σ}}_{\mathrm{\β}}^2---\left(17\right)$

$E\left({\widetilde{\mathrm{\γ}}}^2\right)={\left({\mathrm{\γ}}_{\mathrm{\α}}^{\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\α}}^2+{\left({\mathrm{\γ}}_{\mathrm{\β}}^{\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\β}}^2+\frac{3}{4}{\left({\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\α}}^4+\frac{3}{4}{\left({\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}\right)}^2{\mathrm{\σ}}_{\mathrm{\β}}^4+{\mathrm{\σ}}_{\mathrm{\α}}^2{\mathrm{\σ}}_{\mathrm{\β}}^2\[{\left({\mathrm{\γ}}_{\mathrm{\α}\mathrm{\β}}^{\′\′}\right)}^2+\frac{1}{2}{\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}\]---\left(18\right)$

${\mathrm{\γ}}_{\mathrm{\α}}^{\′}=-\frac{{\mathrm{\α}}_m\left(k\right)}{{\mathrm{\γ}}_m\left(k\right)},{\mathrm{\γ}}_{\mathrm{\β}}^{\′}=-\frac{{\mathrm{\β}}_m\left(k\right)}{{\mathrm{\γ}}_m\left(k\right)}---\left(19\right)$

${\mathrm{\γ}}_{\mathrm{\α}}^{\′\′}=-\frac{1-{\mathrm{\β}}_m^2\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)},{\mathrm{\γ}}_{\mathrm{\β}}^{\′\′}=-\frac{1-{\mathrm{\α}}_m^2\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)},{\mathrm{\γ}}_{\mathrm{\α}\mathrm{\β}}^{\′\′}=-\frac{{\mathrm{\α}}_m\left(k\right){\mathrm{\β}}_m\left(k\right)}{{\mathrm{\γ}}_m^3\left(k\right)}---\left(20\right)$

其中，σ_r，σ_α，σ_β和分别是距离量测、两个方向余弦和多普勒量测的测量偏差。ρ是距离和多普勒量测之间的相关系数。

6.根据权利要求5所述的方法，其特征在于，在所述笛卡尔状态信息提取步骤中，由转换位置量测卡尔曼滤波器提取目标的笛卡尔状态信息，其迭代过程如下

${\hat{X}}_p(k+1,k)={\mathrm{\Φ}}_p{\hat{X}}_p(k,k)---\left(21\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(22\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(23\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(24\right)$

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

其中

$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(26\right)$

7.根据权利要求6所述的方法，其特征在于，在所述伪状态空间构造步骤，构造伪状态空间为

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

其中，利用转换多普勒量测卡尔曼滤波器的迭代提取伪状态信息，过程如下

$\widehat{\mathrm{\η}}(k+1,k)={\mathrm{\Φ}}_{\mathrm{\η}}\widehat{\mathrm{\η}}(k,k)+Gu\left(k\right)---\left(28\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(29\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(30\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(31\right)$

其中

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

${\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(33\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(34\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(35\right)$

其中T是雷达扫描周期，q是直角坐标系中各个坐标轴方向的过程高斯白噪声的方差，式(34)中P_p(k,k)是由笛卡尔状态信息提取步骤提取的笛卡尔状态信息。

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

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{\η}}\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(36\right)$

其中

2)再根据以下公式进行计算

$P_{XZ}=P_p{\stackrel{\·}{C}}^T-P_{p\mathrm{\η}}---\left(37\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(38\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(39\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(40\right)$

$P=P_p-P_{XZ}{\left(P_{ZZ}\right)}^{-1}{\left(P_{XZ}\right)}^T---\left(41\right)$

其中