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

Abstract

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

Description

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

技术领域

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

背景技术

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

多目标跟踪的目的是将探测器所接收到的量测对应不同的信息源，形成不同观测集合或轨迹，根据轨迹估计被跟踪目标的数目以及每一目标的运动参数，实现对多个目标的跟踪。用于多目标状态估计的基本滤波方法有α-β滤波、α-β-γ滤波、卡尔曼滤波、扩展卡尔曼滤波、高斯和近似、最优非线性滤波、粒子滤波和自适应滤波等。α-β和α-β-γ滤波器由于结构简单，计算量小，在早期计算机资源短缺时应用很广。卡尔曼滤波是多目标跟踪的一种基本方法，但是需要知道系统的精确数学模型，并且只适用于线性系统，限制了算法的应用。扩展卡尔曼滤波将卡尔曼滤波理论扩展到非线性领域，用一个高斯分布来近似状态的条件概率分布；而当近似条件不满足时，高斯和滤波器则用一个高斯分布的加权和来近似状态的条件概率分布。最优非线性滤波使用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[Ф₁，Ф₁，Ф₁]为状态转移矩阵，

$\mathrm{\Λ}\left(k\right)={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\Φ}(k+1,\mathrm{\τ})\mathrm{\Γ}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\left[\begin{array}{ccc}{\mathrm{\Λ}}_1^T& {\mathrm{\Λ}}_1^T& {\mathrm{\Λ}}_1^T\end{array}\right]}^T,$

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

${\mathrm{\Φ}}_1=\left[\begin{array}{ccc}1& T& \frac{1}{2}{T}^2\\ 0& 1& T\\ 0& 0& 1\end{array}\right],$ ${\mathrm{\Λ}}_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}{\mathrm{\Σ}}}{\mathrm{\λ}}_{\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}{\mathrm{\Σ}}}{\mathrm{\λ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{i,j}^T\left(k\right)-{\mathrm{\Δ}}_i\left(k\right){\mathrm{\Δ}}_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)的加权和，即

${\mathrm{\Δ}}_i\left(k\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\λ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{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& {\mathrm{\α}}_i& {\stackrel{\·}{\mathrm{\α}}}_i& {\mathrm{\β}}_i& {\stackrel{\·}{\mathrm{\β}}}_i\end{array}\right]}^T;$ 如果雷达能够测量到加速度项，则 $g_i={\left[\begin{array}{ccccccccc}{r}_i& {\stackrel{\·}{r}}_i& {\stackrel{\·\·}{r}}_i& {\mathrm{\α}}_i& {\stackrel{\·}{\mathrm{\α}}}_i& {\stackrel{\·\·}{\mathrm{\α}}}_i& {\mathrm{\β}}_i& {\stackrel{\·}{\mathrm{\β}}}_i& {\stackrel{\·\·}{\mathrm{\β}}}_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)的加权和，即

${\mathrm{\Δ}}_i\left(k\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\λ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{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& {\mathrm{\α}}_i& {\stackrel{\·}{\mathrm{\α}}}_i& {\mathrm{\β}}_i& {\stackrel{\·}{\mathrm{\β}}}_i\end{array}\right]}^T;$ 如果雷达能够测量到加速度项，则 $g_i={\left[\begin{array}{ccccccccc}{r}_i& {\stackrel{\·}{r}}_i& {\stackrel{\·\·}{r}}_i& {\mathrm{\α}}_i& {\stackrel{\·}{\mathrm{\α}}}_i& {\stackrel{\·\·}{\mathrm{\α}}}_i& {\mathrm{\β}}_i& {\stackrel{\·}{\mathrm{\β}}}_i& {\stackrel{\·\·}{\mathrm{\β}}}_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}}}$

$\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}$

$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]$

$\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}$

$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)的加权和，即

${\mathrm{\Δ}}_i\left(k\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\λ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{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)。

Claims (1)

1.一种具有图像确认的多目标跟踪方法，其特征在于包括下述步骤：

(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,$ (x，y，z)为目标在地面参考直角坐标系下的位置坐标；g_i是3、6或9维观测向量，如果雷达只能测量斜距r_i、高低角α_i、方位角β_i时，g_i＝[r_iα_iβ_i]^T；如果雷达能够测量到速度项，则

如果雷达能够测量到加速度项，则 $g_i={\left[\begin{array}{ccccccccc}{r}_i& {\stackrel{\·}{r}}_i& {\stackrel{\·\·}{r}}_i& {\mathrm{\α}}_i& {\stackrel{\·}{\mathrm{\α}}}_i& {\stackrel{\·\·}{\mathrm{\α}}}_i& {\mathrm{\β}}_i& {\stackrel{\·}{\mathrm{\β}}}_i& {\stackrel{\·\·}{\mathrm{\β}}}_i\end{array}\right]}^T;$

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

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

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

λ_i，j(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)的加权和，即

${\mathrm{\Δ}}_i\left(k\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\λ}}_{i,j}\left(k\right){\mathrm{\Δ}}_{i,j}\left(k\right),$

$\left\{\begin{array}{c}{r}_i=\sqrt{x_i^2+y_i^2+z_i^2}\\ {\stackrel{\·}{r}}_i=\frac{1}{\sqrt{x_i^2+y_i^2+z_i^2}}(x_i{\stackrel{\·}{x}}_i+y_i{\stackrel{\·}{y}}_i+z_i{\stackrel{\·}{z}}_i)\\ {\stackrel{\·\·}{r}}_i=\frac{1}{\sqrt{x_i^2+y_i^2+z_i^2}}({\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)-\frac{1}{{(x_i^2+y_i^2+z_i^2)}^{\frac{3}{2}}}{(x_i{\stackrel{\·}{x}}_i+y_i{\stackrel{\·}{y}}_i+z_i{\stackrel{\·}{z}}_i)}^2\\ {\mathrm{\α}}_i={\tan }^{-1}\frac{z_i}{\sqrt{x_i^2+y_i^2}}\\ {\stackrel{\·}{\mathrm{\α}}}_i=\frac{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}{(x_i^2+y_i^2+z_i^2)\sqrt{x_i^2+y_i^2}}\\ {\stackrel{\·\·}{\mathrm{\α}}}_i=\frac{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}{(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{\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}]\\ {\mathrm{\β}}_i={\tan }^{-1}\frac{x_i}{y_i}\\ {\stackrel{\·}{\mathrm{\β}}}_i=\frac{{\stackrel{\·}{x}}_i{y}_i-x_i{\stackrel{\·}{y}}_i}{x_i^2+y_i^2}\\ {\stackrel{\·\·}{\mathrm{\β}}}_i=\frac{{\stackrel{\·\·}{x}}_i{y}_i-x_i{\stackrel{\·\·}{y}}_i}{x_i^2+y_i^2}-\frac{2({\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)}^2}\end{array}\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)}^3}{{(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{z_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{x}_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_i(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分别为雷达转台方位角和高低角转动误差的方差阵，

为仅接收到一个回波时状态估计的协方差阵，而

定义为：

和O_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)。