一种具有图像确认的多目标跟踪方法

摘要

本发明公开了一种具有图像确认的多目标跟踪方法，用于解决现有的多目标跟踪方法精度差的技术问题。技术方案：首先由序列图像确定所跟踪目标的回波，剔除了不是当前所跟踪目标的回波和杂波；再由所确定的回波更新所跟踪目标的测量新计算，减少了多目标跟踪的测量更新误差，提高了下一时间节拍的预测精度；再根据给出的状态估计协方差阵的计算，在图像直接确定目标时，多目标跟踪的精度达到单目标雷达跟踪的精度：方位角和高低角约为1毫弧度，距离误差小于10米；当图像去掉部分杂波时，由于杂波数目减少，多目标跟踪的精度得到了显著提高。

说明书

技术领域

本发明涉及一种多目标跟踪方法，特别是涉及一种具有图像确认的多目标跟踪方法。

背景技术

多目标跟踪技术在军用及民用领域均有广泛的应用，可用于空中目标检测、跟踪 与攻击，空中导弹防御，空中交通管制，港口和海洋监视等。近些年来，随着战场环 境的改变，对抗和反对抗技术的发展，产生了背景强杂波、低信噪比、低检测概率和 高虚警率等一系列问题，对多目标跟踪方法的精度和准确性提出了更高的要求。

多目标跟踪的目的是将探测器所接收到的量测对应不同的信息源，形成不同观测 集合或轨迹，根据轨迹估计被跟踪目标的数目以及每一目标的运动参数，实现对多个 目标的跟踪。用于多目标状态估计的基本滤波方法有α-β滤波、α-β-γ滤波、卡尔曼滤 波、扩展卡尔曼滤波、高斯和近似、最优非线性滤波、粒子滤波和自适应滤波等。α-β 和α-β-γ滤波器由于结构简单，计算量小，在早期计算机资源短缺时应用很广。卡尔 曼滤波是多目标跟踪的一种基本方法，但是需要知道系统的精确数学模型，并且只适 用于线性系统，限制了算法的应用。扩展卡尔曼滤波将卡尔曼滤波理论扩展到非线性 领域，用一个高斯分布来近似状态的条件概率分布；而当近似条件不满足时，高斯和 滤波器则用一个高斯分布的加权和来近似状态的条件概率分布。最优非线性滤波使用 Makov转移概率来描述目标的动力学过程，具有很好的特性，但是计算量较大，因此 一直没有得到广泛应用。粒子滤波采用随机采样，由于计算量太大和粒子退化问题， 不适合实际应用。为了改进粒子滤波，无迹卡尔曼滤波采用确定性采样，使得采样的 粒子点个数减少，避免了粒子滤波中的粒子点退化问题，因此其应用领域很广。自适 应滤波方法通过对目标机动的检测，实时调整滤波器参数或增加滤波器的状态，使滤 波器实时适应目标运动，特别适合对机动目标的跟踪。

在多目标跟踪问题中，文献“史忠科，Kalman滤波新结构及其在目标跟踪中的应 用.自动化学报，1994，Vol.20，No.5，pp.605-609”公开了一种单目标的离散化模型

x(k+1)＝Ф(k+1，k)x(k)+Λ(k)w(k)，

式中，$x={\left(\begin{array}{ccccccccc}x& \stackrel{·}{x}& \stackrel{··}{x}& y& \stackrel{·}{y}& \stackrel{··}{y}& z& \stackrel{·}{z}& \stackrel{··}{z}\end{array}\right)}^T$为状态向量，(x，y，z)为目标在地面参考直角坐 标系下的位置坐标；

w(k)为状态噪声向量，

Ф(k+1，k)＝diag[Ф₁，Ф₁，Ф₁]为状态转移矩阵，

$\Lambda \left(k\right)={\int }_{\mathrm{kT}}^{(k+1)T}\Phi (k+1,\tau )\Gamma \left(\tau \right)\mathrm{d\tau }={\left(\begin{array}{ccc}{\Lambda }_1^T& {\Lambda }_1^T& {\Lambda }_1^T\end{array}\right)}^T,$

