2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)
Multiple Moving Target Tracking with Hypothesis
Trajectory Model for Autonomous Vehicles
Weijie Mei, Guangming Xiong*, Jianwei Gong, Zhai Yong, Huiyan Chen, Huijun Di
Intelligent Vehicle Research Center Beijing Institute of Technology, Beijing 100081China
*corresponding author, xiongguangming @bit.edu.cn
Abstract - Detecting and tracking moving objects is a key
technology for autonomous driving vehicle in dynamic urban
environment. In this paper, we present a hybrid system of
multiple moving target tracking (MMTT) for autonomous driving
vehicles using 3D-Lidar sensor. To detect targets, geometric
model-based method is adopted in the hybrid system. In further,
we propose a novel hypothesis trajectory model to detect moving
targets. The targets detected by the two methods are fused under
Extend Kalman Filter. Besides, a bipartite graph model-based
method with the Hungary algorithm is presented to optimize data
association matrix. Our tracking algorithm is tested on school
road and Beijing’s 3rd ring road using an experimental vehicle
with velodyne-32-E. And, the experiment results illustrate the
hybrid method performs well in real time.
Keywords—autonomous driving vehicle, multiple moving
target tacking, hypothesis trajectory model, data association,
Extend Kalman Filter.
I. INTRODUCTION
Reliable environment perception is fundamental for
autonomous driving vehicles, and the perception system for
autonomous vehicles includes static obstacle detection and
dynamic object detection. Besides, an autonomous vehicle
should be able to make predictions about targets’ position and
behavior and prepare for next actions by tracking moving
objects and analyzing the objects’ motion state. As the moving
objects interfere with each other in dynamic scene, tracking
multiple moving targets in complex urban traffic roads is a
great challenge for the wide application of autonomous driving
vehicles.
Different types of sensors have been employed to deal
with the challenge, and there have been numerous fruits in this
field [1] [2]. In recent years, 3D-Lidar sensors have become
widely applied in area, because they provide rich and accurate
data of spatial information around the vehicle. Most methods
of multiple moving target tracking based 3D-Lidar can be
divided into two main steps [3]: detection and tracking, and
the tracking part includes data association and state updating.
In the detection step, 2.5D grid map is a popular and
efficient representation of the information of 3D point cloud
[4]. In [3], geometric model-based method can effectively
detect the vehicle driving in the road, according to different
geometry features of objects in grid map. However, as the
laser scanner is dispersed in variance distance around 3D-
Lidar, the geometric model-based method is unable to detect
978-1-5386-1526-3/17/$31.00 ©2017 IEEE
the partially occluded or inexplicit objects or to generate error
results [5]. Azim et al. [6] represents the environment using an
octree-based occupancy grid and determining inconsistencies
between the map, incoming scans to detect dynamic objects
and classifying them into pedestrian, bicycle or vehicle. As
trajectory is another feature of the moving object, in our
system, we propose a hypothesis trajectory model by
collecting all probable trajectories, and pick out moving
targets according to the feature of them.
The data association between detected targets and tracked
targets is another problem for MMTT, as the wrong matching
in the process will lead to a wrong tracking. Researchers have
proposed many representative methods to solve this problem,
e.g Global Nearest Neighbor (GNN) [7], Joint Probabilistic
Data Association (JPDA) [8] and Multiple Hypothesis
Tracking (MHT) [9]. Therefore, the GNN method is used in
our system to establish a relationship matrix for current
detection targets and tracked targets. Mathematical methods,
such as graph theory, are introduced to optimize the match
assignment problem during data association. In [10], Yang et
al. propose a graph-based optimization to solve the association
problem. Hungarian method, a combinational optimization
algorithm, is successfully used to process the multiple-step
association stage for multi-pedestrian tracking in [11].
As the measurement error exists in the data from sensors,
researchers usually use filters to calculate estimation result as
the optimal outcome, such as Kalman filter (KF) [5], Extend
Kalman filter (EKF) [12] and Partical filter (PF) [4]. While KF
is useful liner filter with wide scope application, EKF and PF
are suit for nonlinear system. Considering that vehicle’s
moving state is non-linear and the system runs in real time,
EKF performs better than KF for the moving vehicle motion
state estimate.
II. SYSTEM CONFIGURATION
This paper presents a method of multiple moving target
tracking for autonomous driving vehicles with 3D-Lidar The
framework is shown in Fig.1, the method contains three steps:
1) Firstly, the cloud points from 3D-Lidar are projected into
the 2.5D grid map, and all candidate targets are extracted from
the grid map through the preprocessing part. 2) In detection
part, to detect the target from the candidates, geometric
model- based method is adopted. In further, a novel
hypothesis trajectory method is proposed to detect the moving
 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)

