METHOD OF TRACKING TARGET BY USING 2D RADAR WITH SENSOR

Abstract

Embodiments of the present invention include the different methods for data fusion from multi dissimilar sensors to reduce the noise of the tracking the 3D target in Cartesian coordinates. Accuracy of this invention is precise and more stable than the conventional methods that use geometric calculations of 2D radars to track 3D targets. The results of this invention are using the same 3D radars in the tracking system. These methods are not only implemented in existing tracking centers, but also handle the tradeoff between the data transmission capacity at the command center and the computational speed of system. This invention performs the sequential steps: determining the dynamical motion model of target, state prediction and measurement update. Wherein, the variation of steps is shown in the embodiment of this invention by the following different approaches: selective measurement; parallel filtering; state vector fusion; feedback state vector fusion; measurement fusion state vector fusion.

Claims

1. A tracking 3D target system using fusion of 2D radars and bearing-only sensors includes the following steps: STEP 1: determining the dynamical motion model of target; using equation:
{circumflex over (x)}=f(x,w) (1)
z.sub.k=[.sub.i .sub.i r.sub.r].sup.T, (2), wherein include: determining the state of vector x by tracked radars and sensors, and determining is white Gauss noises with w and v.sub.j are zero mean; STEP 2: state prediction, wherein include: determining covariance matrix Q, R.sub.j in respective;
determining estimation state: {circumflex over (x)}.sub.k+1,k.sup.j=f.sub.k({circumflex over (x)}.sub.k,k.sup.j,0) (3)
covariancing the estimation: P.sub.k+1,k.sup.i=F.sub.kP.sub.k,k.sup.iF.sub.k.sup.T+Q (4) STEP 3: measurement update:
determining coefficient of Gain: K.sub.k+1,j=P.sub.k+1,k.sup.jH.sub.k+1,j.sup.T(H.sub.k+1,jP.sub.k+1,k.sup.jH.sub.k+1,j.sup.T+R.sub.j).sup.1 (5);
determining coefficient of State: {circumflex over (x)}.sub.k+1,k+1.sup.j={circumflex over (x)}.sub.k+1,k.sup.j+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.j,0)) (6);
determining coefficient of State covariance: P.sub.k+1,k+1.sup.i=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.j,j=1, . . . , N. (7).

2. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the selective measurement from dissimilar multi-sources 2D radars and bearing-only sensors includes the following steps: step 1: determining the dynamical motion model of target:
{circumflex over (x)}=f(x,w)
z.sub.k=[.sub.i .sub.i r.sub.r].sup.T wherein the dynamical motion model of target include the following step: establish a new measurement vector z.sub.k by selecting from the amount of 2D radar measurements and measurement vector [.sub.i .sub.i].sup.T of bearing-only sensors which could be replaced the measurement vector directly selected from the equation:
z.sub.j=h.sub.j(x,v.sub.j), j=1, . . . , N step 2: state prediction: Using the equation (3), (4) step 3: measurement update: using equation (5) wherein include the following step:
establishing new covariance matrices: R.sub.k=diag[.sub.,i.sup.2 .sub.,i.sup.2 .sub.r,r.sup.2]; establishing new equation (6) and (7) with Jacobi matrix in accordance with new measurement function: $h\left(x\right)=\left[{\tan }^{-1}\left(\frac{z}{r}\right),{\tan }^{-1}\left(\frac{y}{x}\right),\sqrt{x^2+y^2+z^2}\right].$

3. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the measurement fusion method executes for 2D radars and bearing-only sensors includes the following steps: step 1: determining the dynamical motion model of target:
{circumflex over (x)}=f(x,w), z.sub.k=[.sub.i .sub.f r.sub.r].sup.T, wherein include: fuse the azimuths from measurement vector of 2D radar: [.sub.r].sup.T, fuse the azimuths from measurement vector of tracked sensor: [.sub.i].sup.T, combined the azimuths from measurement vector of 2D radar [.sub.r].sup.T and azimuths from measurement vector of tracked sensor [.sub.i].sup.T based on a minimum-mean-square-error criterion extract, these ones are accompanied with elevation of bearing-only sensor and range of 2D radar merged into an augmented measurement vector z.sub.k, step 2: state prediction: using the equation (3), (4) step 3: measurement update: using the equation (5) with just the new covariance matrices: R.sub.k=diag[.sub.,i.sup.2 .sub.,f.sup.2 .sub.r,r.sup.2].

4. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the parallel filter method executes for 2D radars and bearing-only sensors includes the following steps step 1: determining the dynamical motion model of target:
{circumflex over (x)}=f(x,w), z.sub.k=[.sub.i .sub.f r.sub.r].sup.T, wherein include the following step: all measurement vectors can be fused into a new form of measurement vector z.sub.k by combination of 2D measurement vectors z.sub.r=[.sub.r r.sub.r].sup.T, and bearing-only measurement vector z.sub.i=[.sub.i .sub.i].sup.T; step 2: state prediction : using the equation (3), (4); step 3: measurement update: using the equation (5), wherein include: establish the new covariance matrices R.sub.k=diag[.sub.,i.sup.2 .sub.,i.sup.2 .sub.,r.sup.2 .sub.r,r.sup.2] and establish new equation (6) and (7) with Jacobi matrices in accordance with a new measurement function: h.sub.k=[h.sub.i h.sub.r].sup.T.

5. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the state vector fusion method executes for 2D radars and bearing-only sensors includes the following steps: step 1: performing the step 1, 2 and 3 of general tracking system in claim 1 for the 2D radars at a local center to achieve the estimate state vector and the covariance matrices at the local center;
{circumflex over (x)}.sub.k+1,k+1.sup.r={circumflex over (x)}.sub.k+1,k.sup.r+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.r,0));
P.sub.k+1,k+1.sup.r=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.r; step 2: performing the step 1, 2 and 3 of general tracking system in claim 1 for the bearing-only sensors at the local center to achieve the estimate state vector and the covariance matrices at local center;
{circumflex over (x)}.sub.k+1,k+1.sup.i={circumflex over (x)}.sub.k+1,k.sup.i+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.i,0));
P.sub.k+1,k+1.sup.i=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.i; step 3: performing data fusion of the local estimate state vectors at step 2 in this method based on a minimum-mean-square-error criterion to yield a fused state vectors {circumflex over (x)}.sub.f, P.sub.f at a command center.

6. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the feedback state vector fusion method executes for 2D radars and bearing-only sensors includes the following steps: step 1: performing the step 1, 2 and 3 in claim 5 to achieve the fused estimate state vector and the fused covariance matrices {circumflex over (x)}.sub.k,k.sup.f,P.sub.k,k.sup.f at a command center; step 2: the fused state vector and fused state covariance matrix are fed back to a single state predictor of step 1 and the output of this process fed to two measurement update:
state prediction: {circumflex over (x)}.sub.k+1,k=F.sub.k{circumflex over (x)}.sub.k,k.sup.f (3);
covariance P.sub.k+1,k=F.sub.kP.sub.k,k.sup.fF.sub.k.sup.T+Q prediction: (4).

7. A tracking 3D target system using fusion of 2D radars and bearing-only sensors of claim 1, wherein the measurement fusion state vector fusion method executes for 2D radars and bearing-only sensors includes the following steps: step 1: performing the step 1, 2 of general tracking system in FIG. 1 for the 2d radars at local center to achieve the predicted state vectors and the covariance matrices {circumflex over (x)}.sub.k+1,k.sup.r,P.sub.k+1,k.sup.r at a local center; step 2: performing the step 1, 2 of general tracking system in FIG. 1 for the bearing-only sensors at the local center to achieve the predicted state vectors and the covariance matrices {circumflex over (x)}.sub.k+1,k.sup.r,P.sub.k+1,k.sup.r at local center; step 3: performing the first fusion of these locally predicted state vectors in Steps 1 and 2 of this method based on minimum-mean-square-error criterion to obtain a fused predict-state vectors {circumflex over (x)}.sub.k+1,k.sup.f,P.sub.k+1,k.sup.f at the local center; step 4: these fused predict-state vectors are fed to two measurement update at step 3 (measurement update) of general tracking system for 2D radars and bearing-only sensors at local center to obtain an estimated state vectors and a corresponding covariance matrices: {circumflex over (x)}.sub.k+1,k+1.sup.r,{circumflex over (x)}.sub.k+1,k+1.sup.i,P.sub.k+1,k+1.sup.r,P.sub.k+1,k+1.sup.i at local center; step 5: performing the 2nd fusion of theses estimated state vectors at step 4 of this method based on minimum-mean-square-error criterion to yield a fused state-estimate vectors {circumflex over (x)}.sub.k+1,k+1.sup.f,P.sub.k+1,k+1.sup.f at a command center.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0006] The following drawings will be used to more fully describe embodiments of the present invention.

