A Quaternion-Based Tilt Angle Correction

Method for a Hand-Held Device Using an Inertial

Measurement Unit
Seong-hoon Won1, Neda Parnian2, Farid Golnaraghi3, and William Melek4
1, 2, 4
University of Waterloo, 200 University Avenue West, Waterloo, Ontario, Canada N2L 3G1
3
Simon Fraser University, 8888 University Drive, Burnaby, B.C., Canada V5A 1S6
1
E-mail: [email protected]
2
[email protected]
3
[email protected]
4
[email protected]

Abstract —A gyro-based orientation sensor is prone to orient combine the tilt angle measurements from a triaxial
drift due to an integration step. However, a triaxial accelerometer accelerometer and the orientation measurements from gyros.
does not require any integration step to calculate the tilt angles, Also, a hybrid Kalman filter is proposed in [9]. This method
and the calculated tilt angles do not drift over time. In order to uses one Kalman filter to estimate the orientation with gyros
find the tilt angles from a triaxial accelerometer, the sensor should when a robot accelerates, but uses another Kalman filter to
be in an acceleration-free condition. In this paper, an expert
system is proposed to identify the stationary state of an inertial
estimate the orientation by including three accelerometers when
measurement unit (IMU). A Kalman filter is designed to reduce the robot does not accelerate.
the noises of the sensors to make the expert system more reliable. In order to use a triaxial accelerometer as a tilt sensor, an
To validate the tilt angle correction method, two different tests are acceleration-free state must be identified from the IMU
conducted: static and dynamic. When an IMU remains stationary measurements. A fuzzy algorithm is developed to detect
for 30 seconds, almost no angular error is observed: The yaw angle quasi-static states of the sensor by using accelerometers and
stayed at almost 0° for 30 seconds, and the roll and pitch angles are gyros [10]. In [11], a fuzzy expert system is chosen to identify
derived from the accelerations measured by accelerometers. For if a vehicle is stationary from accelerometer signals. When the
the dynamic test, the IMU is moved and then returned to the
jerks in each axis of the vehicle are low, the system assumes that
original orientation. The roll and pitch angles are almost perfectly
corrected but the yaw angle exhibits no significant improvement.
the vehicle is stationary and corrects the orientation and the
velocities of the vehicle. In [12], a rule-based fuzzy logic is
I. INTRODUCTION applied to estimate the dynamics of the vehicle. Depending on
the dynamic states of the vehicle, different sensors are selected
T raditionally, an IMU has been used in inertial navigation
systems, but the development of a micro electro-mechanical
to estimate the orientation.
This paper proposes a new tilt angle correction method for a
system (MEMS) IMU has opened up new applications such as hand-held device based on quaternion orientation
human motion study [1]-[3], orientation sensor [4], and representation properties. This method uses a triaxial
manufacturing [5]. The gyros from an IMU can be employed to accelerometer to measure the tilt angles of the IMU when the
calculate the orientation, but the orientation calculation drifts IMU is stationary and corrects the quaternion terms according
over time due to the integration of gyro errors such as random to the tilt angles. An expert system is employed to identify if the
walk and noise. To reduce orientation errors, various correction IMU is stationary. When the sensor is stationary, the angular
methods that use a triaxial accelerometer as an inclinometer velocity is set to zero to prevent further drift and corrects the tilt
have been reported in the literatures [6]-[12]. angles. Experimental results are presented in this paper to
A triaxial accelerometer measures the gravity vector with verify the proposed method.
respect to the sensor frame when the sensor does not accelerate.
Unlike the orientation calculated by gyros, the tilt angles that are II. ORIENTATION CALCULATION
measured by a triaxial accelerometer do not drift over time
because the angle calculation does not require any integration A direction cosine matrix consists of three unit vectors which
step. In spite of this advantage, a triaxial accelerometer is used represent three body axes projected onto three reference axes.
only to aid gyros to correct the orientation of an object because These vectors form three columns of a direction cosine matrix.
the accelerometer cannot measure the yaw (heading) direction. The matrix from the body frame to the reference frame is
It is demonstrated in [6] how to correct the tilt angles by described as follows:
multiplying the correction angular velocity that are calculated
based on the acceleration measurements. This method yields ⎡c Xx cYx c Zx ⎤
improved results when the sensor is stationary, but still exhibits C = ⎢⎢c Xy cYy c Zy ⎥⎥
r
b (1)
some drift. In [7, 8], Kalman filtering techniques are used to ⎢⎣c Xz cYz c Zz ⎥⎦

