# Models and Algorithms for Tracking Target with Coordinated Turn Motion

Authors: Xianghui Yuan; Feng Lian; Chongzhao Han
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Tracking target with coordinated turn (CT) motion is highly dependent on the models and algorithms. First, the widely used models are compared in this paper—coordinated turn (CT) model with known turn rate, augmented coordinated turn (ACT) model with Cartesian velocity, ACT model with polar velocity, CT model using a kinematic constraint, and maneuver centered circular motion model. Then, in the single model tracking framework, the tracking algorithms for the last four models are compared and the suggestions on the choice of models for different practical target tracking problems are given. Finally, in the multiple models (MM) framework, the algorithm based on expectation maximization (EM) algorithm is derived, including both the batch form and the recursive form. Compared with the widely used interacting multiple model (IMM) algorithm, the EM algorithm shows its effectiveness.


---

## Body

## 1. Introduction

The problem of tracking a single target with coordinated turn (CT) motion is considered. The motion of a civil aircraft can usually be modeled as moving by constant speed in straight lines and circle segments. The former is known as constant velocity (CV) model and the latter is coordinated turn model. In tracking applications, only the position part of the state can be measured by the sensor and the turn rateωis often unknown. So the measurement data can be seen as the incomplete data. This is a resource-constrained problem for tracking target with coordinated turn motion.CT model is highly dependent on the choice of state components [1]. The turn rateωcan be augmented in the CT model, called ACT model. There are two types of ACT models: ACT model with Cartesian velocity and ACT model with polar velocity. The state vectors are [x,y,x˙,y˙,ω]′ and [x,y,v,ϕ,ω]′, respectively. The two are both nonlinear models and have been compared in [2, 3] based on EKF. For unscented Kalman filter (UKF) is a very efficient tool for nonlinear estimation [4, 5], here the two models are compared based on UKF.When the target with CT motion has a constant speed, it satisfies a kinematic constraint:V·A=0, whereV is the target velocity vector and A is the target acceleration vector. If the dynamic model incorporates the constraint directly, it will become a highly nonlinear one. To avoid this nonlinearity, the kinematic constraint was incorporated into a pseudomeasurement model [6–8].A maneuver-centered model is introduced in [9]. The state components are [r,θ,ω]′. The model’s state equation has a linear form, but its measurement equation is pseudolinear because the noise covariance is actually state dependent [10]. The center of the turn should be accurately determined, which is inherently a nonlinear problem.Target dynamic models and tracking algorithms have intimate ties [1]. In the single model tracking framework, the tracking algorithms are interpreted and compared.The interacting multiple model (IMM) approach has been generally considered to be the mainstream approach to maneuvering target tracking. It utilizes a bank ofNKalman filters, each designed to model a different maneuver [11]. IMM algorithm is a suboptimal algorithm based on the minimum mean square error (MMSE) criterion. Under the MMSE criterion, to get the optimal estimation of the target state, the computational load grows exponentially when the measurements are increasing. In recent years, tracking target based on maximum a posteriori (MAP) criterion has received a lot of interest [12–17]. Expectation maximization (EM) algorithm is the state estimation approach based on MAP criterion. Using EM algorithm, the computational load grows linearly during per iteration and the optimal estimation based on MAP criterion can be achieved finally.The existing EM algorithm to track maneuvering target can be classified into two categories: one formulates the maneuver as the unknown input [12–14] and the other formulates the maneuver as the system’s process noise [15]. Aiming at the problem to track a target with CT maneuver, an EM algorithm is presented. The maneuver is formulated by the turn rate. First, the turn rate sequence is estimated using the EM algorithm. Then, with the estimated turn rate sequence, the target state sequence is estimated accurately.The rest of this paper is organized as follows. Section2 presents all the CT models’ state equations and measurement equations. The tracking algorithms based on single model are interpreted in Section 3; the simulations are also presented. In Section 4, the batch and recursive EM algorithms are derived and compared with the IMM algorithm in simulation. Section 5 provides the paper’s conclusions.


## 2. Dynamic Models for CT Motion

A maneuvering target can be modeled by(1)Xk+1=fk(Xk)+wk,zk=hk(Xk)+ek,
where Xk and zkare target state and observation, respectively, at discrete timetk; wkandek are process noise and measurement noise sequences, respectively; fk and hkare vector-valued functions.


### 2.1. CT Model with Known Turn Rate

The coordinated turn motion can be described by the following equation:(2)Xk+1=F(ωk)Xk+wk.The measurement equation is:(3)zk=HCTXk+ek.The components of state areX=[xx˙yy˙]′.  ωkstands for the turn rate in timek.Where(4)F(ωk)=[1sin(ωkT)ωk0-1-cos(ωkT)ωk0cos(ωkT)0-sin(ωkT)01-cos(ωkT)ωk1sin(ωkT)ωk0sin(ωkT)0cos(ωkT)]w=[wxwy]′E[wk]=0,E[wkwl′]=QCTδkl.Assume only position could be measured, where(5)HCT=[10000010]E[ek]=0,E[ekel′]=Rδkl.This model assumes that the turn rate is known or could be estimated. When the range rate measurements are available, the turn rate could be estimated by using range rate measurements [18, 19]. The tracking performance will be deteriorated when the assumed turn rate is far away from the true one. This model is usually used as one of the models in a multiple models framework.


### 2.2. ACT Model with Cartesian Velocity

In this model, the state vector is chosen to beX=[x,y,x˙,y˙,ω]′; the state space equation can be written as
(6)Xk+1=fACT1(Xk)+GACT1wk,
where
(7)fACT1(X)=[x+x˙ωsin(ωT)-y˙ω(1-cos(ωT))y+x˙ω(1-cos(ωT))+y˙ω(sin(ωT))x˙cos(ωT)-y˙sin(ωT)x˙sin(ωT)+y˙cos(ωT)ω],(8)GACT1=[T220T000T220T000001]′,(9)w=[wxwywω]′,(10)E[wk]=0,E[wkwl′]=QACT1δkl.Assume only position could be measured, the measurement equation can be written as(11)zk=HACT1Xk+ek,
where
(12)HACT1=[1000001000],(13)E[ek]=0,E[ekel′]=Rδkl.