Γ(t)为系统噪声的系数矩阵，$\Gamma ={\left(\begin{array}{ccc}{\Gamma }_1^T& {\Gamma }_1^T& {\Gamma }_1^T\end{array}\right)}^T,$Γ₁＝[0 0 1]^T，

${\Phi }_1=\left(\begin{array}{ccc}1& T& \frac{1}{2}{T}^2\\ 0& 1& T\\ 0& 0& 1\end{array}\right),$${\Lambda }_1=\left(\begin{array}{ccc}\frac{1}{6}{T}^3& \frac{1}{2}{T}^2& T\end{array}\right),$T为采样周期，

观测方程为

y(k)＝H(k)x(k)+v(k)，

式中，y(k)为观测向量，H(k)为观测矩阵，v(k)为观测噪声向量， 跟踪估计方法为：

$x_i(k/k)=x_i(k/k-1)+G_i\left(k\right)\{\underset{j=1}{\overset{m}{\Sigma }}{\lambda }_{\mathrm{ij}}\left(k\right)z_i\left(k\right)-g_i[x_i(k/k-1)\left]\right\}$

$P_i(k/k)=P_i(k/k-1)-P_i(k/k-1)H_i^T\left(k\right)S_i^{-1}\left(k\right)H_i\left(k\right)P_i(k/k-1)+$

$G_i\left(k\right)[\underset{j=1}{\overset{m}{\Sigma }}{\lambda }_{i,j}\left(k\right){\Delta }_{i,j}\left(k\right){\Delta }_{i,j}^T\left(k\right)-{\Delta }_i\left(k\right){\Delta }_i^T\left(k\right)]G_i^T\left(k\right)$

$S_i\left(k\right)=H_i\left(k\right)P_i(k/k-1)H_i^T\left(k\right)+R_i\left(k\right)$

式中，下标i代表第i个目标的状态或测量值，

x_i(k/k)为第i个目标kT时刻状态的滤波值，

x_i(k/k-1)为第i个目标kT时刻状态的一步预测值，

λ_ij(k)为权系数，

g_i为观测向量，

z_i为g_i的实际测量值，

P_i(k/k)为状态估计误差的协方差阵，

S_i(k)为新息向量的方差阵，

R_i(k)为观测误差的方差阵，

定义Δ_i，j(k)为第j个候选回波新息向量，且

Δ_i，j(k)＝z_i，j(k)-g_i[x_i(k/k-1)]，

而Δ_i(k)为Δ_i，j(k)的加权和，即

${\Delta }_i\left(k\right)=\underset{j=1}{\overset{m}{\Sigma }}{\lambda }_{i,j}\left(k\right){\Delta }_{i,j}\left(k\right),$

多目标跟踪技术大致可分为目标状态估计和数据关联两个主要方面。一般单目标 跟踪的精度为：方位角和高低角约为1毫弧度，距离误差小于10米。而多目标跟踪时， 在目标状态估计的新息计算中，所考虑的候选回波不仅来自被跟踪的多个目标，而且 还来自不在跟踪之列的其他目标、杂波、虚警以及射频干扰等，这些与跟踪目标无关 的虚假回波会造成新息的不确定或错误，使得跟踪时误差成倍增加，一般在两倍以上， 不仅影响了跟踪精度，导致下一时间节拍的预测误差增大，误差的不断积累又会导致 目标丢失。

发明内容

为了克服现有的多目标跟踪方法精度差的不足，本发明提供一种具有图像确认的 多目标跟踪方法，该方法在多目标跟踪的测量更新中，通过序列图像确定的有关参数 确认回波，分三种情况断定是否为某个跟踪的目标，直接剔除与跟踪目标无关的回波， 从而减少回波个数，可以提高多目标跟踪的精度。