[0007] FIG. 1 is a block diagram showing a general overview of the tracking system according to the prior art

[0008] FIG. 2 is a block diagram describing a selected measurement method according to an embodiment of the present invention

[0009] FIG. 3 is a block diagram describing a measurement fusion method according to an embodiment of the present invention

[0010] FIG. 4 is a block diagram illustrating a parallel filter method according to an embodiment of the present invention

[0011] FIG. 5 is a block diagram demonstrating a state vector fusion method according to an embodiment of the present invention

[0012] FIG. 6 is a block diagram depicting a feedback state vector fusion method according to an embodiment of the present invention

[0013] FIG. 7 is a block diagram interpreting a measurement fusion and state vector fusion method according to an embodiment of the present invention

[0014] FIGS. 8.1 and 8.2 are tables illustrating in further detail the root mean square errors of each 2D radar, bearing-only sensor and six different methods at time T=30 sec. and T=60 sec. according to an exemplary embodiment.

[0015] FIGS. 9.1 and 9.2 are figures explaining the absolute errors and root sum square errors at time T=30 sec. according to an exemplary embodiment.

[0016] FIGS. 10.1 and 10.2 are figures explaining the absolute errors and root sum square errors at time T=60 sec. according to an exemplary embodiment.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT

[0017] The present invention relates to tracking a 3D target such as aircraft, car, or ship from multi-sensors. Among the various techniques available for Multi-sensor data fusion (MSDF), Extended Kalman Filtering-based approach is used for the present case, as it proves to be an efficient recursive algorithm suitable for real-time application and for a dynamical target which is tracked by bearing-only sensors is supposed have the nonlinear system model in Cartesian coordinates.

[0018] In our tracking 3D target system applying the data fusion technique for 2d radar and bearing-only sensor thus we have to reconstruct some of variation the Extended Kalman Filter (EKF) that one may fit six methods in our invention.

[0019] Let us briefly describe the main steps of general tracking system that use EKF algorithm as follow:

[0020] Step 1: Supposing the motion of target follows the dynamic nonlinear model as

{circumflex over (x)}=f(x,w) (1)

z.sub.j=h.sub.j(x,v.sub.j), j=1, . . . , N (2)

[0021] where x is state of vector and we have N tracked sensors and radars, z.sub.j is measurement vector, w and v.sub.j are zero mean white Gauss noises with covariance matrix Q,R.sub.j.

[0022] Step 2: State prediction:

estimation state vector: {circumflex over (x)}.sub.k+1,k.sup.j=f.sub.k({circumflex over (x)}.sub.k,k.sup.j,0) (3)

state covariance matrix: P.sub.k+1,k.sup.i=F.sub.kP.sub.k,k.sup.iF.sub.k.sup.T+Q (4)

[0023] Step 3: Measurement Update:

Gain: K.sub.k+1,j=P.sub.k+1,k.sup.jH.sub.k+1,j.sup.T(H.sub.k+1,jP.sub.k+1,k.sup.jH.sub.k+1,j.sup.T+R.sub.j).sup.1 (5)

State: {circumflex over (x)}.sub.k+1,k+1.sup.j={circumflex over (x)}.sub.k+1,k.sup.j+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.j,0)) (6)

State covariance: P.sub.k+1,k+1.sup.j=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.j,j=1, . . . , N. (7)

[0024] where F.sub.k and H.sub.k,j are corresponding Jacobi matrices of f and h.sub.j at time step k when we carry out Taylor series expansion on these functions.

[0025] The block diagram is shown in FIG. 1.

[0026] We introduce here parameters that are input in our tracking system: z.sub.r=[.sub.r r.sub.r].sup.T and z.sub.i=[.sub.i .sub.i].sup.T: are measurement vectors 2D radars and bearing-only sensors in polar coordinates, respectively. Where the sign r corresponds to radars and i corresponds to bearing-only sensors.