### 2.3. ACT Model with Polar Velocity

This model’s state vector isX=[x,y,v,ϕ,ω]′, and the dynamic state equation is given by
(14)Xk+1=fACT2(Xk)+GACT2wk,
where
(15)fACT2(X)=[x+(2vω)sin(ωT2)cos(ϕ+ωT2)y+(2vω)sin(ωT2)sin(ϕ+ωT2)vϕ+ωTω]GACT2=[00T200000T22T2]′w=[wvwω]′E[wk]=0,E[wkwl′]=QACT2δkl.
However the measurement equation is the same as (11) to (13).


### 2.4. Kinematic Constraint Model

For a constant speed target, the acceleration vector is orthogonal to the velocity vector:(16)C(X)=V·A=0,
whereV is the target velocity vector and A is the target acceleration vector.This kinematic constraint can be used as a pseudomeasurement. The state vector is chosen to beX=[xx˙x¨yy˙y¨]′. So the dynamic model is the constant acceleration (CA) model, given by
(17)Xk+1=FXk+Gwk,
where
(18)F=[1TT2200001T0000010000001TT2200001T000001]G=[T22T1000000T22T1]′w=[wxwy]′E[wk]=0,E[wkwl′]=QCAδkl.The measurement equation is given by(19)zk=Hxk+ek,
where
(20)H=[100000000100]E[ek]=0,E[ekel′]=Rδkl.The pseudomeasurement is(21)Vk∣kSk∣k·Ak+μk=0,
where Vk|k=[x˙k∣ky˙k∣k]′ and Ak=[x¨ky¨k]′.S
k
∣
k is the filtered speed at time k:(22)Sk∣k=x˙k∣k2+y˙k∣k2,(23)μk~N(0,Rkμ),(24)Rkμ=r1(δ)k+r0,0≤δ<1,
where r1 is chosen to be large for initialization and r0 is chosen for steady-state conditions.


### 2.5. Maneuver-Centered CT Model

This model’s state vector is given byX=[rθω]′. The process state space equation is
(25)Xk+1=ΦXk+Γwk,
where
(26)Φ=[10001T001]Γ=[100T201]w=[wrwω]′E[wk]=0,E[wkwl′]=Qmδkl.Assume the center of the CT motion is(x^c,y^c). The transformation between Cartesian coordinates and maneuver-centered coordinates is given by
(27)r=(x-x^c)2+(y-y^c)2θ=tan-1(y-y^cx-x^c).
So the measurement equation is given by
(28)zk=HmXk+ek,
where
(29)Hm=[100010]E[ekel′]=Rm=JrθRJrθ′.Jrθ is the Jacobian matrix based on (27), which leads to
(30)Jrθ=[cosθsinθ-sinθrcosθr].


## 2.1. CT Model with Known Turn Rate

The coordinated turn motion can be described by the following equation:(2)Xk+1=F(ωk)Xk+wk.The measurement equation is:(3)zk=HCTXk+ek.The components of state areX=[xx˙yy˙]′.  ωkstands for the turn rate in timek.Where(4)F(ωk)=[1sin(ωkT)ωk0-1-cos(ωkT)ωk0cos(ωkT)0-sin(ωkT)01-cos(ωkT)ωk1sin(ωkT)ωk0sin(ωkT)0cos(ωkT)]w=[wxwy]′E[wk]=0,E[wkwl′]=QCTδkl.Assume only position could be measured, where(5)HCT=[10000010]E[ek]=0,E[ekel′]=Rδkl.This model assumes that the turn rate is known or could be estimated. When the range rate measurements are available, the turn rate could be estimated by using range rate measurements [18, 19]. The tracking performance will be deteriorated when the assumed turn rate is far away from the true one. This model is usually used as one of the models in a multiple models framework.


## 2.2. ACT Model with Cartesian Velocity

In this model, the state vector is chosen to beX=[x,y,x˙,y˙,ω]′; the state space equation can be written as
(6)Xk+1=fACT1(Xk)+GACT1wk,
where
(7)fACT1(X)=[x+x˙ωsin(ωT)-y˙ω(1-cos(ωT))y+x˙ω(1-cos(ωT))+y˙ω(sin(ωT))x˙cos(ωT)-y˙sin(ωT)x˙sin(ωT)+y˙cos(ωT)ω],(8)GACT1=[T220T000T220T000001]′,(9)w=[wxwywω]′,(10)E[wk]=0,E[wkwl′]=QACT1δkl.Assume only position could be measured, the measurement equation can be written as(11)zk=HACT1Xk+ek,
where
(12)HACT1=[1000001000],(13)E[ek]=0,E[ekel′]=Rδkl.


## 2.3. ACT Model with Polar Velocity

This model’s state vector isX=[x,y,v,ϕ,ω]′, and the dynamic state equation is given by
(14)Xk+1=fACT2(Xk)+GACT2wk,
where
(15)fACT2(X)=[x+(2vω)sin(ωT2)cos(ϕ+ωT2)y+(2vω)sin(ωT2)sin(ϕ+ωT2)vϕ+ωTω]GACT2=[00T200000T22T2]′w=[wvwω]′E[wk]=0,E[wkwl′]=QACT2δkl.
However the measurement equation is the same as (11) to (13).


## 2.4. Kinematic Constraint Model

For a constant speed target, the acceleration vector is orthogonal to the velocity vector:(16)C(X)=V·A=0,
whereV is the target velocity vector and A is the target acceleration vector.This kinematic constraint can be used as a pseudomeasurement. The state vector is chosen to beX=[xx˙x¨yy˙y¨]′. So the dynamic model is the constant acceleration (CA) model, given by
(17)Xk+1=FXk+Gwk,
where
(18)F=[1TT2200001T0000010000001TT2200001T000001]G=[T22T1000000T22T1]′w=[wxwy]′E[wk]=0,E[wkwl′]=QCAδkl.The measurement equation is given by(19)zk=Hxk+ek,
where
(20)H=[100000000100]E[ek]=0,E[ekel′]=Rδkl.The pseudomeasurement is(21)Vk∣kSk∣k·Ak+μk=0,
where Vk|k=[x˙k∣ky˙k∣k]′ and Ak=[x¨ky¨k]′.S
k
∣
k is the filtered speed at time k:(22)Sk∣k=x˙k∣k2+y˙k∣k2,(23)μk~N(0,Rkμ),(24)Rkμ=r1(δ)k+r0,0≤δ<1,
where r1 is chosen to be large for initialization and r0 is chosen for steady-state conditions.