3D cloud
points Tracked
Targets list

2.5D Grid Targets Data

Geometric model- detected Tracked targets
Map Association with (.)
based targets detect Targets list state update
Hungary algorithm
Clustering Fused
Candidate targets
Targets list Hypothesis Detected Trajectories Data Tracked
trajectories Hypothesis Association with (.) Trajectories
collection trajectories list Hungary algorithm state update

Tracked
trajectories list

SUHSURFHVVLQJ 'HWHFWLRQ 7UDFNLQJ 5HVXOW

Fig. 1 Multi-target tracking algorithm framework
The framework is consist of three parts: preprocessing part, detection part and tracking part. In the preprocessing part, the Grid map is constructed and all
candidate targets are clustered. The geometric model-based targets list and hypothesis trajectories list are generated parallel from the candidate targets in the
detection part. Data association between detected target list and tracked target list is carried out in the tracking part, and the targets’ motion state updates with
EKF. Finally, the targets tracked through both method are fused as final result output.

connected component analysis to cluster the candidate targets,

targets, including that do not meet the requirement of the
the morphological closing operation is therefore applied on
geometric model. 3) In the tracking part, Data association
the binary image to adjoin the obstacle pixels belonging to the
between detected target list and tracked target list is carried
same object.
out, and the Extend Kalman Filter is employed to estimate and
update the target’s motion state. A correlation relationship
III. MOVING TARGET DETECT
matrix is established to deal with the data association problem,
and the matrix is optimized though a bipartite graph model A. Geometric Model-based Target Detect
with Hungary algorithm. Finally, as the geometric model- Moving vehicle around the autonomous driving vehicle
based target and the hypothesis trajectory model-based target presents a serial symbolic shapes at different viewpoints and
are come from the same candidate target, the targets tracked distances of the 3D-LIDAR in the grid map, such as "ᶨ", "I"
through both method are fused as final result output. and "L" shape [4]. According to these features, the geometric
A. Grid Map Construction model-based method to detect the target is applied. After the
First of all, a size of M × N 2.5D grid map around the process of connected component analysis, the minimum box
autonomous driving vehicle is built with resolution of model, i.e., rectangle with direction, is adopted to represent
0.1m × 0.1m. The 3D cloud points from the lidar are projected and to detect the target in the image, shown as Fig.2 (b).
into the 2.5D grid map, as shown in Fig.2 (a). The maximum
height Hmax, minimum height Hmin and height difference ΔH of
each grid are calculated according to the points falling in the
ąжĆmodel
grid. Compare ΔH with a preset threshold D : if ΔH > D , the
grid value will be set as 1, which means the grid is occupied by
obstacle; else, the grid value will be set as 0, which presents
the ground place. Thus, a binary image map, the same size as ąIĆmodel
the 2.5D grid map, can be generated according to the grid
value.
B. Candidate Targets Clustering ąLĆmodel
The connected component analysis is used to quickly
(a) (b) (c)
cluster the candidate objects in the binary image. Since there is
Fig. 2 Geometric model-based target detection: (a) point cloud of traffic
a gap between the multi-layer scanners of 3D-Lidar, the points
scene, (b) target detection results, (c) three geometric models. In (b) the
on the object’s surface farther away from the lidar sensor is colorful rectangle is ego car and the blue rectangles depict the detected
sparse, which causes the grids of the object to be discontinued targets.
and a target divided into multiple ones. Before applying the
 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)