[00001]$R_r=\left[\begin{array}{cc}{}_{r,r}^2& 0\\ 0& {}_{,r}^2\end{array}\right],R_i=\left[\begin{array}{cc}{}_{,i}^2& 0\\ 0& {}_{,i}^2\end{array}\right].Math.\text{:}$

: are the noise covariances corresponding to measurements.

[0027] The following formulae will use to transform from Cartesian coordinates to polar coordinates:

[00002]$\begin{array}{cc}r=\sqrt{x^2+y^2+z^2},={\tan }^{-1}\left(\frac{y}{x}\right),={\tan }^{-1}\left(\frac{z}{r}\right).& \left(9\right)\end{array}$

[0028] The Jacobi matrices and the variation of the models will be different depending on each method in the invention which will be described as follows:

1st Method: Selective Measurements (SM)

[0029] Step 1: The motion model

{circumflex over (x)}=f(x,w)

z.sub.k=[.sub.i .sub.i r.sub.r].sup.T

[0030] In this method, the measurement vector in formula (3) will be replaced by the new measurement vector z.sub.k which consists of azimuth vector [r.sub.r].sup.T of 2D radar measurements and measurement vector [.sub.i .sub.i].sup.T of bearing-only sensors.

[0031] Step 2: state prediction: Using the equation (3), (4)

[0032] Step 3: Measurement update: Using the same formulae (5), (6) and (7) with the new covariance R.sub.k=diag[.sub.,i.sup.2 .sub.i.sup.2 .sub.r,r.sup.2] matrices: in formula (5) and the function h(x) in formulae (6)-(7) is slightly changed as:

[00003]$h\left(x\right)=\left[{\tan }^{-1}\left(\frac{z}{r}\right),{\tan }^{-1}\left(\frac{y}{x}\right),\sqrt{x^2+y^2+z^2}\right]$

[0033] The block diagram is shown in FIG. 2.

2.SUP.nd .Method: Measurement Fusion (MF)

[0034] Step 1: The motion model:

{circumflex over (x)}=f(x,w), z.sub.k=[.sub.i .sub.f r.sub.r].sup.T

[0035] we fuse the azimuths [.sub.r].sup.T, [.sub.i].sup.T based on a minimum-mean-square-error criterion extracted from radar measurement and bearing-only. These ones accompanied with elevation of bearing-only sensor and range of 2D radar merged into an augmented measurement vector z.sub.k and measurement noise variances from both sensors are also concatenated to yield the similar process of the 1.sup.st method.

[0036] Step 2: state prediction: Using the equation (3), (4)

[0037] Step 3: Measurement update: Using the same formulae (5), (6) and (7) with just the new covariance matrices: R.sub.k=diag[.sub.,i.sup.2 .sub.,f.sup.2 .sub.r,r.sup.2] in formula (5).

[0038] The block diagram is shown in FIG. 3.

3.SUP.rd .Method: Parallel Filter (PF)

[0039] Step 1: The motion model:

{circumflex over (x)}=f(x,w), z.sub.k=[.sub.i .sub.i .sub.r r.sub.r].sup.T

[0040] All measurement vectors can be fused into a new form of measurement vector z.sub.k by combination of 2D measurement vectors z.sub.r=[.sub.r r.sub.r].sup.T, and bearing-only measurement vector z.sub.i=[.sub.i .sub.i].sup.T

[0041] Step 2: state prediction: Using the equation (3), (4)

[0042] Step 3: Measurement update: Using the same formulae (5), (6) and (7) with the new covariance matrices: R.sub.k=diag[.sub.,i .sub.,i.sup.2 .sub.,r.sup.2 .sub.r,r.sup.2], and the function h(x) in formulae (6)-(7) is slightly changed as: h.sub.k=[h.sub.i h.sub.r].sup.T.

[0043] The block diagram is shown in FIG. 4.

4.SUP.th .Method: State Vector Fusion (SVF)

[0044] Step 1: Performing the step 1, 2 and 3 of general tracking system in FIG. 1 for the 2d radars at local center to achieve the estimate state vector and the covariance matrices at local center

{circumflex over (x)}.sub.k+1,k+1.sup.r={circumflex over (x)}.sub.k+1,k.sup.r+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.r,0))

P.sub.k+1,k+1.sup.r=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.r,

[0045] Step 2: Performing the step 1, 2 and 3 of general tracking system in FIG. 1 for the bearing-only sensors at the local center to achieve the estimate state vector and the covariance matrices at local center