## 2.5. Maneuver-Centered CT Model

This model’s state vector is given byX=[rθω]′. The process state space equation is
(25)Xk+1=ΦXk+Γwk,
where
(26)Φ=[10001T001]Γ=[100T201]w=[wrwω]′E[wk]=0,E[wkwl′]=Qmδkl.Assume the center of the CT motion is(x^c,y^c). The transformation between Cartesian coordinates and maneuver-centered coordinates is given by
(27)r=(x-x^c)2+(y-y^c)2θ=tan-1(y-y^cx-x^c).
So the measurement equation is given by
(28)zk=HmXk+ek,
where
(29)Hm=[100010]E[ekel′]=Rm=JrθRJrθ′.Jrθ is the Jacobian matrix based on (27), which leads to
(30)Jrθ=[cosθsinθ-sinθrcosθr].


## 3. Tracking Algorithms in a Single Model Framework

### 3.1. UKF Filter with ACT Models

If the turn rate is augmented to the state vector, it will become a nonlinear problem. The extended Kalman filter (EKF) has been used to track this kind of motion. Since unscented Kalman filter (UKF) is very suitable for nonlinear estimation [4, 5], here the UKF algorithm is introduced.(i) Calculate the Weights of Sigma Points
(31)
W
0
m
=
λ
(
n
+
λ
)
W
0
c
=
λ
(
n
+
λ
)
+
(
1
-
α
2
+
β
)
W
i
m
=
W
i
c
=
0.5
(
n
+
λ
)
,
i
=
1,2
,
…
2
n
,
wheren is the dimension of the state vector. λ=α2(n+κ)-n is a scaling parameter. α determines the sigma points aroundx- and is usually set to a small positive value (e.g., 1e-3). κ is a secondary scaling parameter which is usually set to 0, and β=2 is optimal for Gauss distributions. Where the((n+λ)Px)i is theith row of the matrix square root.(ii) Calculate the Sigma Points
(32)
ξ
k
-
1
∣
k
-
1
0
=
X
^
k
-
1
∣
k
-
1
ξ
k
-
1
∣
k
-
1
(
i
)
=
X
^
k
-
1
∣
k
-
1
+
(
(
n
+
λ
)
P
x
)
i
i
=
1,2
,
…
,
n
ξ
k
-
1
∣
k
-
1
(
i
)
=
X
^
k
-
1
∣
k
-
1
-
(
(
n
+
λ
)
P
x
)
i
i
=
n
+
1
,
n
+
2
,
…
,
2
n
.(iii) Time Update
(33)
ξ
k
(
i
)
=
f
k
(
ξ
k
-
1
|
k
-
1
(
i
)
)
,
i
=
0,1
,
…
2
n
X
^
k
∣
k
-
1
=
∑
i
=
0
2
n
W
i
m
ξ
k
(
i
)
P
k
∣
k
-
1
=
∑
i
=
0
2
n
W
i
c
(
ξ
k
(
i
)
-
X
^
k
∣
k
-
1
)
(
ξ
k
(
i
)
-
X
^
k
∣
k
-
1
)
′
+
G
Q
k
-
1
G
′
.(iv) Measurement Update. Because we assume the measurement equation is linear, the following is just the same as the traditional Kalman filter:
(34)z^k∣k-1=HX^k∣k-1Sk=HPk∣k-1H′+RkKk=Pk∣k-1H′Sk-1X^k∣k=X^k∣k-1+Kk(zk-zk|k-1)Pk∣k=Pk∣k-1-KkSkKk′.
For the cases where the measurement equation is also nonlinear, the measurement update can be referred to [10] for details.


### 3.2. Kinematic Constraint Tracking Filter

The Kalman filtering equations for processing this kinematic constraint as a pseudomeasurement are given below, where the filtered state estimate and error covariance after the constraint have been applied are denoted byXk∣kC and Pk∣kC, respectively [8].(i) Time Update
(35)
X
^
k
∣
k
-
1
=
F
X
^
k
-
1
∣
k
-
1
C
P
k
∣
k
-
1
=
F
P
k
-
1
∣
k
-
1
C
F
′
+
G
Q
k
-
1
G
′
.(ii) Measurement Update. The measurement update is the same as (34).(iii) Constraint Update
(36)
K
k
C
=
P
k
∣
k
C
k
T
[
C
k
P
k
∣
k
C
k
′
+
R
k
μ
]
-
1
X
^
k
∣
k
C
=
[
I
-
K
k
C
C
k
]
X
^
k
∣
k
P
k
∣
k
C
=
[
I
-
K
k
C
C
k
]
P
k
∣
k
,
where
(37)Ck=1Sk∣k[00x˙^k∣k00y˙^k∣k].


### 3.3. Maneuver-Centered Tracking Filter

