Proceedings of the 2011 IEEE

International Conference on Mechatronics

April 13-15, 2011, Istanbul, Turkey

Multi-sensor data fusion of DCM based orientation

estimation for land vehicles
Z.Ercan#l, v.Sezel1, H.Heceoglu#l, C.Dikilitas#2, M.Gokasan#l, AMugan#2 , S.Bogosyan#3

Control Engineering Department, Faculty of Electronics Engineering, Istanbul Technical

University, Istanbul, Turkey
Mechanical Engineering Department, Faculty of Mechanical Engineering, Istanbul Technical
University, Istanbul, Turkey
Electrical and Computer Engineering Department, University of Alaska Fairbanks, USA

zyercan@gmail.com
gokasan@elk.itu.edu.tr
mugan@itu.edu.tr
sbogosyan@alaska.edu

Abstract- In this paper, an algorithm estimating orientation is
implemented using Direction Cosine Matrix (DCM) method,
chosen due to its linear process model and ease of use. Two
Kalman fIlters were used to estimate the rotation matrix
elements where the Euler Angles are easily computed. A rule
based decision structure is used to choose the best measurement
available in the system from GPS and digital compass. Also the
dynamic motion of the vehicle is considered to overcome the slow
response of the digital compass. The algorithm is tested with real
time logged data set and a decision structure is developed to have
the best information provided from the multiple sensors. The
algorithm is also tested under artificial GPS outages, performs
successfully for both attitude and heading angles.
Keywords- Orientation Estimation, Multi-sensor fusion,
Kalman Filter, DCM, Driverless Car
I. INTRODUCTION
Research on driverless cars is currently attracting a lot of
interest. The ultimate objective is to increase safety by
reducing traffic accidents caused by human faults. To reach
this objective with desired reliability, these vehicles require
the use of multiple sensors of various types and artificial
intelligence. A fundamental capability of a driverless car is
navigation. Using the information from various sensors, a
driverless car should be capable of determining vehicle's
dynamic states (position, velocity and orientation etc.), path
planning and calculating the necessary maneuvers to move
between desired locations. [l] Estimation of the vehicle states
is important to achieve other navigation tasks. The common
way to estimate the vehicle states is the Inertial Navigation
System (INS) and Global Positioning System (GPS)
integration. Since all the dead-reckoning sensors measure
related information in the vehicle's body frame (x, y, z), their
measurements should be transformed to the navigation frame
(n, e, d) in order to use with the INS mechanization. That
transformation should be correct to have accurate position and
velocity estimations [2]. Therefore to estimate orientation
accurately, multi-sensor stand-alone methods are proposed. In
[3], a Direction Cosine Matrix (DCM) based orientation
estimation algorithm is presented with two cascaded Kalman
Filters. A low cost strapdown inertial measurement unit (IMU)
and a magnetic compass are sampled at the same frequency to
use Kalman Filter directly. In another study, the Quaternion
method in the computer frame approach is used to estimate
orientation and to derive INS algorithm for low cost IMU to
solve the initial attitudes uncertainty using in-motion
alignment. The distribution approximation filter (DAF) is used
to implement non-linear data fusion algorithm [4]. In [2], a
fuzzy multi-sensor fusion of orientation estimation using
Euler method is presented.
In this paper, a multi-sensor based orientation estimation
algorithm is proposed using the DCM method due to its linear
model and also because of the fact that no singularity occurs
with this method for land vehicle applications. The main
contribution is the use of GPS, compass and IMU in a data
fusion algorithm that utilizes each sensor in the best possible
way. As a novel approach, the prediction stage of the Kalman
filter is used for extrapolation of data from sensors with
different sampling frequencies, or for the extrapolation of
unavailable sensor data i.e. due to GPS outage. Another
novelty is in the determination of heading angle, which is
often performed with the use of one sensor, however, in this
study, two sensors, namely, GPS and digital compass are used
to this aim.
The algorithm is tested with real time logged data and a
decision structure is developed to have the best information
provided from sensors. Also in simulation, artificial GPS
outages are presented to test the algorithm's behaviour.