本发明解决其技术问题所采用的技术方案：一种具有图像确认的多目标跟踪方 法，其特点是包括下述步骤：

(1)根据所跟踪第i个目标的距离、高低角、方位角、速度、回波信息，高速获 取序列图像，通过序列图像处理获得目标的距离、高低角、方位角、速度、回波信息， 计算

e＝k_v(v_i-v_im)²+k_α(α_i-α_im)²+k_β(β_i-β_im)²+k_r(r_i-r_im)²

如果e＞e_d，则该回波不是当前所跟踪的目标；

式中，v_i是当前跟踪目标的速度，α_i是当前跟踪目标的高低角，β_i是当前跟踪目标的 方位角，r_i是当前跟踪目标斜距，v_im是序列图像确定的目标速度，α_im是序列图像确定 的目标高低角，β_im是序列图像确定的目标方位角，r_im是序列图像确定的目标斜距， k_v，k_α，k_β，k_r为设定的参数；e_d是设定阈值；

(2)所跟踪第i个目标的测量新计算为

式中，x_i为9维状态向量，$x_i={\left(\begin{array}{ccccccccc}{x}_i& {\stackrel{·}{x}}_i& {\stackrel{··}{x}}_i& y_i& {\stackrel{·}{y}}_i& {\stackrel{··}{y}}_i& z_i& {\stackrel{·}{z}}_i& {\stackrel{··}{z}}_i\end{array}\right)}^T,$

g_i是3、6或9维观测向量，如果雷达只能测量斜距r_i、高低角α_i、方位角β_i时， g_i＝[r_iα_iβ_i]^T；如果雷达能够测量到速度项，则$g_i={\left(\begin{array}{cccccc}{r}_i& {\stackrel{·}{r}}_i& {\alpha }_i& {\stackrel{·}{\alpha }}_i& {\beta }_i& {\stackrel{·}{\beta }}_i\end{array}\right)}^T;$如果 雷达能够测量到加速度项，则$g_i={\left(\begin{array}{ccccccccc}{r}_i& {\stackrel{·}{r}}_i& {\stackrel{··}{r}}_i& {\alpha }_i& {\stackrel{·}{\alpha }}_i& {\stackrel{··}{\alpha }}_i& {\beta }_i& {\stackrel{·}{\beta }}_i& {\stackrel{··}{\beta }}_i\end{array}\right)}^T;$

式中，下标i代表第i个目标的状态或测量值，

x_i(k/k)为第i个目标kT时刻状态的滤波值，

x_i(k/k-1)为第i个目标kT时刻状态的一步预测值，

λ_ij(k)为权系数，

g_i为观测向量，

z_i为g_i的实际测量值，

P_i(k/k)为状态估计误差的协方差阵，

S_i(k)为新息向量的方差阵，

R_i(k)为观测误差的方差阵，

定义Δ_i，j(k)为第j个候选回波新息向量，且

Δ_i，j(k)＝z_i，j(k)-g_i[x_i(k/k-1)]，

而Δ_i(k)为Δ_i，j(k)的加权和，即

${\Delta }_i\left(k\right)=\underset{j=1}{\overset{m}{\Sigma }}{\lambda }_{i,j}\left(k\right){\Delta }_{i,j}\left(k\right),$

系数矩阵