(i) Estimating Center of Maneuver. The center of the maneuver should be estimated from the measurements. It can be estimated through least square method which requires an iterative search procedure. The following simple geometrically oriented procedure of estimating the center was proposed in [9]. The main idea is as follows: if two points are on a circle then the perpendicular bisector of the chord between those points will pass through the center of the circle. The slope(m) andy intercept(b)of the perpendicular bisector is given by
(38)m=(x1-x2)(y2-y1)b=(y1+y2)2-m(x1+x2)2,
where(x1,y1)and(x2,y2)are the coordinates of the two points. The center can be given by
(39)x^c=(b1-b2)(m2-m1)y^c=(m1b2-m2b1)(m1-m2).(ii) Maneuver Detection. In the absence of a maneuver, the target is assumed to be traveling in a straight line and modeled by a constant velocity (CV) motion. (CV model is very simple and commonly used, which will not be listed here.) When the maneuver is detected, the filter switches to the maneuver-center CT model. While the end of a maneuver is detected, the filter will then switch back to CV model.Here a fading memory average of the innovations is used to detect if a maneuver occurs. The equation is given by(40)uk=ρuk-1+dk
with
(41)dk=νk′Sk-1νk,
where0<ρ<1, νkis the innovation vector, andSkis its covariance matrix.u
kwill have a chi-squared distribution with degrees
(42)nu=nz1+ρ1-ρ,
wherenzis the dimension of the measurement vector. Whenukexceeds a threshold (e.g., 95% or 99% confidence interval), then a maneuver onset is declared. The end time of a maneuver will be determined in a similar fashion. The procedure can be referred to [9] for details.


### 3.4. Simulation Results

(i) The Scenario. The scenario simulated here is very similar to that described in [20]. It includes few rectilinear stages and few CT maneuvers. Four consecutive 180∘ turns with rates ω=1.87, −2.8, 5.6, −4.68 are simulated, respectively, for scans [56,150], [182,245], [285,314], and [343,379]. The target trajectory can be seen in Figure 1.Figure 1
The test trajectory.The initial target position and velocities areX0=60 km, Y0=40 km, X˙0=-172 km, and Y˙0=246 km. It is assumed that the sensor measures Cartesian coordinatesX andY directly. It is also assumed thatσX=σY=100 mand the sample rateT=1.(ii) Algorithms’ Parameters. UKF controlled ACT model’s parameter:
(43)α=10-3,β=2,κ=0QACT1=diag{1110-4}QACT2=diag{110-4}.Kinematic constraint model’s parameter:(44)QCA=diag{11}δ=0.92,r0=1,r1=200.Maneuver centered model’s parameter:(45)Qm=diag{10610-4}ρ=0.8.(iii) Results. The four models are listed as follows.Method 1: ACT model with Cartesian velocity.Method 2: ACT model with polar velocity.Method 3: kinematic constraint model.Method 4: maneuver-centered CT model.Root mean squared errors (RMSE) are used here for comparison. The RME position errors are defined as follows:(46)RMS.P.E(k)=1M∑i=1M[(xki-x^ki)2+(yki-y^ki)2],
where M=200 are the Monte-Carlo simulation runs. xki and yki stand for the true position, whilex^k∣kiand y^ki are the position estimates.The RMS position errors of all but the first ten are shown in Figure2.Figure 2
RMS position errors of the four methods.Table1 summarizes the average RMS of the position errors.Table 1
Average RMS of position errors.
Method
Average RMS of position errors (m)
1
94.26
2
81.57
3
109.51
4
183.46Table2 summarizes the relative computational complexity, normalized to method 4.Table 2
Relative computational load.
Method
Relative computational load
1
7.26
2
7.07
3
1.42
4
1It can be seen from the figure and tables that method 2 has the best performance and its computational load is roughly the same as method 1. So we can conclude that ACT model with polar velocity is better than ACT model with Cartesian velocity. Method 4 has the least computational load but its performance is poor. Method 3 is slightly more complex than method 4 but can decrease the error greatly. So if the computational load is of great concern, kinematic constraint model is a good choice.


## 3.1. UKF Filter with ACT Models

If the turn rate is augmented to the state vector, it will become a nonlinear problem. The extended Kalman filter (EKF) has been used to track this kind of motion. Since unscented Kalman filter (UKF) is very suitable for nonlinear estimation [4, 5], here the UKF algorithm is introduced.(i) Calculate the Weights of Sigma Points
(31)
W
0
m
=
λ
(
n
+
λ
)
W
0
c
=
λ
(
n
+
λ
)
+
(
1
-
α
2
+
β
)
W
i
m
=
W
i
c
=
0.5
(
n
+
λ
)
,
i
=
1,2
,
…
2
n
,
wheren is the dimension of the state vector. λ=α2(n+κ)-n is a scaling parameter. α determines the sigma points aroundx- and is usually set to a small positive value (e.g., 1e-3). κ is a secondary scaling parameter which is usually set to 0, and β=2 is optimal for Gauss distributions. Where the((n+λ)Px)i is theith row of the matrix square root.(ii) Calculate the Sigma Points
(32)
ξ
k
-
1
∣
k
-
1
0
=
X
^
k
-
1
∣
k
-
1
ξ
k
-
1
∣
k
-
1
(
i
)
=
X
^
k
-
1
∣
k
-
1
+
(
(
n
+
λ
)
P
x
)
i
i
=
1,2
,
…
,
n
ξ
k
-
1
∣
k
-
1
(
i
)
=
X
^
k
-
1
∣
k
-
1
-
(
(
n
+
λ
)
P
x
)
i
i
=
n
+
1
,
n
+
2
,
…
,
2
n
.(iii) Time Update
(33)
ξ
k
(
i
)
=
f
k
(
ξ
k
-
1
|
k
-
1
(
i
)
)
,
i
=
0,1
,
…
2
n
X
^
k
∣
k
-
1
=
∑
i
=
0
2
n
W
i
m
ξ
k
(
i
)
P
k
∣
k
-
1
=
∑
i
=
0
2
n
W
i
c
(
ξ
k
(
i
)
-
X
^
k
∣
k
-
1
)
(
ξ
k
(
i
)
-
X
^
k
∣
k
-
1
)
′
+
G
Q
k
-
1
G
′
.(iv) Measurement Update. Because we assume the measurement equation is linear, the following is just the same as the traditional Kalman filter:
(34)z^k∣k-1=HX^k∣k-1Sk=HPk∣k-1H′+RkKk=Pk∣k-1H′Sk-1X^k∣k=X^k∣k-1+Kk(zk-zk|k-1)Pk∣k=Pk∣k-1-KkSkKk′.
For the cases where the measurement equation is also nonlinear, the measurement update can be referred to [10] for details.


## 3.2. Kinematic Constraint Tracking Filter