978-1-61284-985-0/11/$26.00 ©2011 IEEE

672

Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.
 ll. ORIENTATION ESTIMATION USING DCM METHOD
In this section, basics of DCM method are given. Roll angle
(0) and pitch angle (cfJ) estimation and yaw (heading) angle ('I')
estimation algorithm is given respectively.
A. Basics of DCM Method
The relationships between vectorial quantities in the body
frame and the navigation frame can be described by forming a
rotation matrix R� . It is formed by three consecutive plane
rotations involving Euler angles, yaw ('1'), pitch (0), and roll
angle ( cfJ ), respectively. The DCM method is based on
integrating the measured angular rate from gyros with the
following relationship.
(1)
(2)
Where R� is body frame to navigation frame rotation
matrix. If the position and velocity information are
unavailable then wf:t could be ignored because this term is
dependent on position and velocity. wf:, is the angular velocity
of the body frame with respect to the inertial frame resolved in
body frame (gyro measurements) and [W�bX] is the skew­
symmetric representation of W�b'
Using orthogonality property of the rotation matrix, it is
sufficient to integrate only two rows separately. At first step, a
Kalman filter is used for estimating the third row of R� using
gyro measurements, where roll and pitch angle could be easily
calculated. This is followed by a second Kalman filter which
fuses the heading information from GPS and a digital compass
which has different sampling frequency from the algorithm to
estimate the second row of R�, through which the yaw angle
is easily calculated.
B. Roll and Pitch Angle Estimation
Generally, accelerometers (tilt sensors) are used to
estimate roll and pitch angles using the projection of gravity
vector in each axis. But to do this calculation accurately, the
vehicle should be going at a constant speed or should be at
rest. So the tilt sensors only measure the projection of gravity
vector, not the sum of motional acceleration and the projection
gravity vector. To use the tilt sensors as attitude angle
measurements, vehicle stops are proposed to correct the roll
and pitch angles periodically [2].
Some Inertial Measurement Units (IMU) provides
stabilized pitch and roll angles by combining the angular rate
sensors with accelerometers. The weighting between the
accelerometers and gyros is controlled by a parameter named
erection rate. In general for dynamic measurements low
erection rate is appropriate since IMU depends more on gyros
than the accelerometers to calculate the stabilized pitch and
roll angles. In this work, high erection rate (100+) is set when
the vehicle is at rest [7]. To avoid interference on the IMU for
changing the erection rate any time whether the motion is
dynamic or slow, a Kalman filter is designed to provide
673
Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.
optimal estimates for both situations by simply changing the
measurement covariance matrix. The state space
representation of the process model is given as follows.
(3)
Where R�(3:) is the third row of the rotation matrix and
the states of the system. Although the state matrix is time
varying, it is constant between two sampling times. The
process noise Wi (t) is assumed to be zero-mean Gaussian
white noise with the power spectral density (PSD)
Qw (assumed to be diagonal) . The equation for process noise
covariance matrix is given in (6) [6].
The measurement model is given as
Z(t)T = [- sin 8; sin $ cos 8; cos $ cos 8] (4)
z(t) = 13X3R�(3:) + vi(t) (5)
Qi(k) = ctJkGQwGT ctJrllt (6)
Where the measurement z(t) is formed by the roll angle
and pitch angle measurements ($ and 8 respectively) of the
IMU. The measurement noise Vi (t) is assumed to be zero­
mean Gaussian white noise with the measurement noise
covariance matrix of Ri(k) (assumed to be diagonal). ctJk is
the state transition matrix and G is the input matrix.
Since the measurements occur at discrete time intervals, a
Discrete Kalman Filter (DKF) should be designed using the
process model (3) and measurement model (5). More details
about the discretization process and DKF algorithm, the
reader should refer to [5].
The noise covariance matrices Qi(k) and Ri(k) are used as
performance tuning parameters. The diagonal elements of the
matrices Qi(k) and Ri(k) are selected to have a smooth
response. Since the erection rate is set a high value when the
vehicle is at rest at the beginning, the angle measurements of
IMU are accurate when the angular motion is slow. In this
case the diagonal elements of Ri(k) is set to be smaller than
Qi(k) to make Kalman filter trusting measurements more than
the dynamic model. However, if the motion is dynamic (i.e.
high angular rates), the diagonal elements of Ri(k) is set to
be bigger than Qi(k) to make Kalman Filter relying on
dynamic model more than the measurements. Since new
measurement is weighted less, the states converge slowly and
vice versa [5].
Normalization should be done after each estimation step to
keep the unity property of the DCM [3].
C. Yaw Angle(Heading) Estimation
For heading measurement, generally GPS and digital
compass are used. GPS heading information is more accurate
especially for dynamic motion but an outage could be easily
occurred dependent on the environment and the position of
satellites. A digital compass is more reliable in a way, since
 there is no outage (unless sensor fails) but due to its slow