Further, the objects, without obvious geometry feature, such

as too large or too small shapes, will be filtered out. As a
result, the wrong targets will be reduced and the detection
efficiency will promote as well.
B. Hypothesis Trajectories Collection
For low quality candidate targets with less cloud points
or less obvious geometry features, they will be miss-tracked
by the geometric model-based method. Through analysing the
feature of moving target’s trajectory, we presents a hypothesis
trajectory model to detect the low quality targets.
To build the hypothesis trajectory model, all candidate
targets are reserved in sequent n frames, and the total number
of candidate targets in t th frame is P(t), the hypothesis
trajectory model is built as Fig.3. From t-n th moment to t th
moment, a hypothesis trajectory consists of n nodes and n-1
paths, thus, a target may contain several hypothesis
trajectories. The trajectory marks the possibility of a candidate
target being a moving target within this period, and the
collection of all trajectories is named as Traj(t), weaved like a
network as Fig.3 (b). As result, there will be a large number of
trajectories to tracking depend on the number of candidate
targets during the frames.
C. Hypothesis Trajectory Model Fitting
Most hypothesis trajectories in Traj(t) are irregular, such
as green trajectories shown in Fig.4 (a), because only the
distance factors are considered in the connection between two
adjacent frames. In order to select out the moving target from
the substantial trajectories, we make the following assumptions:
z if n is small, the moving target on the road will move
in a near straight line in a short time.
z if n is small, the moving target on the road is at nearly
uniform speed in a short time.
Thus, we use straight line to fit each trajectory of Traj(t),
and calculate variance of the fitted line, average speed in the
(a) (b)
Fig. 4 Trajectory model-based target detection. The green lines in (a) show all
collected hypothesis trajectories, the yellow lines in (b) are trajectories fitting
the hypothesis model and the number is the detected target ID.
trajectories and variance of the speed. If they are fall in the
range of the thresholds, this path will be considered as the
trajectory generated by the moving target in the n frames.
Besides, the average speed and the direction of the line is used
as the moving target’s velocity in the moment of t. As shown
in Fig.4 (b), when n set as 5, the moving targets’ trajectories
fitting the hypothesis trajectory model are left and most of
them present like straight line.
IV. TARGET TRACKING
A. Data Association
Data association is to match the current detected target
with the tracked target, which is the key step for multi-target
tracking to update the state of the tracked target or put a new
target into the tracking target list. We adopt GNN method to
match the current detected target with the same target in
tracked list, and the similar value of the i th target in detection
list with the j th target in tracked list is calculated as (1). In (1),
(x,y) is the position of the target in the grid map, W is the
width of target, L is the length of target, and a, b, c, and d are
weighting factor, which has different values according to the
target’s position in the viewpoint of 3D-Lidar.

(a)
j fj kt Ci , j =
a
+
b
+
c
+
d
(1)
t-1
kt
fkktt+1 x −x
t
l
t y −y l
t W −W
t
m
t L −L
t
l
t t
k fkt−1 fit+1
kt i We build a correlation matrix C to restore the result of
Time t GNN, as in (2). The size of matrix C is (n_detect × n_track),
n_detect is the total number of detected targets and n_track is
Targets the total number of tracked targets. The matrix element Ci,j is
. ..

. ..

list
. ..

i the similar value calculated through (1). Then, we put the

. ..

i
fitk−t1 fktt+1 k
fktt+1 value of Ci,j into 1 or 0 according to the similar value of Ci,j : if
. ..

Mt (k) k Nt (k) the similar value of Ci,j goes beyond a certain threshold, Ci,j
(b)
fktk−t1 f kt
jt−1 f jt+1 will be set as 1, which means relationship exists between the
kt
j targets; otherwise, Ci,j will be set as 0, which means there is no
. ..

