16th International Conference on Information Fusion

Istanbul, Turkey, July 9-12, 2013

Transforming local sensor tracks prior to

track-to-track fusion in an automotive safety system
Marcus Andersson

Fredrik Sandblom

AF
Gothenburg, Sweden
Email: Marcus.Andersson@afconsult.com

Volvo Group Trucks Technology

Gothenburg, Sweden
Email: fredrik.sandblom.2@volvo.com

AbstractThis paper addresses a practical issue that arises

when the output from two local trackers, using two different
state-space models, are to be fused with each other in a global
track-to-track fusion system. Such fusion algorithms require the
local tracks to be represented in a joint state-space, meaning that
at least one of these tracks needs to be transformed prior to the
track fusion.
Three methods for transforming a local track have been
evaluated using simulations and real data. Our evaluation shows
that it is possible to design a filter for transforming the local
track that can be tuned to balance accuracy and latency.
These results are important when the underlying assumptions
in local trackers are unknown to a centralized fusion system.
Therefore, our findings are believed to have practical value
to anyone working with track-to-track fusion, particularly in
automotive safety systems.

I. I NTRODUCTION
Automotive safety is a global concern that receives increased attention. Traffic-related accidents are the 10th largest
cause of death, accounting for 2.1% of all deaths in 2002 [1].
The development of intelligent safety systems - autonomous
systems that avoid accidents or mitigates their effects - contribute towards reducing these injuries. The development is
driven not only by vehicle manufacturers, who are introducing
new safety systems at an increased pace [2], [3], but the
society as a whole. One example is that trucks weighing over 8
tons must be equipped with a lane departure warning system
and an automatic emergency braking system in order to be
type-approved in the European union starting from late 2013
[4]. Another example is the substantial number of publicly
financed research programs, see e.g. [5][7].
In order to understand the traffic situation, vehicles are
equipped with sensors such as radars and cameras. The information is put together in what is commonly referred to as
a perception layer [8], before being used by threat assessment
algorithms, such as [9], and if required to control the
vehicle. The perception layer constitutes a fusion system and
there are several challenges to its design; the processing power
is limited, as is the communication bandwidth to the sensors.
Therefore, extensive filtering is often performed already at
sensor level. Hence, it can be argued that a track-to-track
(T2T) fusion scheme is a proper approach, see e.g., [10].
Furthermore, in that article, simulated data is used to show
that such fusion methods are preferred over re-filtering using
a global Kalman filter, when sensor data is pre-filtered locally.

978-605-86311-1-3 2013 ISIF

An obvious benefit with T2T fusion is that the sensor

supplier is responsible for filtering the raw data, a task they
arguably are best equipped to take on. At the other hand,
filtering at a sensor level reduces the amount of information
accessible to the fusion system and therefore also the potential
benefit. In fact, since the actual filtering applied prior to
the fusion can be kept secret by the supplier, finding an
optimal fusion scheme may not even be theoretically possible.
Furthermore, T2T-methods found in literature, e.g., in [10]
[12], require tracks to be defined in the same state-space. This
is a substantial limitation since a well-behaved track that has
been filtered using a particular process model cannot as a
general rule be expected to behave well over time when
expressed in a different state-space. Consequently, this issue
must be addressed before the T2T fusion can take place.
In this paper, we compare three methods for expressing a
previously filtered track in a different state-space: re-filtering
using a Kalman filter approximation, direct transformation
of the posterior distribution, and a T2T fusion method that
combines the transformed state with a prediction using the
desired process model. The performance of the filters is
analyzed using both simulations and actual measured tracks
from a radar-equipped vehicle.
II. P ROBLEM F ORMULATION
Consider a single sensor that is independently tracking a tar k , for k = 1, 2, ...,
get and periodically produces an estimate, x
k is the conditional mean given all measurements,
where x
k with an estimate,
y1...k , from that sensor. We wish to fuse x
k , from another sensor that uses a different dynamic model.
That is, the second sensor produces its output in a different
state space such that commonly used T2T fusion methods
[10][12] cannot be directly applied.
Assume there is a non-linear one-to-one mapping,
= g(x),

(1)

that denotes the transformation from the first state-space to

the other. From a theoretical point of view, the conditional
k is calculated, p(x|y1...k ), can be
distribution from which x
k , in the new state space using
used to calculate an estimate, x
Eq. (1):
Z
k = g(x)p(x|y1...k )dx,
x
(2)

655

 k and k can be readily fused. The posterior