{circumflex over (x)}.sub.k+1,k+1.sup.i={circumflex over (x)}.sub.k+1,k.sup.i+K.sub.k+1,j(z.sub.k+1,jh.sub.k+1,j({circumflex over (x)}.sub.k+1,k.sup.i,0))

P.sub.k+1,k+1.sup.i=(IK.sub.k+1,jH.sub.k+1,j)P.sub.k+1,k.sup.i,

[0046] Step 3: Performing data fusion of the local estimate state vectors at step 2 in this method, based on a minimum-mean-square-error criterion to yield a fused state vectors {circumflex over (x)}.sub.f, P.sub.f at command center.

[0047] The block diagram is shown in FIG. 5.

5.SUP.th .Method: Feedback State Vector Fusion (FSVF)

[0048] Step 1: Performing the step 1, 2 and 3 in method 4 to achieve the fused estimate state vector and the fused covariance matrices {circumflex over (x)}.sub.k,k.sup.f, P.sub.k,k.sup.f at a command center

[0049] Step 2: The fused state vector and fused state covariance matrix are fed back to a single state predictor of step 1 and the output of this process fed to two measurement update

State prediction: {circumflex over (x)}.sub.k+1,k=F.sub.k{circumflex over (x)}.sub.k,k.sup.f

Covariance prediction: P.sub.k+1,k=F.sub.kP.sub.k,k.sup.fF.sub.k.sup.T+Q

[0050] The block diagram is shown in FIG. 6.

6.SUP.th .Method: Measurement Fusion State Vector Fusion (MFSVF)

[0051] Step 1: Performing the step 1, 2 of general tracking system in FIG. 1 for the 2d radars at local center to achieve the predicted state vectors and the covariance matrices {circumflex over (x)}.sub.k+1,k.sup.r,P.sub.k+1,k.sup.r at local center.

[0052] Step 2: Performing the step 1, 2 of general tracking system in FIG. 1 for the bearing-only sensors at local center to achieve the predicted state vectors and the covariance matrices {circumflex over (x)}.sub.k+1,k.sup.r,P.sub.k+1,k.sup.r at local center.

[0053] Step 3: Performing the first fusion of these locally predicted state vectors in Steps 1 and 2 of this method based on minimum-mean-square-error criterion to obtain (at the local center) a fused predict-state vectors {circumflex over (x)}.sub.k+1,k.sup.f,P.sub.k+1,k.sup.f

[0054] Step 4: These fused predict-state vectors are fed to two measurement update at step 3 (measurement update) of general tracking system for 2D radars and bearing-only sensors at local center to obtain (at local center) an estimated state vectors and a corresponding covariance matrices: {circumflex over (x)}.sub.k+1,k+1.sup.r,{circumflex over (x)}.sub.k+1,k+1.sup.i,P.sub.k+1,k+1.sup.r,P.sub.k+1,k+1.sup.i,

[0055] Step 5: Performing the 2nd fusion of theses estimated state vectors at step 4 of this method based on minimum-mean-square-error criterion to yield a fused state-estimate vectors {circumflex over (x)}.sub.k+1,k+1.sup.f,P.sub.k+1,k+1.sup.f at command center.

[0056] The block diagram is shown in FIG. 7.

[0057] To evaluate results we run two hundred Monte Carlo simulation for two steps of time, T=30 sec. and T=60 sec. and use then the root mean square error (RMSE) in position, velocity and acceleration which are shown in in FIG. 8.1 and FIG. 8.2. The root sum square error (RSSE) in position which are illustrated in figure FIG. 9.2 for the time T1=30 sec. and in figure FIG. 10.2 for the time T2=60 sec. Absolute error (AE) in x, y, z direction which are depicted in FIG. 9.1 and FIG. 10.1. It can conclude that all errors of six methods are tremendously smaller 92.5% to 97.6% than errors which were provided 2D radar or 98.3% to 99.35 to IR sensor through the RMSE. As a side note, the results of SF, MF, FSVF and MFSVF method are equivalent and it is worth mentioning here that, PF and SVF method are slightly worse than the others with the tendency of error in PF method to get smaller as the time gets larger while the SVF method is opposite. This is explained by the error of PF method is very high at initial time step which quickly descends and tends to other methods whereas the error of SVF method is stable in a short time from initial time and ascends fast later.