connection between them.

. ..

. ..

. ..

P(t)
ª C1,1 " C1, j " C1,n _ track º
Fig. 3 Hypothesis trajectories model. The colourful lines in (a) represent the « »
possible trajectory of k th target from time t-1 to t+1. In (b), the rings « # % " % # » (2)
represent the target, the connection lines mean the hypothesis trajectory as in C = « Ci ,1 " Ci, j " Ci,n _ track »
(a). There may exist M(k) targets in t-1 th moment and N(k) target in t+1 th « »
moment connecting with the k th target in t th moment. The connection « # % # % # »
side f k jt +1 is the hypothesis trajectory between k th target in and the j th target «C " C " C »
t ¬ n_detect ,1 n_detect , j n_detect , n _ track ¼

of t+1 th moment.
 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)

Updating
 function, Vt is the Noise of the measurement system with the
matching state
known covariance, and Wt −1 is the Noise of the process
Empty Seeking augment path

Matching from every vertices 1 system with the known covariance.
G(V,E) < The kinematics formulation shown in (4) describes the
motion state relationship between the previous time step and
Go through
 1 Exit augmented the current, and, the state transition matrix f can be written as
all path path
(5).
< Á xt 3 xt #1 ! vt #1 :t cos șt #1
Ä
HQG ÄÄ yt 3 yt #1 ! vt #1 :t sin șt
 (4)
Fig. 5 Seeking augment path flow.
Ävt 3 vt #1
Ä
ÄÃșt 3 șt #1
The ideal matrix C should present only one-to-one
correspondence relationship between the detection targets and ¾1 0 vt #1:t cos șt #1 :t sin șt #1 Î
¿ Ï
the tracked targets. However, in fact, there will be situations of ¿0 1 # vt #1:t sin șt #1 :t cos șt #1 Ï
the one to many, many to one or many to many due to the large At,t #1 = ¿ Ï (5)
¿0 0 1 0 Ï
number of targets, as well as the empty match. To solve the ¿ Ï
problem, the GNN method is usually implemeted with Greedy À¿0 0 0 1 ÐÏ
algorithem [13], however, we prsent a bipartite graph model-
based method with Hungary algorithm to optimaze the data Where, ( x, y ) is the position of tracked target, center
association relationship matrix C. position of the box, in the 2.5D grid map, v is relative
The data association relationshiop bipartite graph model velocity of the target, and ș is heading of the box with the y
G=(V,E) is established to represent the matrix C. In G=(V,E), axis of grid map coordinate system and is relative value as
V is a set of vertices with the size of (n_detect + n_track) well.
consisting of the detected targets and the tracked targets, and The state estimated for the moving target is represented
E is the relationship edges between the vertices, consisting of T
by ( x, y, v, ș ) . These states are independent of each other in
one detection target and one tracked target, and the value of
the element according to them in C is 1. According to the same moment, so the error covariance between any two of
Hungarian algorithm, we keep seeking the augmenting path in them is 0. The states ( x, y ) is fixed and can be written in
the bipartite graph G until there is no augmenting path starting measure matrix as (6):
with a intial place(the seeking flow show as Fig.5). And when ª1 0 0 0 º (6)
Hi = « »
meeting the following requirements, the total number of 1 in C ¬0 1 0 0¼
can reach maximum:
z There exists the element Ci,j, whose value is 1. V. EXPERIMENTS
z There exists no element Ci,k or Cg,j, whose value is1, We have evaluated the performance of our algorithm in
in the same row or column as Ci,j. real time in the following two experiments, which are typical
Thus, the matrix C is transformed into the matrix, where urban environment and the results will be discussed. The
only one-one matching relationship exists and the one-to-one autonomous driving vehicle used in the experiments is our
matched relationship achieves the maximum. self-developed Intelligent Vehicle (as shown in Fig.6), which
took part in the China Future Challenge of Intelligent Vehicle
B. Extended Kalman Filter for State Estimation from 2013 to 2016. A Velodyne-HDL-32E LIDAR is mounted
The Kalman Filter offers an efficient, iterative method to on the top the autonomous vehicle, with the rotation frequency
collect information from sensors for estimating the state of set as 10Hz. We implement the algorithm in C++ as well as the
the vehicle. Compared with standard Kalman filter, EKF is
widely used in non-linear system, such as the vehicle motion.
Considering that motion state of vehicle is non-linear model at
the situation with acceleration and turning, in this paper, EKF
is used to estimate motion state of the target. The filtering
model we use is as following:
ÄÁ X t 3 f ( X t #1 , t # 1) ! wt #1
 (3)
ÄÃ Z t 3 h( X t , t) ! Vt

Where X t ’ R n is the state, Z t ’ R n is the measurement,

f is the dynamic model function, h is the measurement model Fig. 6 The intelligent vehicle in the experiments.
 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)