The Kalman filtering equations for processing this kinematic constraint as a pseudomeasurement are given below, where the filtered state estimate and error covariance after the constraint have been applied are denoted byXk∣kC and Pk∣kC, respectively [8].(i) Time Update
(35)
X
^
k
∣
k
-
1
=
F
X
^
k
-
1
∣
k
-
1
C
P
k
∣
k
-
1
=
F
P
k
-
1
∣
k
-
1
C
F
′
+
G
Q
k
-
1
G
′
.(ii) Measurement Update. The measurement update is the same as (34).(iii) Constraint Update
(36)
K
k
C
=
P
k
∣
k
C
k
T
[
C
k
P
k
∣
k
C
k
′
+
R
k
μ
]
-
1
X
^
k
∣
k
C
=
[
I
-
K
k
C
C
k
]
X
^
k
∣
k
P
k
∣
k
C
=
[
I
-
K
k
C
C
k
]
P
k
∣
k
,
where
(37)Ck=1Sk∣k[00x˙^k∣k00y˙^k∣k].


## 3.3. Maneuver-Centered Tracking Filter

(i) Estimating Center of Maneuver. The center of the maneuver should be estimated from the measurements. It can be estimated through least square method which requires an iterative search procedure. The following simple geometrically oriented procedure of estimating the center was proposed in [9]. The main idea is as follows: if two points are on a circle then the perpendicular bisector of the chord between those points will pass through the center of the circle. The slope(m) andy intercept(b)of the perpendicular bisector is given by
(38)m=(x1-x2)(y2-y1)b=(y1+y2)2-m(x1+x2)2,
where(x1,y1)and(x2,y2)are the coordinates of the two points. The center can be given by
(39)x^c=(b1-b2)(m2-m1)y^c=(m1b2-m2b1)(m1-m2).(ii) Maneuver Detection. In the absence of a maneuver, the target is assumed to be traveling in a straight line and modeled by a constant velocity (CV) motion. (CV model is very simple and commonly used, which will not be listed here.) When the maneuver is detected, the filter switches to the maneuver-center CT model. While the end of a maneuver is detected, the filter will then switch back to CV model.Here a fading memory average of the innovations is used to detect if a maneuver occurs. The equation is given by(40)uk=ρuk-1+dk
with
(41)dk=νk′Sk-1νk,
where0<ρ<1, νkis the innovation vector, andSkis its covariance matrix.u
kwill have a chi-squared distribution with degrees
(42)nu=nz1+ρ1-ρ,
wherenzis the dimension of the measurement vector. Whenukexceeds a threshold (e.g., 95% or 99% confidence interval), then a maneuver onset is declared. The end time of a maneuver will be determined in a similar fashion. The procedure can be referred to [9] for details.


## 3.4. Simulation Results

(i) The Scenario. The scenario simulated here is very similar to that described in [20]. It includes few rectilinear stages and few CT maneuvers. Four consecutive 180∘ turns with rates ω=1.87, −2.8, 5.6, −4.68 are simulated, respectively, for scans [56,150], [182,245], [285,314], and [343,379]. The target trajectory can be seen in Figure 1.Figure 1
The test trajectory.The initial target position and velocities areX0=60 km, Y0=40 km, X˙0=-172 km, and Y˙0=246 km. It is assumed that the sensor measures Cartesian coordinatesX andY directly. It is also assumed thatσX=σY=100 mand the sample rateT=1.(ii) Algorithms’ Parameters. UKF controlled ACT model’s parameter:
(43)α=10-3,β=2,κ=0QACT1=diag{1110-4}QACT2=diag{110-4}.Kinematic constraint model’s parameter:(44)QCA=diag{11}δ=0.92,r0=1,r1=200.Maneuver centered model’s parameter:(45)Qm=diag{10610-4}ρ=0.8.(iii) Results. The four models are listed as follows.Method 1: ACT model with Cartesian velocity.Method 2: ACT model with polar velocity.Method 3: kinematic constraint model.Method 4: maneuver-centered CT model.Root mean squared errors (RMSE) are used here for comparison. The RME position errors are defined as follows:(46)RMS.P.E(k)=1M∑i=1M[(xki-x^ki)2+(yki-y^ki)2],
where M=200 are the Monte-Carlo simulation runs. xki and yki stand for the true position, whilex^k∣kiand y^ki are the position estimates.The RMS position errors of all but the first ten are shown in Figure2.Figure 2
RMS position errors of the four methods.Table1 summarizes the average RMS of the position errors.Table 1
Average RMS of position errors.
Method
Average RMS of position errors (m)
1
94.26
2
81.57
3
109.51
4
183.46Table2 summarizes the relative computational complexity, normalized to method 4.Table 2
Relative computational load.
Method
Relative computational load
1
7.26
2
7.07
3
1.42
4
1It can be seen from the figure and tables that method 2 has the best performance and its computational load is roughly the same as method 1. So we can conclude that ACT model with polar velocity is better than ACT model with Cartesian velocity. Method 4 has the least computational load but its performance is poor. Method 3 is slightly more complex than method 4 but can decrease the error greatly. So if the computational load is of great concern, kinematic constraint model is a good choice.


## 4. The Expectation Maximization (EM) Algorithm for Tracking CT Motion Target

In this part, the model in Section2.1 is used.The turn rateωk can be described by a Markov chain [21, 22] and has r possible values:
(47)ωk∈Mr={ω(1),ω(2),…,ω(r)}.Assume the initial probabilityτi and the one-step transition matrix are known, as follows:
(48)τi=p(ω0=ω(i)),i=1,2,…,rπi,j=p(ωk+1=ω(j)∣ωk=ω(i)),i,j=1,2,…,r.The measurement sequence is defined byZ1:N={z1,z2,…,zN}, state sequence is X1:N={X1,X2,…,XN}, and maneuver sequence is Ω1:N={ω1,ω2,…,ωN}.


### 4.1. Batch EM Algorithm

