An IMM EKF Approach For Enhanced Multi-Target State Estimation For App
An IMM EKF Approach For Enhanced Multi-Target State Estimation For App
fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 1
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 2
The contributions of this paper are as follows. A standard follows: 1) to find the set of validated measurement for each
process model which can describe multiple modes of behavior target and 2) to find the minimum variance estimated state and
of target vehicle has been derived and discretized. Second, the associated error covariance matrix for each target.
through an analysis of the automotive radar characteristics, a
standard measurement model is derived and twelve specific
B. Coordinate Systems
measurement models are selected so that can cover all possible
measurement patterns. Finally, an IMM algorithm using In this subsection, coordinate systems used to derive
extended Kalman filters (IMM/EKF) for multi-target state standard process and measurement models are presented. These
estimation is provided with proper parameter design and coordinate systems are described in Fig.1. Frame {0} is the
validated via vehicle tests in real road. ground based fixed coordinate system of which the position is
same with the host vehicle, frame {1} is the host vehicle’s
II. PROBLOEM FORMULATION body-fixed moving coordinate system, frame {2} is the
sensor’s body-fixed moving coordinate system, and frame {3}
In this section, a stochastic hybrid system is formulated in
is the target vehicle’s body-fixed moving coordinate system.
the form of an IMM/EKF algorithm for tracking multiple target
And superscript means a corresponding time step. The standard
vehicles. A standard process model to describe behaviors of
process model describes the predicted state of the target
target vehicle is derived and discretized. And a standard
vehicle’s mass center on the frame of {0}k+1 based on the
measurement model is defined to describe automotive radar
current state of the target vehicle’s mass center and system
characteristics.
input which are defined on the frame of {0}k. And the standard
A. Stochastic hybrid system measurement model describes the state of measured point such
Assume that there are total Ntarget targets and the system of as position and longitudinal velocity on the sensor based
moving frame {2}.
each target can be modeled as one of the N mode hypothesized
models where the target set is denoted as T 1, 2, , Nta rget and
the model set is denoted as M 1, 2, , Nmode . From the work of
Li and Bar-shalom [8], a general description for a stochastic
hybrid system of target n Τ during the sampling period
tk 1 , tk can be given as:
xnj k fnj xnj k 1 , u k 1 wnj k 1 (1)
(a) at time k
z k h x k , u k v k
j
n
j
n
j
n
j
n (2)
where xnj is the state vector of target n when model
j M is in effect, u is the input vector(same for all targets),
fnj and hnj are the nonlinear and time invariant system
structure. The process noise w nj k and the measurement noise
v nj k are mutually uncorrelated zero-mean white Gaussian with
covariance Wnj k and Vnj k respectively. And a (possibly
(b) at time k+1
state dependent) Markovian transition probability of the system Fig.1. Four coordinate systems used to derive standard process model and
mode index is given as follows. standard measurement models
i, j Μ
(3) C. Standard Process Model
Pr mn k 1 j mn k i Φi , j xn k
Various driving patterns of a vehicle including straight line,
where mn k is the system mode index of target n which is curve, cut-in/out, U-turn and interchange can be represented by
in effect at time step k . a combination of a constant velocity rectilinear motion, a
The following notations and definitions are used regarding constant acceleration rectilinear motion, a constant angular
the measurement. In general, at any time k , some velocity curvilinear motion and a constant angular acceleration
measurements may be due to clutter and some due to the target, curvilinear motion [1].
i.e. there can be more than a single measurement [6]. The To describe all these motions, the state vector and input
measurement set (not yet validated or associated) is denoted as vector of target n are defined as follows:
Yk y1 k , y 2 k , , y N k where Nmeasure is the number of xn pn, x pn, y n n an, x n
T
measure
vn, x (4)
measurements. The set of validated measurement of target n at u vx
T
(5)
time k is denoted by
where subscript x and y denote x-axis and y-axis of each
Zk z1 k z 2 k , , z N k (4)
target
frame, subscript n denotes “of target n “, p denotes the
Therefore, the goals of the algorithm can be summarized as relative position denotes the relative yaw angle, v denotes
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 3
q ~ 0, Q
u u k
decay-rate of the longitudinal acceleration, k is the decay-rate where u is time-derivative of input vector where the
of the yaw-acceleration, and q is the continuous white process components’ physical meanings are host vehicle’s longitudinal
noise with covariance Q . Above continuous process model can acceleration and yaw-acceleration. As the result, xn k 1
be discretized via Taylor methods as follows [11]. describes the predicted state of the target vehicle’s mass center
x n k 1 x n t T on the frame of {0}k 1 . Note that various driving patterns of a
d 1 d2
xn t x n T 2 x n T 2 h.o.t. vehicle can be represented by the above standard discretized
dt 2 dt
process model by adjusting a and Q of continuous process
x n t a x n , u q T a x n , u q T 2
1 d
2 dt model. The details about specific modes of process model will
x n t a x n , u q T be discussed in following subsection, III-A.
1 2 D. Standard Measurement Model
2 x a x , u x a x , u u q T
u
n n n
n In an automotive target tracking, changes in the target aspect
x n t a x n , u q T with respect to the radar can cause the apparent point of radar
reflections (relative position seen by the antenna) to wander
1 x a x n , u n
a x , u q
(7) significantly [12]. To represent these characteristics, the
n
T
2
2 2 n n
the state of measured point on the sensor based moving frame
a p {2}. s is sensor position vector defined on the host vehicle’s
host n , y host
2 2 body-fixed moving frame {1} and b n is measured point vector
an sin n n vn cos n pn , x host of target n defined on the target vehicle’s body-fixed moving
2 2 2 2 pn , x host
f 2 T T pn , y frame {3}. The first order approximation of the measurement
host pn , y host vhost vn cos n v sin
n n error covariance has been presented in previous works as
2 follows [13].
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 4
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 5
Markovian probability governed by (3). Consequently, the A transition probability matrix of the proposed algorithm can
process model can be suitable described in the framework of be decomposed into two sub-matrices; one is the sub-transition
the stochastic hybrid systems. probability matrix related to multiple process models and the
other matrix is about multiple measurement models. Relation of
B. Set of Multiple Measurement Model
three matrices is given as:
As shown in Fig.4, by means of relative position and Φ1process Φmeasure Φ1process Φ measure Φ1process Φ measure
1 2 1
orientation between host and target vehicle, the configuration process measure
Φ21 Φ Φ Φ Φ2process process Φ
process measure measure
22 N mode
of measurement can be altered. Ideally, the detecting point Φ (14)
should be located on the four sides or four edges of the target
Φ process Φ measure Φ process
Φ measure
Φ N process N process Φ
process measure
vehicle. As an example, for the case of the cutting in vehicle, a Nmode
process
1 process
N mode 2 mode mode
rear right edge is the most likely measurement model while lane where Φ process is the matrix defining a transition probability
changing. However, after cutting-in, the most likely measured between process mode indices, N is the number of process process
mode
point is transited to a center of rear side of the target vehicle. model set, Φmeasure defines a transition probability between
As aforementioned, to describe these various measurement measurement mode indices. Φ process can be expressed in a single
patterns, several specific modes can be derived by adjusting b n matrix, but this is impossible in the case of Φmeasure as
in standard measurement model. From the viewpoint of aforementioned. To solve this problem, different measurement
detecting position defined on the target-body-fixed frame, a mode transition matrices are used depending on the target
specific mode set depicted in Fig.5 can be utilized to describe aspect event. All eight possible events are defined in Fig.6.
the variations of the measurement patterns. We can see that the From the work of Zhang and Bar-shalom [14], an approach
proposed mode set include twelve points around the boundary using state-dependent mode transition probabilities has been
of the target vehicle and all case examples in Fig.4 can be presented to track move-stop-move targets. And in our research,
represented well by the proposed mode set. Consequently, this approach is extended to eight stages and applied with some
derived set of measurement models can be suitable described modifications. Target index is omitted unless otherwise
by (2) of the stochastic hybrid systems. required for clarity. First, measurement mode transition
probability matrix with no event (physically not allowed) is
designed in Gaussian distribution manner as follows:
i, j Μ
1 1 d 2 (15)
Φimeasure ,0
exp i , j
2 d 2 d
,j
Θ E
i, j 0 i j M invalid
measure , E
(17)
Fig.5. Twelve modes of detected point of measurement model to describe 0
i j
various measurement patterns.
where Mvalid
measure , E
is the subset of valid measurement modes
C. Event Dependent Transition Probability Matrix
and Minvalid
measure , E
is the subset of invalid measurement modes on an
In a conventional IMM estimator the Markov chain of the
mode states is described by a single mode transition matrix. event E . Consequently, the conditional transition probability
However, unlike the conventional system, mode transition matrix has zero values as entries of columns of invalid modes,
process of an automotive radar system cannot be modeled by a which means that under that given event, transitions to invalid
single mode transition probability matrix. As can be seen in modes are not allowed.
Fig.6, transitions to invalid modes are infeasible since the radar Finally, the actual transition probability matrix of the system
cannot penetrate the vehicle’s body. And a subset of invalid at time step k is calculated as a combined form which can be
modes can be different for different situations. As an example, written as follows:
i E
mode transition from the front-right edge to rear-left edge is
(18)
impossible in event.1 while it is possible in event.8. Therefore,
i
Φmeasure Pr E k i z 0 ,
, z k Φmeasure,,i
we can conclude that the mode transition probabilities of the
automotive radar system are target aspect dependent and multi Where E i i 1, ,8 is a set of eight event indexes and
transition matrices should be derived to describe all cases of the
Pr E k i z 0 , , z k is an event probability which means the
mode jump process.
probability of the target being in a specific event i at time step
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 6
Fig.6. Set of measurement model, their two subsets classified by validity (of each target vehicle in multi traffics), and eight events of target aspect.
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 7
Measurement update:
the mixed initial condition for filter matched to mode state j . xˆ nj k xnj k K nj k rnj k (36)
Note that, when the modes used have different dimension state ˆ j k I K j k H j k P j k
P (37)
n n n n
vectors, an augmentation with zeros the lower dimension state
3) Mode Probability Update:
estimate may lead to a bias toward zero for the state
The N mode weights ni are updated from the innovations of
components of the larger state vector. In the previous work, a
simple procedure to avoid this “biasing” is presented, together the N mode extended Kalman filters.
with a suitable augmentation of the covariance of the smaller Likelihood function:
1
exp rnj k Snj rnj k
state that yields an unbiased and consistent mixing [15]. nj 2 Snj
1/2 T 1
(38)
Predicted mode probability: 2
nj Pr mn k j z n k 1 Mode probability:
(25) nj nj
Φij k 1 ni k 1 nj (39)
i ni in
i
Mixing probability:
4) Combination:
ni j Pr mn k 1 i mn k j , z n k 1
for output purpose only, xˆ n k and Pˆ n k are computed
Φij k 1 ni k 1 (26)
according to
j
n
Combined condition:
Mixed condition: xˆ n k xˆ nj k nj (40)
xˆ 0n j k 1 xˆ in k 1 ni j (27) j
Pˆ n k Pˆ nj k xˆ nj k xˆ n k xˆ njj k xˆ n k nj
i T
(41)
xˆ n k 1 xˆ n k 1
i 0j
i j j
Pˆ n0 j k 1 Pˆ ni k 1 T
(28)
xˆ n k 1 xˆ n k 1
i 0j n
i
C. Track Management
2) Extended Kalman Filtering: In track management, there are three processes required
Each of the N mode pairs weights xˆ 0n j k 1 , Pˆ n0 j k 1 is used as during each time step: track update, track deletion and track
input to an extended Kalman filter matched to mode state j . creation. In this study, each track has a rewarding counter that is
Time update: rewarded when the track is updated with validated
fnj measurement and decreased when the measurement is missed.
Fnj k 1 (29) In the part of track update, the measurements are associated
x xˆ 0n j k 1,uˆ k 1
with the existing tracks. Following (28), the most likely
xnj k fnj xˆ 0n j k 1 , uˆ k 1 (30)
candidate measurement for target n at time k can be defined as
P k F k 1 P
ˆ k 1 F k 1 W k 1 T
n
j
n
j 0j
n n
j
n
j
(31) follows:
k min y zˆ n k Sn k y zˆ n k
T 1
Filter gain: z candi
n (42)
yY
h nj
k
H nj k (32) where
x xnj k ,uˆ k
zˆ nj k hnj xnj k , uˆ k
Snj k Hnj k Pnj k Hnj k Vnj k
T
(33)
zˆ n k zˆ nj k nj
K nj k Pnj k Hnj k Snj k
T 1
(34) j
Innovation:
Sn k Snj k zn k znj k zn k znj k
T
n
j
r k z n k hnj xnj k , uˆ k
j
j
(35)
k is validated as z n k if
n
The candidate measurement z candi
n
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 8
and only if situation are shown in Fig.12. We can see that, from 0-12s, the
z k zˆ n k Sn k z k zˆ n k rth
1
candi T
(43)
candi target vehicle is detected by rear radar and left side of the target
n n
is in effect during that period. A period from 12-14s is blind
where rth is the gate threshold corresponding to the gate
area where there is no measurement for the target vehicle by
probability PG . Note that all targets share a common threshold. any radar. From 14-20s, the target vehicle is detected by front
If the track has no validated measurement, the rewarding radar and rear side of the target is in effect during that period.
counter is decreased and the time-prediction is performed as the
current estimate (which means a zero-gain EKF). If a rewarding
counter drops below a certain threshold, the track is deleted.
And newly arriving measurements generate new tracks. The
initial state of new track is decided by a model selection
strategy depicted in Fig.15. And the new track is assumed to
have the same speed with the host vehicle, zero yaw-rate, zero
acceleration, and zero yaw acceleration. Additionally, some
large value of error covariance is given for the newly generated
target.
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 9
Fig.14. Estimated target states and the distributions of the estimation errors
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 10
TABLE 1
STATISTICS OF ESTIMATION ERRORS (N=400)
Model-switching/EKF IMM/EKF
Standard (Ratio to Standard (Ratio to
RMS (Ratio to IMM) (Ratio to IMM) RMS
Deviation Model-Switch) Deviation Model-Switch)
algorithm is approximately three times more accurate than the The selected test set is a complex driving scene where the
model-switching/EKF. targets may enter and leave the field of view, showing the
Finally, computing complexity of these two approaches have capability for birth and death of targets.
been compared. An average consumption time of IMM/EKF Fig.16 shows a frontal image of the vision camera, and the
algorithm is about 4.9806ms and mode-switching/EKF is about multi-target state estimation results in complex driving
1.6787ms. When considering that IMM uses twelve filters in situation. The radar measurements are depicted by blue circles
parallel, this three times more complexity is relatively good with inner star. And based on the lane information, a region of
performance. Moreover, although IMM based algorithm has interest is defined and the outside is represented by red area. As
high complexity compared to mode-switching method, as the can be seen in the figure, it is evident that the proposed
sampling rate of the overall algorithm is 100ms, computing algorithm manages to follow the correct true targets. A true
complexity of IMM/EKF algorithm is not in question. target is most generally defined to be an object that will persist
in the tracking volume for at least several scans. False alarms
(or false targets) refer to erroneous detection events (such as
those caused by random noise or clutter) that do not persist over
several scan. In the figure, we can show that those events
(caused by road struchers such as guardrail in red area) cannot
generate new track. At t=8sec, the proposed algorithm tracks 5
targets. And at t=13sec, one target enters the field of view and
track#19 is newly generated. And at the same time track#10
starts leaving the sensing range. At t=15.5sec, track#10 leaves
the sensing range completely, and zero-gain EKF is applied.
The track#14 starts its lane changing and cuts-in front of the
host vehicle. Due to this lane changing, a measurement of
hidden vehicle is newly arrived. And at t=20sec, track#12 is
newly generated from this measurement while tracks other
existing 4 targets. In summary, these results indicate that GNN
based track management can be utilized with satisfactory. The
GNN based track management approach, which considers the
single most likely hypothesis for track update and new track
initiation, works well because of widely spaced targets,
Fig.15. Architecture of model-switching/EKF Algorithm relatively accurate measurements in the given experiment
situation. However, the problem of closely-spaced unresolved
targets can occur and this can be solved by MHT which forms
VI. EXPERIMENTAL RESULTS WITH MULTI-TARGET SITUATION
multiple hypotheses and manages these hypotheses [17].
In this section, the experimental results dealing with Fig.17 presents the target state estimation results for
multi-target scenarios are presented. Real-road driving data track#14. Each graph shows each state variables, rewarding
were collected from a highway and an urban area. The camera function, and mode probability update result as a function of
is used to analyze the driving situation and to acquire the lane time. As can be inferred from the vision image, the vehicle of
information. Especially, one test set for validation is extracted track#14 changes the lane and cuts into the host vehicle’s lane.
from the collected real-driving data to explore the multi-target From the figure, we can see that the overall trends of each
tracking performance and target state estimation performance. estimated state can represent actual target motion with
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 11
satisfactorily while the rewarding function of track#14 driving in the left lane during that period. And a transition to
maintains the maximum value (100 in this paper). In the case of mode.1 (center of rear side) from mode.12 has occurred in a
mode probability update results, from 0-18 sec, mode.12 period of lane changing. Finally, the probability of mode.1
(rear-right edge) is evaluated as the most highly likely mode. ranked highest after the target vehicle becomes the in-lane
This can be explained by the fact that the target vehicle was preceding vehicle. Overall, the mode evaluation results are
Fig.17. Target state estimation results for Track 14 (a target cutting in front of Fig.18. Target state estimation results for Track 10 (a target vanishing to
the host vehicle) rear-right-blind area)
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 12
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI
10.1109/TVT.2014.2329497, IEEE Transactions on Vehicular Technology
VT-2013-01321.R2 13
0018-9545 (c) 2013 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See
http://www.ieee.org/publications_standards/publications/rights/index.html for more information.