point cloud library (PCL) .This algorithm runs on a system
equipped with 4GB RAM and Intel (R) Core(TM) i7 2.60 Hz
CPU. Besides, we equip the intelligent vehicle with sensors
such as GPS/IMU, which can receive the ego vehicle’s
position.
A. Experiment I (on Campus Road)
We evaluate the precision of target’s motion state
estimation in our tracking system in the scene shown in Fig.7
(a). The intelligent vehicle drives on the campus road and
tracks the cars parked on the sides of road as shown in Fig.7
(b). As shown in Fig.8, the velocity estimation of static target
with EKF is compared with the estimation result of KF in 160
frame.
(a) Campus road of experiment I (b) point cloud scene of experiment I
Fig. 7 Experiment I scene.
Fig. 8 Static target’s reference velocity estimation results comparison.
rd
B. Experiment II (on Beijing’s 3 ring road))
We test the multi-target tracking algorithm on Beijing’s
3rd ring road as well. As shown in Fig.9 (a), the blue line is
the test route on Beijing third ring road, which is a typical
complex traffic with lots of moving vehicles.
(a) (b)
Fig. 9 Traffic scene of the experiment on the Beijing third ring road. The
blue ring in (a) is the third ring road and our experiment route as well, and
(b) shows the traffic scene.
We choose a ramp-through datasets to evaluate our
tracking system. The traffic scene of dataset is show as Fig.9
(b), where is crowded with a vehicle stream. The dataset was
collected by the intelligent vehicle, with a speed between
20km/h and 45km/h.
In Fig.10 (a), the number of tracked targets is compared
between the geometric model method and the hybrid method,
and the tracking performance with the data association
procession proposed in the paper is compared with greedy
algorithm. The truth target in Fig.10 (a) is labeled in the off-
line datasets. Fig.10 (b) shows the cost time of the proposed
multi-target tracking algorithm and illustrates the tracking
algorithm can meet the requirements of running in real time.
Besides, Fig.11 shows the scenes of tracking result.
(a)
(b)
Fig. 10 Tracking results of experiment II. (a) is the number of detected targets
in the view of 3D-Lidar through different tracking methods. (b) presents the
cost time for tracking the targets.
VI. CONCLUSION
In this paper, we proposed a hybrid system to track
moving targets for autonomous driving vehicle. The multiple
targets tracking system combined geometric method with the
hypothesis trajectory model and performed better than single
method of them. The proposed hybrid multiple moving target
tracking system was tested with our intelligent vehicle in the
two typical real traffic environment, and the experiment result
shows the algorithm can successfully track abundant targets in
real time. However, the algorithm proposed in this paper
extracts the target through the 2D model target without
making the best of 3D-lidar. Our next step is to analyse the 3D
feature of different type objects and identify classifications of
objects.
 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC)