response to dynamic motion, the heading information is time
delayed and also it is sensitive to magnetic fields in the
environment causing inaccurate measurements. In absence of
these situations, the compass measurement is accurate. So a
rule based decision structure should be constructed to select
the best measurement depending on the environment and the
motion of the vehicle. This structure is constructed using "if
and else" logic given below in figure 1.
Where R�(2:) is the second row of the rotation matrix and
the states of the system. Although the state matrix is time
varying, it is constant between two sampling times.
The process noise wz(t) assumed to be zero-mean
Gaussian white noise with the power spectral density (PSD)
Qwz (assumed to be diagonal).
The measurement model is given as
Z(t)T = [cos e sin l/i ; cos ¢ cos l/i + sin ¢ sin (j sin l/i ...
; - sin ¢ cos l/i + cos ¢ sin e sin l/i] (10)

(11)

Where the measurement z(t) is formed by the roll and

pitch angle estimates of the first Kalman filter (¢ and e
respectively) and the heading angle measurement (l/i), that is

�
formed from the decision structure.

rFALSE Is ,ehlcle's mOti E The measurement noise V2(t) is assumed to be zero-mean

Gaussian white noise with the noise covariance matrix of
I
Yaw ,:re":�'�;eshOl d '

Rz(k) (assumed to be diagonal). Since there are two

r- �--,
Fig. I A simple ''if - else" rule base for decision structure
Whether there is GPS outage or not, the structure checks
the yaw rate measurement from IMU whether the angular
motion of the vehicle about z-axis is slow or dynamic. The
threshold value is selected to be 0.1 radian / sec (5.73°/sec).
This value is found experimentally.
Also, a digital compass measures the heading from the
Magnetic North so it should be compensated with magnetic
declination (8) to align with the GPS heading measurement
which is with respect to True North as given in (7). Magnetic
declination is equal to + 4° 37' for IstanbullTURKEY.
•
measurement sources, RGPs(k) and Rcompass ( k ) are
different. The related noise covariance matrix is selected with
the related measurement used in Kalman filter in that time
interval. RGPs(k) is determined according to DOP value and
quality of the GPS solution experimentally. Rcompass ( k ) is
selected relatively bigger than RGPs(k) according to trust
degree.
Since measurements are made at 10 Hz and Kalman filter
works at 100Hz, the conventional Kalman filter design can not
be used. In this case, prediction steps are computed every time
but the update steps are computed each time the measurement
arrives.
After normalization is done to form the whole rotation
matrix R� using orthogonality property, R�(l:) could be
found by
R�(l:) = R�(2:)xR�(3:) (12)

l/lTrue North = l/lMagnetic North + 0 (7)

The block diagram of the process is given below in figure 2.

For fusing the GPS heading (l/lGPs) and the compensated

compass heading measurement ( l/lcompass ) ' a simple Roll and Pitch Angle

weighting equation is used where 0 :5 a :5 1 .

Measurements

An ular Rates

l/lFused = a l/lGPS + (1 - a) l/lcompass (8)

GPS
The parameter a is selected according to the quality of the
Heading
Measurement

GPS heading measurement and Dilution of Precision (DOP) DECISION

STRUCTURE
value. For example if the DOP is low (i.e. 1-5) and the quality Compass Heading
Measurement (fusion)
is Real Time Kinematics (RTK) then a is chosen to be 0.7 to
weight GPS measurement more.
The state space representation of the process model is given
as follows.
Fig. 2 Block Diagram of the DeM based Orientation Algorithm

(9)

674

Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.
 III. SYSTEM ARClllTECTURE AND TEST RESULTS The raw measurements from the sensors are given in below
The developed algorithms are tested on "Otonobil", which figures.
is a car developed as part of the Driverless Car Project

1'1 : -�.
conducted at the Istanbul Technical University (ITU)
Mechatronics Education and Research Center (MEAM).

�, .
�� 10 00

,-,�
� � 00 ro M 00
Otonobil is capable of three driving modes. It can be �

driven manually. The data logging was performed in manual

driving mode in a land area at ITU. It was done by using a
sensor read & data logging program running at 100 Hz written n :
��
� 10 � 00 � � 00 ro M 00
in Labview software. The data was logged in tdms format.
The trajectory marked on Google Earth software is given in
figure 3. The duration of the test was about 90 seconds and
during this time no GPS outages and no sensor failures were
"1 : �.
-0.50 10 20 30 40 50
Time(sec)
so 70 80 90

occurred naturally. The vehicle was at rest first 10 seconds
and last 5 seconds. The platform which is mounted on the
vehicle roof for GPS antennas and digital compass is given in
figure 4.
Fig. 5 Raw angular rate measurements of IMU
Raw Measure.,.nt. of He.:llng from CPS and Dlgltsl Compass
� r---r---r---.---�--,---r---r---.---,

soo

i
�250
1200
�

� 150
l 100
50
GPSHNdlng

00 10 30 40 50 60
•

70
Comp_ HMtIlng
80 90
Tlme(aec)

Fig. 6 Raw heading measurements of GPS and compass

Fig. 3 The trajectory of the data logging test

Fig. 7 Raw Roll and Pitch angle measurements of IMU

From the figure 5, it is seen that the roll and pitch rate
was not smooth because the land was rough. For land vehicles,
the yaw rate is much higher than the pitch and roll rate. The
Fig. 4 The test platform for GPS antennas and digital compass
roughness of the land is seen in figure 7 as the pitch and roll
The sensors that were used in the test are given in table 1. angle was not smooth. In figure 6, the slow dynamics of the
IMU data output rate was chosen as 100Hz. A two- antenna digital compass is seen. The time delay between GPS heading
GPS with RTK correction is used for heading solution and and compensated compass heading is almost two seconds.
data output rate was chosen as 10Hz. Also the digital compass The algorithm given in figure 2 was run at 100Hz in
is aligned with the GPS heading vector with the data output Labview software. The data log was read from the file and the
rate 10Hz. results were logged in tdms format again. In this simulation,
GPS outages were created artificially in the periods [10, 20],
Table 1 The list of the sensors that were used [25, 30], [40, 50], [60, 65] and [80, 85] seconds. The results
Sensor Type Model Sampling Frequency are shown in the following figures.
GPS Receiver Trimble SPS 851&551H 10 Hz
IMU Crossbow VG700AB-201 100 Hz
Digital Compass KVH Azimuth 1000 10 Hz

675

Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.
 Roll .,d Pitch Angle ntlrMta

"
:2f
�! A," �
t
. '0<-----;
,0 � V\
;;----<20;;---
- -;;';;-
0 ----;;
40;-----;;50;----..;;
60 0-----<;; ;0;' - 90
700----80
Tlmo(ooc)
KalrNm Filter Meuur.ment va Pitch Angle Estimate

'0

Fig. 8 Heading Measurement of Compass and GPS vs. estimated heading

Fig. 12 Roll and Pitch Angle &timate vs. Measurements

Kalman flit_ Aleasuntment vs v.. Estinwt_
�r---'----'---.--.---'

300
- Roll and Pitch Angle estimate
Roll and Pitch Angle measurement

j250
'"
�200
!i
! 150
100 Kalman
Tlmo(MC)
Altar Measurement va Pitch Angle Eltlmata
7
50 -estimated �Ing Angle
Ueasuntmenl used In KF
00 10 '0 '" 50 80
T1me(sec)

Fig. 9 &timated Heading vs. heading measurements used in KF

Kalman Filter Aleasuntment vs v_ Estlm.te

Fig. 13 Roll and Pitch Angle Estimate vs. Measurements in t=[50,70] sec
190

As in figure 8, the estimated yaw angle is compared with

the individual sensor measurements. Since GPS heading
system is more reliable than the compass unless an outage
occurs, the second Kalman filter trusts GPS measurement
more than the compass. It is also seen that the estimate is not
-EstlrMted HNdlng
HNdlng mNSUr.n.nt u-.d In KF affected from the time delayed measurements of compass in
34 36
dynamic motion. A zoomed plot is shown in figure 9 and 10
Fig. 10 &timated Heading vs. heading measurements in t= [32, 46] sec between 32nd and 46th seconds. A GPS outage occurred at
t=40sec and it is seen that the Kalman filter relied on its states
Decillion Made for beet l1'\e8Buramant more than the compass measurements. The measurements
were gone after a while since the vehicle was turning and
Kalman filter only propagated its states. The output of the
decision structure is shown in figure 11.
Figures 12 and 13 show the roll and pitch angle estimation
of first Kalman Filter. The estimated angles are better than the
measurements since the Kalman filter trusts its states more
than the measurements.

Fig. 11 The output of the decision structure
IV. CONCLUSIONS
In this paper, a multi-sensor data fusion approach based on
the DCM method is proposed for the estimation of orientation.
The DCM method is preferred since its process model is linear
unlike nonlinear process models of Quaternion and Euler
Angle derivative methods. This linear model enables the use
of basic Kalman filter structure for estimation of the states.
This way, the errors related to first order approximation are
avoided. A decision structure is used to have the best heading

676

Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.
 measurement presented in the system. By checking the
dynamics of the vehicle, slow characteristics of the digital
compass are overcome. Also artificial GPS outages were
created to test the performance of the algorithm and the
decision structure. The results indicate that the developed
algorithm works successfully for both attitude and heading
angles.

ACKNOWLEDGMENT

The authors would like to thank to State Planning Office

(DPT) for their support under the Grant Number 90143.

REFERENCES
[1] Jay.A.Farrell, Aided Navigation.. GPS with High Rate Sensors, New
York.. McGraw·Hill , c2008
[2] Jau-Hsiung Wang and Yang Yao, Multi-sensor Data Fusion for Land
Vehicle Attitude Estimation using a FuzzY Expert System, Data Science
Journal, Volume 4, pp 127-139 , 28 November 2005
[3] N.H.Quoc Phuong, Hee-Jun Kang, Young-Soo Suh, Young-Sik Ro ,A
DCM Based Orientation Estimation Algorithm with an Inertial
Measurement Unit and a Magnetic Compass, Journal of Universal
Computer Science , vol 15,no.4(2009) pp 859·876
[4] Xiaoying Kong, INS Algorithm Using Quatemion Model for Low Cost
IMU, Science Direct, Robotics and Autonomous Systems 46(2004) pp
221-246
[5] Jingrong Cheng, Yu Lu, Elmer R.Thomas and J.A.Farrell, Data Fusion
via Kalman Filter.. GPS and INS ,Autonomous Mobile Robots Chapter
3 pp 99-147
[6] Eun Hwan Shin, Accuracy Improvement of Low Cost INSIGPS for
Land Applications, Master of Science Thesis, University of Calgary
2001
[7] Crossbow VG700AAlAB User's Manual

677

Authorized licensed use limited to: ULAKBIM UASL ISTANBUL TEKNIK UNIV. Downloaded on January 23,2023 at 08:21:19 UTC from IEEE Xplore. Restrictions apply.