Assume the measurement sequence is known, this algorithm focuses on finding the best maneuver sequence based on MAP criterion. There is one best maneuver sequenceΩ1:N(B)  in rN possible sequences that makes the conditional probability density function be the maximum. When Ω1:N(B) is achieved, the state sequence X1:N can be estimated accurately.According to EM algorithm,Z1:N is considered to be the incomplete data, X1:N to be the “lost” data, and Ω1:N to be the data that needs to be estimated. EM algorithm carries out the following two steps iteratively.(1)  Expectation Step (E step)
(49)
J
(
Ω
1
:
N
,
Ω
1
:
N
(
j
)
)
=
E
X
1
:
N
{
ln
p
(
X
1
:
N
,
Z
1
:
N
,
Ω
1
:
N
)
∣
Z
1
:
N
,
Ω
1
:
N
(
j
)
}
,
where J(Ω1:N,Ω1:N(j)) is defined as the cost function, Ω1:N(j) is the maneuver sequence estimation afterjtimes iteration.(2)  Maximization step (M step)
(50)
Ω
1
:
N
(
j
+
1
)
=
arg
max
Ω
1
:
N
J
(
Ω
1
:
N
,
Ω
1
:
N
(
j
)
)
.
If the initial value is given, the above E step and M step are carried out repeatedly, until convergence.(i) E step. The union probability density function can be decomposed as follows:
(51)p(X1:N,Z1:N,Ω1:N)=∏k=1Np(zk∣Xk)×∏k=1Np(Xk∣Xk-1,ωk-1)×p(X0)×∏i=1Np(ωiωi-1)×p(ω0).p(Xk∣Xk-1,ωk-1) and p(ωi∣ωi-1) rely on the maneuver sequence Ω1:N. The state equation is Gaussian distribution:
(52)p(Xk∣Xk-1,ωk-1)=N{Xk-F(ωk-1)Xk-1,Qk},
where N{μ;Σ} is the Gaussian probability density function with mean μ and covariance Σ.From the above analysis,(53)J(Ω1:N,Ω1:N(j))=EX1:N{lnp(X1:N,Z1:N,Ω1:N)∣Z1:N,Ω1:N(j)}=∑k=1N{lnp(ωk∣ωk-1)-12(X^k∣N-F(ωk-1)X^k-1∣N)′×Qk-1(X^k∣N-F(ωk-1)X^k-1∣N)12},
where
(54)X^k|N=E[Xk∣Z1:N,Ω1:N(j)].Those terms which are independent ofΩ1:N are omitted here.In the E step, ifΩ1:N(j) is given, the cost function can be achieved using Kalman smoothing algorithm.(ii) M step. In the maximization step, a new Ω1:N is chosen for a higher conditional probability. Then a better parameter estimation is achieved compared to the former iteration. The following Viterbi algorithm can solve this problem perfectly.Viterbi algorithm is a recursive algorithm looking for the best path. As shown in Figure3, the path connects the adjacent points with the weights to be the logarithm function of the likelihood, named cost. The path’s total cost is the sum of its each point’s cost. The best path has the maximum cost. The detailed method to find the best path can be found in [12].Figure 3
Viterbi algorithm for path following.(iii) Calculating Algorithm
(1)
Initialization:the initial maneuver sequence Ω1:N(1) and threshold ε should be given.
(2)
Iteration:for each circle (j=1,2,…), carry out the following steps: (1) E step, according to (53), calculate the cost between the adjacent point. (2) M step, according to Viterbi algorithm, find a better maneuver sequence.
(3)
Stop: if ∥Ω1:N(j+1)-Ω1:N(j)∥≤ε, then stop the iteration. The best maneuver sequence is Ω1:N(B)=Ω1:N(j+1); then the state estimation sequence is calculated according to Ω1:N(B).


### 4.2. Recursive EM Algorithm

In target tracking applications, the target’s state always needs online estimation. So a recursive EM algorithm is needed for calculatingωk.(i) Recursive Equation. Under the MAP criterion,
(55)Ω1:k(B)=argmaxΩ1:k{p(Ω1:k∣Z1:k)},
where p(Ω1:k∣Z1:k) can be calculated online. 								(56)p(Ω1:k∣Z1:k)=p(Ω1:k∣zk,Z1:k-1)=p(zk∣Ωk,Z1:k-1)p(Ωk∣Z1:k-1)p(zk∣Ω1:k-1)=p(zk∣Ω1:k,Z1:k-1)p(ωk∣Ω1:k-1)×p(Ω1:k-1∣Z1:k-1)(p(zk∣Z1:k-1))-1.
Because Ω1:k is Markov chain,
(57)p(ωk∣Ω1:k-1)=p(ωk∣ωk-1).
The possible maneuver sequence grows exponentially as the time grows. For the computation to be feasibility, it is assumed that
(58)p(zk∣Ω1:k,Z1:k-1)≈p(zk∣ωk,Z1:k-1)=N(υk,Sk),
where υk is the Kalman filter’s innovation and Sk is the covariance of the innovation.The cost function is defined as(59)J(ωk(i))=lnp(Ω1:k,ωk(i)Z1:k),i=1,2,…,r,
which stands for the cost to model i until time k.From (57) to (59),
(60)J(ωk(j))=J(ωk-1(i))+lnπij-12υk′(i,j)Sk-1(i,j)υk(i,j)i,j=1,2,…,r,
where υk(i,j) stands for the innovation when modeli is chosen in time k-1 and modeljis chosen in time k. Sk(i,j) is the corresponding covariance.Because of using the assumption (58), the iteration algorithm is not the optimal algorithm under MAP criterion, but a suboptimal one.(ii) Calculating Algorithm. Only one-step iteration is listed here.(1)
E Step Calculation. Using (10), calculate each cost from time k-1 to k; r2 costs are needed.(2)
M Step Calculation. According to Viterbi algorithm, find out the maximum cost Jmax(ωk(i)) related to each model. Jmax(ωk(i)) is the initial value to be the next iteration.(3)
Filtering. According to the path which reaches each model, calculate each model’s state estimation X^k(i) and covariance Pk∣k(i), i=1,2,…,r.(4)
The Final Results. From Jmax(ωk(i)), i=1,2,…,r, choose the maximum one as the final filtering result:
(61)j=argmaxi{Jmax(ωk(i))}i=1r.(62)X^k(B)=X^k(j),Pk∣k(B)=Pk∣k(j).