Fig. 11 Tracking results scenes in the frames when the intelligent vehicle drives through one ramp of Beijing third ring road. The tracking results are presented by
rectangles labeled with ID numbers, and the rectangle’s color represents different detection methods proposed in the paper. Blue rectangle is detected by
geometric model-based method, red rectangle is detected by hypothesis trajectory model-based method and yellow rectangle means the target is detected with
both methods. The green thin line is the trajectory of target.

[6] A. Azim and O. Aycard, ‘‘Detection, Classification and Tracking of

ACKNOWLEDGMENT
This work was supported by the National Natural Science
Foundation of China (Grant No. 91420203) and Project Grant
No.: 2017CX02005, Key Laboratory of Biomimetic Robots
and Systems (Beijing Institute of Technology), Ministry of
Education, Beijing 100081, China. The authors would like to
thank all of our team members.
REFERENCES
[1] A. W. Smeulders, D. M. Chu, R. Cucchiara, S. Calderara,“Visual
tracking: An experimental survey,” IEEE Transactions on Pattern
Analysis and Machine Intelligence, vol. 36, no. 7,pp. 1442–1468, 2014.
[2] Pedro Girao, Alireza Asvadi, Paulo Peixoto, and Urbano Nunes, “3D
Object Tracking in Driving Environment: a short review and a
benchmark dataset,” in Proc. IEEE Conf. Intell. Transp. Syst., Rio de
Janeiro, Brazil, 2016, pp. 7–12.
[3] A. Petrovskaya and S. Thrun, ‘‘Model based vehicle detection and
tracking for autonomous urban driving,’’ Journal of Autonomous Robots,
vol. 26, pp. 123-139, 2009.
[4] J. Choi, S. Ulbrich, B. Lichte, and M. Maurer, “Multi-target tracking
using a 3d-lidar sensor for autonomous vehicles,” in Proc. IEEE Conf.
Intell. Transp. Syst., Hague, Netherlands, 2013, pp. 881–886.
[5] Yang Fei, Zhu Zhu, Gong Xiaojin, Liu Jilin. “Real-time dynamic
obstacle detection and tracking using 3D-Lidar”. Journal of Zhejiang
University (Engineering Science), 2012, vol.46, pp.1565-1571.
Moving Objects in a 3D Environment,’’ in Proc. 2012 IEEE Intelligent
Vehicles Symposium, Madrid, Spain, 2012.
[7] Konstantinova P, Udvarev A, Semerdjiev T, “A study of a target tracking
algorithm using global nearest neighbor approach,” Proceedings of the
International Conference on Computer Systems and Technologies, pp.
290-295, 2003.
[8] T. E. Fortmann, Y. Bar-shalom, and M. Scheffe, ‘‘Sonar Tracking of
Multiple Targets Using Joint Probabilistic Data Association,’’ IEEE J.
Oceanic Engineering, vol. OE-8, no.3, 1983.
[9] Blackman S S, ‘‘Multiple hypothesis tracking for multiple target
tracking,’’ IEEE Aerospace and Electronic Systems Magazine, vol. 19,
no. 1, pp. 5-18, 2004.
[10] Y. Fei, W. Christmas, and J. Kittler, ‘‘Layered data association using
graph-theoretic formulation with application to tennis ball tracking in
monocular sequences,’’ IEEE Transactions on Pattern Analysis and
Machine Intelligence, vol. 30, no. 10, pp. 1814---1830, 2008.
[11] C. Huang, B. Wu, and R. Nevatia, ‘‘Robust object tracking by
hierarchical association of detection responses,’’ in ECCV ’08:
Proceedings of the 10th European Conference on Computer Vision,
2008, pp. 788---801.
[12] S. Rezaei and R. Sengupta, “Kalman filter-based integration of DGPS
and vehicle sensors for localization.” IEEE Transactions on Control
Systems Technology, vol. 15, no. 6, pp. 1080–1088, 2007.
[13] C ox I J, Miller M L,“On finding ranked assignments with application
to multitarget tracking and motion correspondence,” IEEE Transactions
on Aerospace & Electronic Systems, 1995, vol. 31, no. 1, pp. 486-489.