such that x
covariance matrix is also needed to perform T2T fusion and
is given by:

Cov(g(xk )) =
(3)
Z h
ih
iT
k g(x) x
k p(x|y1...k )dx.
g(x) x
Since g is a non-linear function, integrals (2)-(3) may not have
closed-form solutions. A perhaps bigger challenge in practice,
k ] may not have
2 , . . . , x
though, is that the sequence [
x1 , x
a dynamic behavior similar to the sequence [1 , 2 , . . . , k ],
which can result in poor behavior of the fused estimate.
The question at hand is therefore how to filter the transformed state to improve estimation accuracy, yet minimize
delays in the dynamic behavior. We will study a representative
transformation of a linear Cartesian state-space model to a
state-space where the heading angle is included.

III. DYNAMIC MODELS

We consider the case when a sensor fusion system receives
state estimates of surrounding vehicles from a radar tracker
on following form:
x=

x y

T

(4)

where [x, y] is the Cartesian position in a coordinate system

centered in the host vehicle. The corresponding posterior
covariance matrix, Px , is also provided. The dynamic model
used by the radar tracker is the discrete wiener process
acceleration model (DWPA) [13] .The desired parametrization
is the state-space defined by a simplified bicycle model [14]:
=

T

(5)

and is illustrated in Fig. 1. The heading angle, , is defined

relative the host vehicle, v and a is the speed and acceleration
in the heading direction and c represents the curvature of
the current path of the vehicle. The state is defined by the
differential equation,

xy

Fig. 1. The desired state representation of a target vehicle, see Eq. (5), in a
coordinate system centered in the host vehicle.

f2 : x 7 :

v cos

(7)
f1 () =

v sin

a cos cv 2 cos
a sin + cv 2 sin

p
f2 (x) =
(8)

2
2
x + y

(
y cos x
sin ) / x 2 + y 2
y sin + x
cos
 
y
where = arctan
. These transformations are sufficient
x
to transform a given point in one state-space to the other and
back again.

IV. E VALUATED METHODS

We compare three methods that have different properties
regarding estimation accuracy and latency. The minimalistic
approach is to transform one of the tracks to the state-space
in which the other track is described (method IV-A). This
approach is compared to two methods that, in different ways,
filter the estimate using the desired dynamic model (methods
IV-B and IV-C).
The evaluation set-up is shown in figures 34.
A. Transformation using the Cubature rule

v(t) cos (t)

v(t) sin (t)

vc(t)

(t)
=

a(t)

0
0

0
0

0
+
0

c(t)

a(t)

(6)

where the process-noise terms, c(t)

and a(t),

are assumed to
be Gaussian processes. The relation between the DWPA state
vector and the bicycle state vector is given by f1 : 7 x and

The first approach is to approximate the integrals in Eq.

(2)-(3), assuming x N (
x, Px ), using the Cubature rule
(CUB) [15]; i.e., to map the density given by the radar to the
bicycle state space using Eq. (8). This transformation does
not involve additional filtering and, consequently, does not
introduce delays.
The Cubature rule is found to be very accurate, for this
particular transformation, when compared to an estimate of the
mean and covariance using Monte-Carlo simulations. Since
the technique is widely used and is simple to apply, other

656

approximations of similar complexity, e.g., linearization, have

not been evaluated.
B. Re-filtering using a Cubature Kalman Filter
The second approach is to re-filter the information given by
the radar, treating the estimates as unfiltered evidence. This
is clearly sub-optimal and is expected to introduce significant
delays in the filtered output. Yet, it is arguably an intuitive
approach towards including the bicycle model knowledge.
The optimal solution, given knowledge regarding all concerned distributions, is the Bayesian filter. However, since the
pre-processing steps are assumed unknown to us, we will evaluate a well-known approximation that can be readily applied.
We assume all distributions can be approximated as Gaussian
distributions and apply the Square Root Cubature Filter (SR CKF) [15], which is known to be a useful approximation for
moderately non-linear, systems [16][18].
We assume that the linear minimum mean squared error
filter equations are known to the reader, see e.g., [13]. The
predictive distribution is calculated using a discretized version
of the bicycle motion model defined in Eq. (6), and the measurement prediction using Eq. (7). The square root of the error
covariance is used for stability reasons in the implementation.
Discretization of the bicycle motion model gives us the model:


a k T 3
ak T 2

xk+1 = xk + cos k vk T + 2 +
2

3
2
k k )T
sin k vk 2k T + (vk k +2a
+
6

4

(a k k ak k )T
+ a k 20k T
8


2
a k T 3
+
yk+1 = yk + sin k vk T + ak2T +
2

3
2
k k )T
+
cos k vk 2k T + (vk k +2a
6

5

(a k k ak k )T 4
+ a k 20k T
8
k+1
vk+1
ck+1
ak+1

= k + vk ck T + ak c2k T +
3
4
a k ck T 3
+ ak c3k T + a k c8k T
6
2
= vk + ak T + a k2T
= ck + ck T
= ak + a k T

c k vk T 2
2

with a prediction of the result from the previous iteration, using

the bicycle motion model (9). The covariance intersection (CI)
method [19] is used since it is robust and requires only the first
two moments of the local tracks to be known. Given estimates,
a, x
b , and their respective covariance matrices, Pa , Pb , the
x
c and Pc , is given by
fused output, x
1
1
P1
c =Pa + (1 )Pb

c =Pf (P1
a
x
a x

+ (1

(10)

b ),
)P1
b x

(11)

where w is a design parameter weighting the influence of

each distribution on the fused state. The method is based
on convex combination of the error covariance and in this
way we intend to influence the result with the desired model
while, contrary to approach IV-B, the posterior covariance
matrix is not underestimated. Consequently, the step-response
is expected to be significantly more responsive.
V. E VALUATION AND RESULTS
We evaluate the methods described in the previous section
using both simulations and radar tracks, logged during approximately 40 km of driving on a highway in Sweden. An
example of the road geometry is shown in Fig. 2.
5000

4000

3000
heading: 2.19
heading: 2.18

2000

1000

heading: 2.17
v: 23.93 m/s

0
4000

(9)
where

3000

2000

1000

Fig. 2. The black line is a segment of the traveled road. The smaller picture
shows the ego vehicle (the circle) and three tracked leading vehicles (stars)
in a close-up around the blue circle on the road segment.

k = vk ck

C. Model enforcement with Covariance Intersection

The CI fusion approach is evaluated for different weights in

the feedback loop, described by a weight vector, w = [w1 w2 ].
The relation between w and the weight in Eq. (10)-(11) is
w1
=
.
(12)
w1 + w2

The third approach attempts to address some of the expected

problems with the methods described in Sec. IV-A and Sec.
IV-B by introducing an ad-hoc feedback-loop that facilitates
balancing output accuracy and responsiveness. The radar track
is first transformed using the method in Sec. IV-A, then fused

The first element in the weight vector corresponds to the

influence of the radar track whereas the second element
corresponds to the influence of the prediction.
The processing steps in the simulation environment are
illustrated in Fig. 3, and in Fig. 4 for the real time analysis.

a k ck T
a k ck T 2
k = ak ck + v ck + ak ck T +
+
2
2
and T is the sample time.

657

1000

Fig. 3.

generated at 20Hz by transforming the true trajectory to the

measurement state space, i.e., range, range-rate and azimuth,
with uncorrelated additive Gaussian noise:
p

x2 + y 2
(15)
zk = v cos(arctan( xy ) ) + vk ,
arctan( xy )

Flow chart describing the simulation environment.

Process
noise

Measurement
noise

Generate
true
trajectory

Generate
radar
measurement

PF

where vk N (0, Pz ), using

1.52
0

0
0.0252
Pz =
0
0

Simulate
radar
tracker

SRCKF

CUB

CI

Evaluation

Fig. 4. Flow chart describing the replay environment and the ground truth
evaluation.

Radar
tracker

SRCKF

CUB

CI

Evaluation

In-vehicle
sensor
information

Postprocessed
ground
truth

0
.
0
2
(/180)

(16)

The radar tracker is modelled by implementing a Square

Root Cubature Kalman filter, using a DWPA process model and
a polar-to-Cartesian transformation as measurement model. A
sequential importance sampling particle filter (PF), see e.g.
[20], is implemented as a reference filter, using measurements,
zk and full model knowledge. The particle filter gives us an indication of the loss in performance when transforming already
filtered data to another state space, relative an asymptotically
optimal filter.
Table I shows the simulation results, presented in terms of
mean squared error (MSE) and step response in the heading
angle. The mean squared error is calculated using 500 scenarios, each 230s long. The step response, defined as the number
of samples to reach 90% of the step change, was obtained by
changing the heading angle of the target from 0 to 0.1 radians
in one time instant. In order to exclude the latency of the
simulated radar tracker in the analysis of the step response, the
true trajectory, point-to-point transformed to the Cartesian state
space, was used in the prediction model at the time of the step.
Hence, the radar tracker converges instantly and remaining
delays originate from our methods alone.
TABLE I
S IMULATION RESULTS FOR THE EVALUATED METHODS

Lane
tracker

The analysis is based on the estimation accuracy of the

vehicle heading angle for two reasons; first, it is an important
component in threat assessment algorithms, e.g., [9]. Second,
the heading can be used in other tasks, such as estimating the
road geometry ahead of the host vehicle.
The use of a heading-based model is expected to improve
this estimate, relative the DWPA model.

CI

Method:

MSE () [ ]:

PF
CUB
CKF

1.9
4.5
2.3
4.2
3.7
3.3
2.7
2.3
2.1

w
w
w
w
w
w

= [1 1]
= [1 5]
= [1 10]
= [1 25]
= [1 50]
= [1 75]

Step-response:
N/A
0
49
5
11
15
23
31
35

B. Ground truth

A. Simulation settings
Target trajectories are generated using the bicycle motion
model (9) with Gaussian process noise:


E [c a]
T = [0 0]T
(13)




0.01 0
Cov [c a]
T =
.
(14)
0
1.5
The tracks are initiated 100m ahead of the host vehicle and has
the same initial velocity, 25m/s. Radar measurements, zk are

Contrary to the comparison using simulated trajectories,

we do not have a ground truth for the radar tracks, which
are logged while driving in regular traffic on a highway in
Sweden. The true heading of the tracked vehicles is instead
approximated with the heading of the road at the same
position.
The road geometry is calculated off-line using in-vehicle
sensor information and measurements of the lane markings
provided by a camera, e.g., described in [21]. Targets that

658

are changing lanes are excluded from the evaluation during

the time of the lane change, since the underlying assumption
that the heading of a tracked vehicle corresponds to the
heading of the road obviously does not hold during such
maneuvers.
The radar data was obtained from approximately 40 km of
highway driving with 1 4 targets ahead of the vehicle at
ranges up to 150m. The result of the evaluation using this
data is presented in table II.
TABLE II
E VALUATION

RESULTS USING LOGGED RADAR TRACKS

Method
CUB
SR - CKF

CI

w
w
w
w

= [1 1 ]
= [1 25]
= [1 50]
= [1 75]

MSE

() [ ]:
3.1
2.9
3.1
2.7
2.8
2.9

VI. C ONCLUSIONS
We have addressed a practical issue the need for transformations that arises when the output from two local trackers,
using two different state-space models, are to be fused with
each other in a global track-to-track fusion system.
Three methods for transforming a local track prior to
the fusion have been evaluated using simulations and real
data. As anticipated, re-filtering the local track in a Kalmanfilter framework results in a slow dynamic behavior, whereas
a direct transformation is fast yet less accurate. The third
method includes a feed-back loop that can be tuned to balance
accuracy and responsiveness. Our evaluation shows that it
performs well both using simulated data and actual radar tracks
logged during approximately 40km of driving on a highway.
More specifically; the simulations indicate that the MSE is
improved when the influence of the (true) model increases,
which is hardly surprising. When using real data, however,
it can be seen that moderate model influence yields the best
result. This should not come as a surprise, either, since the
model cannot describe the complex dynamics of real driving.
There are two main contributions in this paper. First, we
address the practical problem of fusing local tracks that do not
use the same dynamic model. Second, we show that CI fusion
can be used to find a good balance between model usage and
response time of a transformed state. A practical benefit with
this approach is that a prediction of the fused estimate from the
previous iteration can be included jointly with the sensor tracks
in a global T2T-fusion scheme, replacing the local prediction
used in this study.
These results are important when the underlying assumptions in local trackers are unknown to a centralized fusion
system. Therefore, our findings should have practical value to
anyone working with sensor data fusion in automotive safety
systems.

ACKNOWLEDGMENT
This work has been supported by the Strategic Vehicle
Research and Innovation Program (FFI), which is funded by
the Swedish Governmental Agency for Innovation Systems
(VINNOVA).
R EFERENCES
[1] C. Mathers and D. Loncar, Updated projections of global mortality
and burden of disease, 2002-2030: data sources, methods and results,
Geneva, World Health Organization, 2005.
[2] M. Distner, M. Bengtsson, T. Broberg, and L. Jakobsson, City safety
a system addressing rear-end collisions at low speeds, in Proc. 21th
Enhanced Safety Vehicles Conf., paper 09-0371, 2009.
[3] R. Schneiderman, Car makers see opportunities in infotainment, driverassistance systems [special reports], Signal Processing Magazine, IEEE,
vol. 30, no. 1, pp. 1115, Jan.
[4] Regulation (EC) No 661/2009 of the European Parliament
and of the Council of 13 July 2009 concerning type-approval
requirements for the general safety of motor vehicles, their trailers
and systems, components and separate technical units intended
therefor, Official Journal of the European Union, pp. 124, 2009.
[Online]. Available: {http://eur-lex.europa.eu/LexUriServ/LexUriServ.
do?uri=CELEX:32009R0661:EN:NOT}
[5] M. Kolbenstvedt and et.al., Effects of swedish traffic safety research
1971 - 2004. [Online]. Available: http://www.vinnova.se/upload/
EPiStorePDF/va-07-10.pdf
[6] A. Amditis, A. Bolovinou, U. Iurgel, A. Etemad, E. Johansson,
A. Saroldi, and L. Bjelkeflo, Enhanced perception shared by automotive
safety applications supporting active intervention-the interactive system
overview, in 18th ITS World Congress, 2011.
[7] V. L. Neale, T. A. Dingus, S. G. Klauer, and M. Goodman, An overview
of the 100-car naturalistic study and findings, Traffic Safety, pp. 110,
2005.
[8] A. Polychronopoulos and A. Amditis, Revisiting JDL model for automotive safety applications: the PF2 functional model, in Proc. 9th Int.
Conference on Information Fusion, July 2006, pp. 1 7.
[9] M. Brannstrom, E. Coelingh, and J. Sjoberg, Model-based threat
assessment for avoiding arbitrary vehicle collisions, IEEE Trans. Intell.
Transp. Syst., vol. 11, no. 3, pp. 658669, Sep. 2010.
[10] S. Matzka and R. Altendorfer, A comparison of track-to-track fusion
algorithms for automotive sensor fusion, in Multisensor Fusion and
Integration for Intelligent Systems, 2008. MFI 2008. IEEE International
Conference on, Aug., pp. 189194.
[11] S. Mori, W. H. Barker, C.-Y. Chong, and K.-C. Chang, Track association and track fusion with nondeterministic target dynamics, Aerospace
and Electronic Systems, IEEE Transactions on, vol. 38, no. 2, pp. 659
668, 2002.
[12] X. Tian and Y. Bar-Shalom, Track-to-track fusion configurations and
association in a sliding window, J. of Advances in Information Fusion,
vol. 4, no. 2, pp. 146164, 2009.
[13] Y. Bar-Shalom, X. Rong Li, and T. Kirubarajan, Estimation with
Applications to Tracking and Navigation. John Wiley & Sons, Inc.,
2001.
[14] T. D. Gillespie, Fundamentals of vehicle dynamics. Warrendale, PA:
SAE, 1992.
[15] I. Arasaratnam and S. Haykin, Cubature Kalman filters, IEEE Trans,
Autom. Control, vol. 54, no. 6, pp. 12541269, 2009.
[16] I. Arasaratnam, S. Haykin, and T. R. Hurd, Cubature kalman filtering
for continuous-discrete systems: Theory and simulations, Signal Processing, IEEE Transactions on, vol. 58, no. 10, pp. 49774993, 2010.
[17] C. Fernandez-Prades and J. Vil`a-Valls, Bayesian Nonlinear Filtering
Using Quadrature and Cubature Rules Applied to Sensor Data Fusion
for Positioning, in IEEE Int. Conf. on Communications, 2010, pp. 26.
[18] F. Sandblom and L. Svensson, Moment estimation using a marginalized
transform, Signal Processing, IEEE Transactions on, vol. 60, no. 12,
pp. 61386150, Dec.
[19] S. Julier and J. Uhlmann, A non-divergent estimation algorithm in the
presence of unknown correlations, in American Control Conference,
1997. Proceedings of the 1997, vol. 4, Jun, pp. 23692373 vol.4.
[20] M. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, A tutorial
on particle filters for online nonlinear/non-Gaussian Bayesian tracking,
IEEE Trans. Signal Processing, vol. 50, no. 2, pp. 174 88, 2002.

659

[21] A. Eidehall and F. Gustafsson, Obtaining reference road geometry parameters from recorded sensor data, in Intelligent Vehicles Symposium,
2006 IEEE. IEEE, 2006, pp. 256260.

660