### 4.3. Simulation Results

(i) Simulation Scenario. Target initial state is X0=[60000m-172m/s40000m246m/s]′. The sample rate T=1 s. The covariance of process noise
(63)Q=[Qx00Qy],Qx=Qy=[T43T32T32T2].Assume only position can be measured, the measurement equation is the following:(64)zk=[10000010]Xk+vk.The covariance of measurement noiseR=2500I, where I is the 2 × 2 unit matrix.The simulation lasts for 300 s. Target’s true turn rate is(65)ωk={00≤k<1030.033rad/s104≤k<1980198≤k<300.Figure4 gives the target’s true trajectory.Figure 4
Target trajectory.Assume target’s maximum centripetal acceleration is30 m/s2. Under the speed300 m/s, the corresponding turn rate is 0.1 rad/s. Seven models are used for this simulation. From −0.1 to 0.1, the seven models are distributed evenly. Their values are −0.1, −0.067, −0.033, 0, 0.033, 0.067, and 0.1. The initial probability matrix is
(66)τ=[17171717171717].The model transition matrix is(67)πi,j={0.7i=j0.05i≠ji,j=1,2,…,7.(ii) Simulation Results and Analysis. Batch EM algorithm, recursive algorithm, and IMM algorithm are compared in this scenario. Root mean squared errors (RMSE) are used here for comparison. The RME position errors are defined as (46) and velocity error are defined as follows:
(68)RMS.V.E(k)=1M∑i=1M[(x˙ki-x˙^ki)2+(y˙ki-y˙^ki)2],
where M=200 are Monte-Carlo simulation runs and x˙ki, y˙ki and x˙^ki, y˙^ki stand for the true and estimated velocity at timek in the ith simulation runs, respectively.Figures5 and 6 show the position and velocity performance comparison. It can be concluded that the batch EM algorithm has much less tracking errors compared to IMM algorithm. During maneuver onset time and termination time, the IMM algorithm is better than recursive EM algorithm. But on stable period, the recursive EM algorithm performs better.Figure 5
Position performance comparison.Figure 6
Velocity performance comparison.


## 4.1. Batch EM Algorithm

Assume the measurement sequence is known, this algorithm focuses on finding the best maneuver sequence based on MAP criterion. There is one best maneuver sequenceΩ1:N(B)  in rN possible sequences that makes the conditional probability density function be the maximum. When Ω1:N(B) is achieved, the state sequence X1:N can be estimated accurately.According to EM algorithm,Z1:N is considered to be the incomplete data, X1:N to be the “lost” data, and Ω1:N to be the data that needs to be estimated. EM algorithm carries out the following two steps iteratively.(1)  Expectation Step (E step)
(49)
J
(
Ω
1
:
N
,
Ω
1
:
N
(
j
)
)
=
E
X
1
:
N
{
ln
p
(
X
1
:
N
,
Z
1
:
N
,
Ω
1
:
N
)
∣
Z
1
:
N
,
Ω
1
:
N
(
j
)
}
,
where J(Ω1:N,Ω1:N(j)) is defined as the cost function, Ω1:N(j) is the maneuver sequence estimation afterjtimes iteration.(2)  Maximization step (M step)
(50)
Ω
1
:
N
(
j
+
1
)
=
arg
max
Ω
1
:
N
J
(
Ω
1
:
N
,
Ω
1
:
N
(
j
)
)
.
If the initial value is given, the above E step and M step are carried out repeatedly, until convergence.(i) E step. The union probability density function can be decomposed as follows:
(51)p(X1:N,Z1:N,Ω1:N)=∏k=1Np(zk∣Xk)×∏k=1Np(Xk∣Xk-1,ωk-1)×p(X0)×∏i=1Np(ωiωi-1)×p(ω0).p(Xk∣Xk-1,ωk-1) and p(ωi∣ωi-1) rely on the maneuver sequence Ω1:N. The state equation is Gaussian distribution:
(52)p(Xk∣Xk-1,ωk-1)=N{Xk-F(ωk-1)Xk-1,Qk},
where N{μ;Σ} is the Gaussian probability density function with mean μ and covariance Σ.From the above analysis,(53)J(Ω1:N,Ω1:N(j))=EX1:N{lnp(X1:N,Z1:N,Ω1:N)∣Z1:N,Ω1:N(j)}=∑k=1N{lnp(ωk∣ωk-1)-12(X^k∣N-F(ωk-1)X^k-1∣N)′×Qk-1(X^k∣N-F(ωk-1)X^k-1∣N)12},
where
(54)X^k|N=E[Xk∣Z1:N,Ω1:N(j)].Those terms which are independent ofΩ1:N are omitted here.In the E step, ifΩ1:N(j) is given, the cost function can be achieved using Kalman smoothing algorithm.(ii) M step. In the maximization step, a new Ω1:N is chosen for a higher conditional probability. Then a better parameter estimation is achieved compared to the former iteration. The following Viterbi algorithm can solve this problem perfectly.Viterbi algorithm is a recursive algorithm looking for the best path. As shown in Figure3, the path connects the adjacent points with the weights to be the logarithm function of the likelihood, named cost. The path’s total cost is the sum of its each point’s cost. The best path has the maximum cost. The detailed method to find the best path can be found in [12].Figure 3
Viterbi algorithm for path following.(iii) Calculating Algorithm
(1)
Initialization:the initial maneuver sequence Ω1:N(1) and threshold ε should be given.
(2)
Iteration:for each circle (j=1,2,…), carry out the following steps: (1) E step, according to (53), calculate the cost between the adjacent point. (2) M step, according to Viterbi algorithm, find a better maneuver sequence.
(3)
Stop: if ∥Ω1:N(j+1)-Ω1:N(j)∥≤ε, then stop the iteration. The best maneuver sequence is Ω1:N(B)=Ω1:N(j+1); then the state estimation sequence is calculated according to Ω1:N(B).


## 4.2. Recursive EM Algorithm

In target tracking applications, the target’s state always needs online estimation. So a recursive EM algorithm is needed for calculatingωk.(i) Recursive Equation. Under the MAP criterion,
(55)Ω1:k(B)=argmaxΩ1:k{p(Ω1:k∣Z1:k)},
where p(Ω1:k∣Z1:k) can be calculated online. 								(56)p(Ω1:k∣Z1:k)=p(Ω1:k∣zk,Z1:k-1)=p(zk∣Ωk,Z1:k-1)p(Ωk∣Z1:k-1)p(zk∣Ω1:k-1)=p(zk∣Ω1:k,Z1:k-1)p(ωk∣Ω1:k-1)×p(Ω1:k-1∣Z1:k-1)(p(zk∣Z1:k-1))-1.
Because Ω1:k is Markov chain,
(57)p(ωk∣Ω1:k-1)=p(ωk∣ωk-1).
The possible maneuver sequence grows exponentially as the time grows. For the computation to be feasibility, it is assumed that
(58)p(zk∣Ω1:k,Z1:k-1)≈p(zk∣ωk,Z1:k-1)=N(υk,Sk),
where υk is the Kalman filter’s innovation and Sk is the covariance of the innovation.The cost function is defined as(59)J(ωk(i))=lnp(Ω1:k,ωk(i)Z1:k),i=1,2,…,r,
which stands for the cost to model i until time k.From (57) to (59),
(60)J(ωk(j))=J(ωk-1(i))+lnπij-12υk′(i,j)Sk-1(i,j)υk(i,j)i,j=1,2,…,r,
where υk(i,j) stands for the innovation when modeli is chosen in time k-1 and modeljis chosen in time k. Sk(i,j) is the corresponding covariance.Because of using the assumption (58), the iteration algorithm is not the optimal algorithm under MAP criterion, but a suboptimal one.(ii) Calculating Algorithm. Only one-step iteration is listed here.(1)
E Step Calculation. Using (10), calculate each cost from time k-1 to k; r2 costs are needed.(2)
M Step Calculation. According to Viterbi algorithm, find out the maximum cost Jmax(ωk(i)) related to each model. Jmax(ωk(i)) is the initial value to be the next iteration.(3)
Filtering. According to the path which reaches each model, calculate each model’s state estimation X^k(i) and covariance Pk∣k(i), i=1,2,…,r.(4)
The Final Results. From Jmax(ωk(i)), i=1,2,…,r, choose the maximum one as the final filtering result:
(61)j=argmaxi{Jmax(ωk(i))}i=1r.(62)X^k(B)=X^k(j),Pk∣k(B)=Pk∣k(j).


## 4.3. Simulation Results

(i) Simulation Scenario. Target initial state is X0=[60000m-172m/s40000m246m/s]′. The sample rate T=1 s. The covariance of process noise
(63)Q=[Qx00Qy],Qx=Qy=[T43T32T32T2].Assume only position can be measured, the measurement equation is the following:(64)zk=[10000010]Xk+vk.The covariance of measurement noiseR=2500I, where I is the 2 × 2 unit matrix.The simulation lasts for 300 s. Target’s true turn rate is(65)ωk={00≤k<1030.033rad/s104≤k<1980198≤k<300.Figure4 gives the target’s true trajectory.Figure 4
Target trajectory.Assume target’s maximum centripetal acceleration is30 m/s2. Under the speed300 m/s, the corresponding turn rate is 0.1 rad/s. Seven models are used for this simulation. From −0.1 to 0.1, the seven models are distributed evenly. Their values are −0.1, −0.067, −0.033, 0, 0.033, 0.067, and 0.1. The initial probability matrix is
(66)τ=[17171717171717].The model transition matrix is(67)πi,j={0.7i=j0.05i≠ji,j=1,2,…,7.(ii) Simulation Results and Analysis. Batch EM algorithm, recursive algorithm, and IMM algorithm are compared in this scenario. Root mean squared errors (RMSE) are used here for comparison. The RME position errors are defined as (46) and velocity error are defined as follows:
(68)RMS.V.E(k)=1M∑i=1M[(x˙ki-x˙^ki)2+(y˙ki-y˙^ki)2],
where M=200 are Monte-Carlo simulation runs and x˙ki, y˙ki and x˙^ki, y˙^ki stand for the true and estimated velocity at timek in the ith simulation runs, respectively.Figures5 and 6 show the position and velocity performance comparison. It can be concluded that the batch EM algorithm has much less tracking errors compared to IMM algorithm. During maneuver onset time and termination time, the IMM algorithm is better than recursive EM algorithm. But on stable period, the recursive EM algorithm performs better.Figure 5
Position performance comparison.Figure 6
Velocity performance comparison.


## 5. Conclusions

Aiming at the CT motion target tracking, several models and algorithms are introduced and simulated in this paper.In single model framework, four CT models have been compared for tracking applications: ACT model with Cartesian velocity, ACT model with polar velocity, kinematic constraint model, and maneuver-centered model. The Monte-Carlo simulations show that the ACT model with polar velocity has the best tracking performance but the computational load is a bit heavier. The kinematic constraint model has a moderate tracking performance, but its computational load decreases greatly compared with UKF controlled ACT model. So if the computational load is of a great concern, the kinematic constraint model is suggested. If the tracking performance is very important and the computational load is not a problem, the ACT model with polar velocity is suitable.In multiple models framework, EM algorithm is used for tracking CT motion target. First a batch EM algorithm is derived. The turn rate is acted as the maneuver sequence and estimated based on the MAP criterion. Under the E step, the cost function is calculated using the Kalman smoothing algorithm. Under the M step, Viterbi algorithm is used for path following to find out the path with maximum cost. Simulation results show that the Batch EM algorithm has better tracking performance than IMM algorithm. Through modification of the cost function, a recursive EM algorithm is presented. The algorithm can track the target online. Compared with the IMM algorithm, on the stable period, the recursive EM algorithm has better tracking performance.



---

*Source: 649276-2014-01-12.xml*