$p_1=\frac{x_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_2=\frac{y_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_3=\frac{z_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_4=\frac{-x_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$p_5=\frac{-y_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$p_6=\frac{\sqrt{x_i^2+y_i^2}}{(x_i^2+y_i^2+z_i^2)}$

$p_7=\frac{y_i}{x_i^2+y_i^2}$

$p_8=\frac{-x_i}{x_i^2+y_i^2}$

$h_{21}=\frac{{\stackrel{·}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{x_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{24}=\frac{{\stackrel{·}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{y_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{27}=\frac{{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{z_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{31}=\frac{{\stackrel{··}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{x_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{x}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3x}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{32}=\frac{{2\stackrel{·}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2x}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{34}=\frac{{\stackrel{··}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{y_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{y}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3y}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{35}=\frac{{2\stackrel{·}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2y}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{37}=\frac{{\stackrel{··}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{z_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{z}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3z}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{38}=\frac{{2\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2z}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{51}=\frac{2x_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{x_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{54}=\frac{2y_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{57}=-\frac{x_i{\stackrel{·}{x}}_i+{y_i\stackrel{·}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$h_{61}=\frac{{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{··}{x}}_i{z}_i+2x_i{\stackrel{··}{z}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}+(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)$

$[\frac{x_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{62}=\frac{x_i{\stackrel{·}{z}}_i+{2\stackrel{·}{x}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{63}=\frac{{-y}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{64}=\frac{{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-{\stackrel{··}{y}}_i{z}_i+2y_i{\stackrel{··}{z}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}+(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)$

$[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{65}=\frac{y_i{\stackrel{·}{z}}_i-{2\stackrel{·}{y}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$+y_i{z}_i[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{66}=\frac{{-x}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{67}=\frac{-{\stackrel{·}{x}}_i^2-x_i{\stackrel{··}{x}}_i-{\stackrel{·}{y}}_i^2-y_i{\stackrel{··}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$+(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$+(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{2z_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^{\frac{3}{2}}{(x_i^2+y_i^2+z_i^2)}^2}-\frac{2{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}+\frac{4z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^3}]$

$h_{68}=\frac{x_i{\stackrel{·}{x}}_i+{y_i\stackrel{·}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$-(x_i^2+y_i^2)[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{69}=\frac{x_i^2{y}_i^2}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{81}=\frac{-{\stackrel{·}{y}}_i}{x_i^2+y_i^2}-\frac{2x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{84}=\frac{-{\stackrel{·}{x}}_i}{x_i^2+y_i^2}-\frac{2y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{91}=\frac{-{\stackrel{··}{y}}_i}{x_i^2+y_i^2}-\frac{2x_i({\stackrel{··}{x}}_i{y}_i-x_i{\stackrel{··}{y}}_i)}{{(x_i^2+y_i^2)}^2}+\frac{2{\stackrel{·}{y}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)-2{\stackrel{·}{x}}_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}+\frac{8x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

$h_{92}=-\frac{2y_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{94}=\frac{{\stackrel{··}{x}}_i}{x_i^2+y_i^2}-\frac{2y_i({\stackrel{··}{x}}_i{y}_i-x_i{\stackrel{··}{y}}_i)}{{(x_i^2+y_i^2)}^2}-\frac{2{\stackrel{·}{x}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2{\stackrel{·}{y}}_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}-\frac{8y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

$h_{95}=-\frac{2x_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

初始条件为x(0/0)；

(3)状态估计的协方差阵为

$P_i(k/k)=P_i^0(k/k)+P_i^k(k/k)+P_{\mathrm{az}}+P_{\mathrm{al}}$

式中，P_az和P_al分别为雷达转台方位角和高低角转动误差的方差阵，为仅接收 到一个回波时状态估计的协方差阵，而定义为：

和P_i(k/k)按照以下原则计算：

当仅接收到一个回波且图像未起作用时，

$P_i^0(k/k)=P_{i0}=P_i(k/k-1)-P_i(k/k-1)H_i^T\left(k\right)S_i^{-1}\left(k\right)H_i\left(k\right)P_i(k/k-1)$

P_i(k/k)＝P_i0+P_az+P_al

当由图像直接确认目标且无相应雷达回波时，协方差与雷达转台无关，则

P_i(k/k)＝P_{img_ctr}+P_{img_az}+P_{img_al}

式中，P_{img_ctr}为目标中心估计误差的方差阵，P_{img_az}为支撑图像系统云台方位角测量误 差的方差阵，P_{img_al}为支撑图像系统云台高低角测量误差的方差阵；

当由图像直接确认目标且有相应雷达回波时，则

$P_i^{-1}(k/k)={(P_{\mathrm{img}\_\mathrm{ctr}}+P_{\mathrm{img}\_\mathrm{az}}+P_{\mathrm{img}\_\mathrm{al}})}^{-1}+{(P_{i0}+P_{\mathrm{az}}+P_{\mathrm{al}})}^{-1}$

初始条件为P_i(0/0)。

本发明的有益效果是：由序列图像确定所跟踪目标的回波，剔除了不是当前所跟 踪目标的回波和杂波；由所确定的回波更新所跟踪目标的测量新计算，减少了多目标 跟踪的测量更新误差，提高了下一时间节拍的预测精度；根据给出的状态估计协方差 阵的计算，在图像直接确定目标时，多目标跟踪的精度达到单目标雷达跟踪的精度： 方位角和高低角约为1毫弧度，距离误差小于10米；当图像去掉部分杂波时，由于杂 波数目减少，多目标跟踪的精度得到了显著提高。

下面结合实施例对本发明作详细说明。

具体实施方式

1、根据所跟踪第i个目标的距离、高低角、方位角、速度、回波信息，高速获取 序列图像，通过序列图像处理获得目标的距离、高低角、方位角、速度、回波信息， 计算

e＝k_v(v_i-v_im)²+k_α(α_i-α_im)²+k_β(β_i-β_im)²+k_r(r_i-r_im)²

如果e＞e_d，则该回波不是当前所跟踪的目标；

式中，v_i是当前跟踪目标的速度，α_i是当前跟踪目标的高低角，β_i是当前跟踪目标的 方位角，r_i是当前跟踪目标斜距，v_im是序列图像确定的目标速度，α_im是序列图像确 定的目标高低角，β_im是序列图像确定的目标方位角，r_im是序列图像确定的目标斜距， k_v，k_α，k_β，k_r为设定的参数；e_d是设定阈值；

2、所跟踪第i个目标的测量新计算为

式中，x_i为9维状态向量，$x_i={\left(\begin{array}{ccccccccc}{x}_i& {\stackrel{·}{x}}_i& {\stackrel{··}{x}}_i& y_i& {\stackrel{·}{y}}_i& {\stackrel{··}{y}}_i& z_i& {\stackrel{·}{z}}_i& {\stackrel{··}{z}}_i\end{array}\right)}^T,$

g_i可以为3、6、或9维观测向量，如果雷达只能测量斜距r_i、高低角α_i、方位角β_i时， g_i＝[r_iα_iβ_i]^T；如果雷达能够测量到速度项，则$g_i={\left(\begin{array}{cccccc}{r}_i& {\stackrel{·}{r}}_i& {\alpha }_i& {\stackrel{·}{\alpha }}_i& {\beta }_i& {\stackrel{·}{\beta }}_i\end{array}\right)}^T;$如果 雷达能够测量到加速度项，则$g_i={\left(\begin{array}{ccccccccc}{r}_i& {\stackrel{·}{r}}_i& {\stackrel{··}{r}}_i& {\alpha }_i& {\stackrel{·}{\alpha }}_i& {\stackrel{··}{\alpha }}_i& {\beta }_i& {\stackrel{·}{\beta }}_i& {\stackrel{··}{\beta }}_i\end{array}\right)}^T;$

式中，下标i代表第i个目标的状态或测量值，x_i(k/k)为第i个目标kT时刻状态的滤波 值，x_i(k/k-1)为第i个目标kT时刻状态的一步预测值，λ_ij(k)为权系数，g_i为观测向量， z_i为g_i的实际测量值，P_i(k/k)为状态估计误差的协方差阵， S_i(k)为新息向量的方差阵

$S_i\left(k\right)=H_i\left(k\right)P_i(k/k-1)H_i^T\left(k\right)+R_i\left(k\right)$

式中，R_i(k)为观测误差的方差阵，

系数矩阵

$p_1=\frac{x_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_2=\frac{y_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_3=\frac{z_i}{\sqrt{x_i^2+y_i^2+z_i^2}}$

$p_4=\frac{-x_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$p_5=\frac{-y_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$p_6=\frac{\sqrt{x_i^2+y_i^2}}{(x_i^2+y_i^2+z_i^2)}$

$p_7=\frac{y_i}{x_i^2+y_i^2}$

$p_8=\frac{-x_i}{x_i^2+y_i^2}$

$h_{21}=\frac{{\stackrel{·}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{x_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{24}=\frac{{\stackrel{·}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{y_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{27}=\frac{{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{z_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{31}=\frac{{\stackrel{··}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{x_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{x}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3x}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{32}=\frac{{2\stackrel{·}{x}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2x}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{34}=\frac{{\stackrel{··}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{y_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{y}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3y}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{35}=\frac{{2\stackrel{·}{y}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2y}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$h_{37}=\frac{{\stackrel{··}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{z_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2+{\stackrel{·}{z}}_i^2+x_i{\stackrel{··}{x}}_i+y_i{\stackrel{··}{y}}_i+z_i{\stackrel{··}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$-\frac{2{\stackrel{·}{z}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}+\frac{{3z}_i{(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}^2}{{(x_i^2+y_i^2+z_i^2)}^{\frac{5}{2}}}$

$h_{38}=\frac{{2\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2+z_i^2}}-\frac{{2z}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i+z_i{\stackrel{·}{z}}_i)}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}$

$\left(\begin{array}{c}{h}_{51}=\frac{2x_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{x_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]\\ h_{54}=\frac{2y_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]\end{array}\right)$

$h_{57}=-\frac{x_i{\stackrel{·}{x}}_i+{y_i\stackrel{·}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$h_{61}=\frac{{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{··}{x}}_i{z}_i+2x_i{\stackrel{··}{z}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}+(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)$

$[\frac{x_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{62}=\frac{x_i{\stackrel{·}{z}}_i+{2\stackrel{·}{x}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{63}=\frac{{-y}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{64}=\frac{{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-{\stackrel{··}{y}}_i{z}_i+2y_i{\stackrel{··}{z}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}+(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)$

$[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{65}=\frac{y_i{\stackrel{·}{z}}_i-{2\stackrel{·}{y}}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{y_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2y_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$+y_i{z}_i[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$h_{66}=\frac{{-x}_i{z}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}$

$h_{67}=\frac{-{\stackrel{·}{x}}_i^2-x_i{\stackrel{··}{x}}_i-{\stackrel{·}{y}}_i^2-y_i{\stackrel{··}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i{\stackrel{·}{x}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{x}}_i^2{z}_i-x_i{\stackrel{··}{x}}_i{z}_i+y_i{\stackrel{·}{y}}_i{\stackrel{·}{z}}_i-{\stackrel{·}{y}}_i^2{z}_i-y_i{\stackrel{··}{y}}_i{z}_i+x_i^2{\stackrel{··}{z}}_i+y_i^2{\stackrel{··}{z}}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$+(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]$

$+(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)[\frac{2z_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^{\frac{3}{2}}{(x_i^2+y_i^2+z_i^2)}^2}-\frac{2{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}+\frac{4z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^3}]$

$h_{68}=\frac{x_i{\stackrel{·}{x}}_i+{y_i\stackrel{·}{y}}_i}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}-(x_i^2{\stackrel{·}{z}}_i+y_i^2{\stackrel{·}{z}}_i-x_i{\stackrel{·}{x}}_i{z}_i-y_i{\stackrel{·}{y}}_i{z}_i)\left[\frac{2z_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}\right]$

$\left(\begin{array}{c}-(X_i^2+y_i^2)[\frac{x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i}{{(x_i^2+y_i^2)}^{\frac{3}{2}}(x_i^2+y_i^2+z_i^2)}+\frac{2x_i{\stackrel{·}{x}}_i+2y_i{\stackrel{·}{y}}_i+2z_i{\stackrel{·}{z}}_i}{\sqrt{x_i^2+y_i^2}{(x_i^2+y_i^2+z_i^2)}^2}]\\ h_{69}=\frac{x_i^2{y}_i^2}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}\end{array}\right)$

$h_{81}=\frac{-{\stackrel{·}{y}}_i}{x_i^2+y_i^2}-\frac{2x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{84}=\frac{-{\stackrel{·}{x}}_i}{x_i^2+y_i^2}-\frac{2y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{91}=\frac{-{\stackrel{··}{y}}_i}{x_i^2+y_i^2}-\frac{2x_i({\stackrel{··}{x}}_i{y}_i-x_i{\stackrel{··}{y}}_i)}{{(x_i^2+y_i^2)}^2}+\frac{2{\stackrel{·}{y}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)-2{\stackrel{·}{x}}_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}+\frac{8x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

$h_{92}=-\frac{2y_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2x_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}$

$h_{94}=\frac{{\stackrel{··}{x}}_i}{x_i^2+y_i^2}-\frac{2y_i({\stackrel{··}{x}}_i{y}_i-x_i{\stackrel{··}{y}}_i)}{{(x_i^2+y_i^2)}^2}-\frac{2{\stackrel{·}{x}}_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2{\stackrel{·}{y}}_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^2}-\frac{8y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

$h_{95}=-\frac{2x_i(x_i{\stackrel{·}{x}}_i+y_i{\stackrel{·}{y}}_i)+2y_i({\stackrel{·}{x}}_i{y}_i-x_i{\stackrel{·}{y}}_i)}{{(x_i^2+y_i^2)}^3}$

初始条件为x(0/0)；

3、状态估计的协方差阵为

$P_i(k/k)=P_i^0(k/k)+P_i^k(k/k)+P_{\mathrm{az}}+P_{\mathrm{al}}$

假设雷达转台方位角和高低角转动误差的方差阵为P_az+P_al＝0.002I，I为单位阵，则状 态估计的协方差阵为：

$P_i(k/k)=P_i^0(k/k)+P_i^k(k/k)+0.002I$

其中定义为：

Δ_i，j(k)为第j个候选回波信息向量

Δ_i，j(k)＝z_i，j(k)-g_i[x_i(k/k-1)]

而Δ_i(k)为Δ_i，j(k)的加权和，即

${\Delta }_i\left(k\right)=\underset{j=1}{\overset{m}{\Sigma }}{\lambda }_{i,j}\left(k\right){\Delta }_{i,j}\left(k\right)$

和P_i(k/k)按照以下原则计算：

当仅接收到一个回波且图像未起作用时，

$P_i^0(k/k)=P_{i0}=P_i(k/k-1)-P_i(k/k-1)H_i^T\left(k\right)S_i^{-1}\left(k\right)H_i\left(k\right)P_i(k/k-1)$

P_i(k/k)＝P_i0+P_az+P_al＝P_i0+0.002I

当由图像直接确认目标且无相应雷达回波时，协方差与雷达转台无关，假设支撑图像系统云 台方位角和高低角测量误差的方差阵P_{img_az}+P_{img_al}＝0.0015I，则

P_i(k/k)＝P_{img_ctr}+i_{mg_az}+P_{img_al}＝P_{img_ctr}+0.0015I

当由图像直接确认目标且有相应雷达回波时，

$P_i^{-1}(k/k)={(P_{\mathrm{img}\_\mathrm{ctr}}+P_{\mathrm{img}\_\mathrm{az}}+P_{\mathrm{img}\_\mathrm{al}})}^{-1}+{(P_{i0}+P_{\mathrm{az}}+P_{\mathrm{al}})}^{-1}$

$={(P_{\mathrm{img}\_\mathrm{ctr}}+0.0015I)}^{-1}+{(P_{i0}+0.002I)}^{-1}$

初始条件为P_i(0/0)。