k,((( 
 where subscript X, Y, and Z are the orthogonal unit vectors of the q = q 0 + q v = cos(θ ) + u sin(θ ) (7)
reference frame (r), and x, y, and z are the corresponding vectors
of the IMU body frame (b). To rotate a 3D vector v where u is a 3D unit vector ( u = u x i + u y j + u z k ). Then, when
( v = v x i + v y j + v z k ) with a direction cosine matrix, it follows
vector v is rotated by angle θ by using (4), the quaternion
that components of q become

v' = Cbr ⋅ v (2) θ θ θ

q 0 = cos , q1 = u x ⋅ sin , q 2 = u y ⋅ sin , and
2 2 2
where vector v’ is the rotated 3D vector. θ
q 3 = u z ⋅ sin.
The Euler attitude representation is easy to understand but 2
requires a predetermined sequence to find a rotation matrix. The differential equation of quaternion q with respect to time
Since different sequences of rotation result in different rotation has the following matrix form:
matrices, the orientation error accumulates over time. In
addition, the Euler attitude representation has a singularity 1
q k = Qk ⋅ ω k
problem at certain angles; thus, the application of the Euler 2
angle representation is limited and is not suitable for accurate ⎡q 0 ⎤ ⎡ q 0 − q1 − q 2 − q3 ⎤ ⎡ 0 ⎤
3D orientation applications. However, the quaternion attitude ⎢ ⎥ ⎢ ⎥ ⎢ ⎥. (8)
representation obtains a rotation matrix with a single rotation ⎢ 1 ⎥ = 1 ⎢ q1 q 0 − q 3 q 2 ⎥ ⋅ ⎢ω x ⎥
q
about one axis and does not have a singularity problem. A ⎢q 2 ⎥ 2 ⎢q 2 q3 q 0 − q1 ⎥ ⎢ω y ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
quaternion consists of one real number and three imaginary ⎣ q 3 ⎦ ⎣ q3 − q 2 q1 q 0 ⎦ ⎣ω z ⎦
numbers as
Then, the quaternion terms after rotation are
q = q0 + qv = q0 + (q1i + q 2 j + q3 k ) (3)
q k = q k −1 ⋅ Δt + q k −1 (9)
where q0 , q1 , q 2 , and q3 are real numbers. To rotate a 3D
vector v, the vector is multiplied by quaternion q and by the where qk is the quaternion at the kth term, and Δt is the
conjugate of quaternion q, denoted as q*, as follows: sampling time.

v′ = q ⋅ v ⋅ q * (4) III. ORIENTATION CORRECTION METHOD

A Kalman filter and an expert system are designed to correct
From (2) and (4), the direction cosine matrix from the body the orientation calculation. Fig. 1 depicts the structure of the
frame to the fixed frame in quaternion terms becomes system. An IMU consists of three accelerometers and three
gyros that measure the linear accelerations and angular
⎡c Xx cYx c Zx ⎤ velocities, respectively. After a Kalman filter is employed to
C = ⎢⎢c Xy cYy c Zy ⎥⎥
f
b reduce the noise of the sensors, an expert system is used to
⎢⎣c Xz cYz c Zz ⎥⎦ verify that the IMU is stationary by using the filtered
(5) measurements. When the IMU is stationary, the tilt angles of
⎡q 02 + q12 − q 22 − q32 2(q1 q 2 − q 0 q3 ) 2(q1 q3 + q0 q 2 ) ⎤ the IMU are determined from the triaxial accelerometer, and the
⎢ 2 2 2 2 ⎥ orientation is corrected. If the IMU is moving, the orientation is
= ⎢ 2(q1 q 2 + q0 q3 ) q 0 − q1 + q 2 − q3 2(q 2 q3 − q0 q1 ) ⎥.
⎢ 2( q q − q q ) calculated by using only gyros.
2(q 2 q3 + q 0 q1 ) q 0 − q1 − q 2 + q3 ⎥⎦
2 2 2 2
⎣ 1 3 0 2

A. Kalman Filter
When a unit quaternion is used, (3) should satisfy The Kalman filter is an optimal observer that estimates the
states of a linear dynamic system from noisy measurements. In
2 2 2
q 0 + (q1 + q 2 + q3 ) = 1 .
2
(6) the proposed method, the Kalman filter is used to reduce the
noise of the sensor to find more accurate tilt angles and to detect
the stationary state with a higher reliability. When both
Then, (3) can be written as
accelerometers and gyros are accurately calibrated, the model

IMU
Angular velocity Orientation
Accelerometers Stationary Orientation
Kalman state
filter Acceleration Angular velocity correction
detection
Gyroscopes

Fig. 1: Overview of the proposed orientation correction system

k,((( 
 for the IMU system in the discrete time domain becomes (a) 0.12
(b) 0.12
0.09 0.09

Acceleration (m/s )
x k = I 6×6 x k −1 + wk −1 (10)

2
0.06 0.06

y k = I 6×6 x k + e k (11) 0.03 0.03

0 0
-0.03 -0.03
where x k is the state at time t k , wk −1 is the system noise at -0.06 -0.06

time t k −1 , y k is the measurement at time t k , and ek is the -0.09 -0.09

-0.12 -0.12
measurement noise at t k . 0 0.5 1 1.5 2 0 0.5 1 1.5 2
Time (sec) Time (sec)
Fig. 2 illustrates a comparison of the acceleration results with
Fig. 1: Acceleration comparison of different systems
and without a Kalman filter when the sensor is stationary. Since (a) without the Kalman filter and (b) with the Kalman filter
the acceleration with a Kalman filter has a much lower noise, the
resultant tilt angles have less noise, and the expert system can where the average accelerations are expressed as
identify the stationary state with a higher reliability.
n

B. Stationary State Detection Avg _ Ax = ∑ A (i) / n,

i =1
x

The tilt angles are measured with a triaxial accelerometer

n
when the IMU is stationary. To identify this state, the dynamic
states are related to the IMU measurements. When the IMU is
Avg _ Ay = ∑ A (i ) / n,
i =1
y (15)
stationary, the angular velocity is zero, the accelerations in each n
axis are constant, and the magnitude of the acceleration is equal
to the magnitude of the gravity vector. However, since a gyro
Avg _ Az = ∑ A (i) / n
i =1
z

has a random walk property and noise, the angular velocity

measurements are usually not zero. In addition, the acceleration where n is the number of acceleration samples. While the IMU
measurements contain noise, and the magnitudes of the is stationary, n increases to obtain more steady average
acceleration measurements due to gravity vary depending on the accelerations.
tilt angles because of the non-linearity property of the When the IMU is stationary, the gravity-free acceleration
accelerometers. To identify the stationary state from these (Acc) should be lower than the maximum error of the
inaccurate measurements, an expert system is incorporated by accelerometers which includes non-linearity error and the
using acceleration estimations, angular velocity estimations, maximum noise after filtering. In addition, the magnitude of the
and acceleration fluctuations. An expert system consists of angular velocity (Ang_vel) should be lower than the maximum
rules in the form of “If A, then C”, where A is a set of error of the gyro which includes random walk and the maximum
antecedents and C is a set of conclusions. When A is satisfied, C noise after filtering. If an IMU is moved by a person, it is
is true. unlikely to move with a constant acceleration in each axis for 1
Let the magnitude of gravity-free acceleration (Acc) and the second and with almost a zero angular velocity. Therefore, the
magnitude of angular velocity (Ang_vel) be expressed as IMU is assumed stationary when these conditions are met as
follows:
2 2 2
Acc = ( Ax + A y + Az ) 0.5 − g , (12)
Rule 1:
2 2 2
Ang _ vel = (ω x + ω y + ω z ) 0.5 (13) 2 2 2
IF Acc < ( Ax _ max_ error ) + ( Ay _ max_ error ) + ( Az _ max_ error )

where Ax, Ay, and Az are the acceleration measurements in each AND
2 2 2
axis direction after filtering, g is the magnitude of the gravity Ang _ vel < (ω x _ max_ error ) + (ω y _ max_ error ) + (ω z _ max_ error )
vector, and ω x , ω y , and ω z are the angular velocity AND
2 2 2
measurements in each axis after filtering. Acc _ fluc < ( Ax _ max_ noise ) + ( Ay _ max_ noise ) + ( Az _ max_ noise )
When the IMU is stationary, the accelerations of each axis for the last 1 second data, THEN IMU is stationary
should be constants. Thus, the fluctuation of the acceleration ELSE IMU is moving.
measurements in each axis should be less than the maximum
acceleration noise from the Kalman filter. The acceleration
where Aaxis _ max_ error is the maximum acceleration error in each
fluctuation (Acc_fluc) is defined as
axis, ω axis _ max_ error is the maximum angular velocity error in
2 2
Acc _ fluc = (( Ax − Avg _ Ax ) + ( Ay − Avg _ Ay ) each axis, and Aaxis _ max_ noise is the maximum acceleration error
(14)
+ ( Az − Avg _ Az ) 2 ) 0.5 . in each axis, after filtering.
When the IMU is stationary, three accelerometers measure the
tilt angles, the angles between the gravity vector and the IMU

k,((( 
 body frame axes. The relationship between the tilt angles and c Xz × q3 _ cor _ 0 + cYz × q0 _ cor _ 0
the direction cosine matrix should be established to correct the q1 _ cor _ 0 = 2 2
(23)
2 × (q0 _ cor _ 0 + q3 _ cor _ 0 )
tilt angles. When the Z-axis of the reference frame is chosen to
be in the opposite direction of the gravity vector, the tilt angle −c Xz × q0 _ cor _ 0 + cYz × q3 _ cor _ 0
q 2 _ cor _ 0 = (24)
components of the direction cosine matrix from (1) are c Xz , 2 2
2 × (q0 _ cor _ 0 + q3 _ cor _ 0 )
cYz , and c Zz . The tilt angle components can be derived by
normalizing the three acceleration components. To reduce the where q 0 _ cor _ 0 , q1 _ cor _ 0 , q 2 _ cor _ 0 , and q 3 _ cor _ 0 are the
acceleration measurement noise, the average values are used corrected quaternion terms. Eq. (22) and (23) should be
instead of the instantaneous accelerations to calculate the tilt calculated after (20) and (21) is calculated because (22) and
angles. The tilt angle components in terms of the average (23) uses corrected terms. Since a quaternion has four
accelerations in each axis become components, four correction possibilities are evaluated. The
corrected terms are fed to (20) and the corrected terms that do
Avg _ Ax not satisfy (20) were discarded. From all possible quaternion
c Xz = 2 2 2
,
( Avg _ Ax + Avg _ Ay + Avg _ Az ) corrections, the correction with the minimum root-mean-square
error (RMSE) is chosen as the best correction possibility. To
Avg _ Ay
cYz = , (16) prevent from further orientation drift, the angular velocity is set
2 2 2
( Avg _ Ax + Avg _ Ay + Avg _ Az ) to zero when the IMU is stationary. Thus, when the IMU is
− Avg _ Az stationary, it follows:
c Zz = 2 2 2
.
( Avg _ Ax + Avg _ Ay + Avg _ Az )
Rule 2:
IF the IMU is stationary, THEN ω x = 0 , ω y = 0 , ω z = 0 AND
From (5), the quaternion terms in the tilt angles are
correct the quaternion terms AND n = n+1
2(q1q3 − q0 q2 ) = c Xz , (17) ELSE n = 1/sampling time.
2(q2 q3 + q0 q1 ) = cYz , (18)
IV. EXPERIMENTS
2 2 2 2
q0 − q1 − q2 + q3 = c Zz . (19)
The proposed orientation estimation method is tested with a
MEMS IMU (MDIMU-I-221-777, Mechworks Systems). The
From (6) and (17), there are four equations and four unknowns, IMU consists of two 2g accelerometers, one 10g accelerometer,
but the equations cannot be solved analytically since the and three 75°/sec gyros. The orientation correction method is
equations are in non-linear forms. To find the four quaternion tested under two conditions: static and dynamic.
terms, one of them is fixed, and the other three terms are For the first, the IMU is left stationary in a horizontal plane for
corrected. From (19) and (6), 30 seconds, and the orientation is computed as seen in Fig. 3.
Fig. 3 (a) reveals the orientation calculation by the angular
2 2 2 2
1 − 2q1 − 2q 2 = 2q0 + 2q3 − 1 = c Zz . (20) velocities from the gyros, and Fig. 3 (b) shows the orientation
calculation by the tilt angle correction method. Fig. 3 (a) shows
When q0 is fixed, the corrected quaternion terms from (17), that the orientation drifts over time. With this method, the
reference frame matches the initial body frame. Fig. 3 (b)
and (18) and (20) become
illustrates that the orientation slowly drifts for only the first 1
second. At 1 second, the expert system concludes that the IMU
q 0 _ cor _ 0 = q 0 (21)
is stationary and calculates the roll and pitch angles from the
q3 _ cor _ 0 = sign(q3 ) × ( c Zz + 1 − 2 × q02 _ cor _ 0 / 2) 0.5 (22) gravity vector. Since the Z-axis of the reference frame is
parallel to the gravity vector in this method, the roll and pitch
angles are corrected with respect to the accelerometer

(a) 3 (b) 1
roll
2
pitch 0.8
yaw
0.6 roll
Angle (°)

1
Angle (°)

pitch
0.4
yaw
0
0.2
-1
0

-2 -0.2
0 5 10 15 20 25 30 0 5 10 15 20 25 30
Time (sec) Time (sec)
Fig. 3: Orientation calculation when the IMU is stationary (a) with the correction method and (b) without the correction method.
k,((( 
 TABLE I
ORIENTATION ERROR COMPARISON BETWEEN WITH AND WITHOUT THE TILT ANGLE CORRECTION METHOD
Orientation error Orientation error
with correction method(°) without correction method (°)
Roll Pitch Yaw Roll Pitch Yaw
Test 1 (after 30 seconds) -0.1 -0.3 -0.4 -0.6 -1.9 -0.5
Test 2 (after 60 seconds) 0.1 0.1 1.1 3 9.2 1.4
Test 3 (after 90 seconds) 0 -0.1 0.8 -0.8 2.5 1

50 whereas the roll and pitch angle values change in relation to the
roll
40 pitch
average acceleration values. For the second experiment, the
yaw IMU moves and then returns to its original orientation. The tilt
30
angles are corrected accurately, but the yaw angle shows no
Orientation (°)

20 significant improvement with the proposed tilt angle correction

10 method.
0
REFERENCES
-10
[1] T. Liu, Y. Inoue, K. Shibata, and X. Tang, “A Wearable Inertial Sensor
-20 System for Human Motion Analysis,” in Proceedings 2005 IEEE
International Symposium on Computational Intelligence in Robotics
-30
0 10 20 30 40 50 60 70 and Automation, Finland, June 27-30, 2005, pp. 409-514.
Time (sec)
[2] H. J. Luinge, P.H. Veltink, and C.T.M. Baten, “Ambulatory
Fig. 4: Orientation outputs of Test 2 of Table I Measurement of Arm Orientation,” Journal of Biomechanics, vol.
40, no. 1, pp. 78-85, 2007.
measurements at 1 second. After 1 second, the roll and pitch [3] H. J. Luinge and P. H. Veltink, “Measuring Orientation of Human
angles demonstrate very small fluctuations for the next 5 Body Segments Using Miniature Gyroscopes and Accelerometers,”
seconds, but as more acceleration data are used for the average Medical & Biological Engineering & Computing, vol. 43, pp. 273-282,
2005.
acceleration calculations, more steady tilt angles are observed.
However, the yaw angle does not fluctuate at all after 1 second [4] A. Kim and M. F. Golnaraghi, “A Quaternion-Based Orientation
because the angular velocities in all three directions are set to Estimation Algorithm Using an Inertial Measurement Unit,” in
Proceedings IEEE Position Location and Navigation Symposium
zero and the yaw angle is not affected by the gravity vector. 2004, Monterey, CA, April 26 - 29, 2004, pp. 268-272.
For the second test, the IMU is placed horizontally and rotated
in random continuous motions for 30 seconds, 60 seconds, and [5] N. Parnian and M. F. Golnaraghi, “Integration of vision and inertial
sensors for industrial tools tracking,” Sensor Review, vol. 27, no. 2,
90 seconds, and then returned to the original orientation. Table pp.132-141, 2007.
I summarizes the results of the orientation errors with and
[6] Lekskulchai Pongsak, Masafumi Okada, and Yoshihiko Nakamura,
without the tilt angle correction method. The results reveal that
“Attitude Estimation by Compensating Gravity Direction,” in
the errors in the roll and pitch direction are almost zero degrees Proceedings of the 21st Annual Conference of Robotics Society of
when the tilt angle correction system is implemented. However, Japan, 2A23, 2003.
the error in the yaw direction, which is parallel to the gravity [7] H. J. Luinge, P. H. Veltink, and C.T.M. Baten, “Estimation of
vector, cannot be eliminated because the rotated angles along Orientation with Gyroscopes and Accelerometers,” Technology and
the gravity vector have the same tilt angles. There are small Health Care, vol. 7, pp. 455-459, 1999.
errors in the roll and pitch angles, but it might be an [8] J. Vaganay, M. J. Aldon, and A. Fournier, “Mobile Robot Attitude
experimental error. The orientations of Test 2 of Table 1 are Estimation by Fusion of Inertial Data,” in Proceedings of the 1993
shown in Fig. 4. In this figure, a sudden angle changes in the roll IEEE International Conference on Robotics and Automation, May
and yaw angles at 65 second due to the proposed tilt angle 2-6, 1993, Atlanta, GA, USA, pp. 277-282.
correction method. [9] H. Rehbinder and X. Hu, “Drift-free Attitude Estimation for
Accelerated Rigid Bodies,” Automatica 40, pp. 653-659, 2004.
V. CONCLUSIONS [10] Ashutosh Saxena, Gaurav Gupta, Vadim Gerasimov, and Sébastien
Ourselin, “In Use Parameter Estimation of Inertial Sensors by
This paper proposes a tilt angle correction method based on Detecting Multilevel Quasi-static States,” In Ninth International
the quaternion attitude representation calculation. A Kalman Conference on Knowledge-Based Intelligent Information &
filter is developed to reduce the noise of the IMU to achieve Engineering Systems, Melbourne, Australia, September 2005, pp.
higher accuracies of the tilt angles and to improve the reliability 595-601.
of the expert system. The steady state of the IMU is related to [11] J-H Wang and Y. Gao, “Multi-sensor Data Fusion for Land Vehicle
the outputs of the IMU to identify if the IMU is stationary. If it Attitude Estimation using Fuzzy Expert System,” Data Science
is, the correction method calculates the tilt angles of the IMU Journal , vol. 4, pp.127-139, 2005.
based on the acceleration measurements. In order to test the [12] L. Ojeda and J. Borenstein, “FLEXnav: Fuzzy Logic Expert
proposed method, static and dynamic tests are conducted. Rule-based Position Estimation for Mobile Robots on Rugged
The first experimental result shows that the proposed method Terrain.” in Proceedings of the 2002 IEEE Int. Conference on
Robotics and Automation, May 11-15, 2002, Washington, DC, USA,
can identify the stationary state and correct the tilt angle error. pp. 317-322.
When the IMU is stationary, the yaw angle does not change
k,(((