Contrtd Eng, Practice, VoL 2, No. 2, pp.

281-287, 1994
Pergamon ~ © 1994l~ev~rSciea~l.,td
Pan~ i. ~ eaua.. ~ aOa re.m~t
0967-0661/94 $6.00 + 0.00

ATTITUDE ESTIMATION FOR A VEHICLE USING

INERTIAL SENSORS
J. Vaganay and MJ. Aldon
URMM, URM CNRS 9928, Universit~ Montpeilitr !I, 161 Rue Ada, 34392 Mompellier Ct.dex 5, France

Abstract. This paper presents an attitude estimation system for an autonomous ground vehicle
which is based on the cooperation of low-cost inertial sensors. The attitude is obtained using two
different methods. The fLst method is based on acceleromelric measurements of gravity. The
second one proceeds by integration of the differential equation relating the robot's attitude and its
instantaneous angular velocity which is measured by gyrometers. The advantages of these two
solutions are exploited by fusing together the redundant attitude information they provide using
an extended Kalman filter.This algorithm has been implemented and tested on a vehicle.
Experimental results are presented.

Key Words. Inertial navigation, Acceleration measurement, Angular velocity measurement,

Kahnan filters, Vehicles, Robots.

1. Ilff'I"RODUCTION environment and allow an orientation update. But

A fundamental requirement for an autonomous
vehicle is its ability to localize itself with respect to
its environment. Navigation on a flat and horizontal
ground only requires estimations of position and
heading (Vaganay et ai. 1993a). However, in many
cases, the environment is not so well structured, and
the angular orientation of the vehicle may change
along its path. In this case, a real-time estimation of
the attitude may be necessary. All-terrain
navigation, for example, requires at least rough
measurements of the pitch and roll angles to ensure
vehicle safety. Some specific tasks, like those
executed by robots for road construction, require
controlling of the attitude of a tool (Le Corm and
Garcia, 1992). Indoor mobile robots generally need
an accurate navigation system. If they move on
inclined planes, the orientation can be used to
correct the position and heading estimations
provided by odomelry, or to interpret external sensor
data in order to build an accurate model of the
environment (Vi6ville and Faugeras, 1989a; Bhanu
etal. 1990).
The attitude estimation may be done by using
exteroeeptive sensors like cameras (Wang and
Wilson, 1992), or range-finders, which perceive the
these methods are not robust and reliable enough and
they are generally too time-consuming. On the other
hand, new research is being developed on the
implementation of inertial sensors on robots. As a
matter of fact, the inertial navigation systems
developed for airplanes cannot be used for ground
vehicles for two reasons. First, the motion models
do not contain the same parameters and the required
perfonnanc~ are fliffcrent. Second, these systems are
too expensive for robotic applications (Frappier,
1990; Turpin, 1986).
The method presented in this paper is based on the
following ideas. Pitch and roll angles may be
estimated by using the gravity components deduced
from the measurements of two accelerometers
(Ferrand. 1991). However, this solution is not well
adapted to the dynamic localization of ground
vehicles for two major reasons :
- when the robot moves, an estimation of its own
acceleration must be available,
- measurements are subject to large fluctuations
when the motion occurs on uneven ground.
Thus, a multisensor fusion method is proposed
(Vaganay, 1993b). It consists of correlating these
angle estimations with those resulting from the
integration of the angular velocities measured by

281
282 J. Vaganay and M.J. Aldon

three orthogonal gyrometers. This paper describes A = [ Ax Ay Az ]T = [y(Oa/R0)] r _ [g]r. (5)

the algorithms which have been developed. The
system has been implemented and successfully
tested on a mobile platform equipped with low-cost 3.1. Static Determination
sensors. Experimental results are presented and
discussed in the last section. Roll and pitch are calculated from the components
of gravity in flame Rr, which are directly measured
by the accelerometers. Indeed, when [y(Oa/R0)]r =
2. REPRESENTING THE ROBOTS ATTITUDE 0, eqnmion (4) becomes :
Let R0 be an absolute reference frame such that z0
A = -[g]r = [gx gy gz] T.
is vertical and directed upward. A frame Rr is
attached to the robot. Its origin Or is in the middle
The rotation matrix r[A] 0 from R r to R0, is given
of the tread, which is the distance between the front
wheels. Xr is pointing in the direction of by :
displacement of the robot, and Zr is directed upward.
r[A]0 = Rot (x,-0).Rot (y,-¥)
Roll-pitch-yaw angles (O,v,O) can be used to
represent the attitude and the heading of the robot. If
I CV 0 "Sv 1
the direction cosines matrix 0[A] r, defining the r[A]0= S¥S0 C0 C¥S0
attitude and the heading of R r is given, the roll-
s¥co -so cvco
pitch-yaw angles can be extracted as follows :
The components of gravity in frame R0 are known
Sx nx ax
and those in frame Rr are measured; they are related
0lAir= Sy ny ay by :
r
Sz n z a z
"[g]r =" [A]o[g]o"
0 = A r c t a n - ~ _+km (1)
SX
The expansion of this equation gives :
-Sz
¥ = Arctan C0sx+S0s,, (2)
y
S0ax-C0ay
0 = Arctan -b^nx+C0nya~

where CO=cos O, and SO=sin O.

(3)
gy = - c. 0 s li: 1
SvS0 c0 CvS0
sVc¢ -so c v c o .g
gz

Finally, the expressions of the attitude angles as

3. DETERMINING THE ATITIUDE WITH TWO functions of the components of gravity in frame Rr
ACCELEROMETERS altO:

An accelerometer provides an output voltage V = -Aresin gx (6)

g
proportional to the projection on its sensitivity axis
of the acceleration applied to it (Radix, 1980; 0 = Arcsin ~gCv or 0 = Arccos gC~"
Sz (7)
Vidville and Faugeras, 1989b). Consider a set of
three accelerometers rigidly attached to the robot, Note that two accelerometers are sufficient to
such that their sensitivity axes define an orthogonal determine the attitude. This is why only the x and y
frame Ra with origin Oa. If y(Oa/R0) designates accelerometers will be considered in the following
the absolute acceleration of the robot and g the sections.
gravity vector, the accelerometric output is :

A = [y(Oa/RO). g]a. (4) 3.2. Dynamic Determination

The accelerometers are attached to the robot so that
the transformation between Rr and Ra is reduced to
the translation OrOa=[dx dy dz]T (no alignment
error) which has to be calibrated. With this
assumption, (4) can be rewritten as follows :
When the robot moves, the non-gravitational
acceleration [y(Oa/RO)]r has to be removed from the
accelerometric output (5). Since this acceleration is
not directly measurable, it has to be approximated.
It is given by :
 Attitude Estimation
y(Oa/R0) = y (Or/R0)+~(Rr/R0)^OrOa
+ o (Rr/RO)^[O(Rr/R0)^OrOa] (s)
where Q is the angular velocity of the vehicle.
The x and y components of y (Oa/R 0) have to be
computed. Odomelxy gives an estimate of the
translational velocity Vx(Or/R0) which can be used
to approximate Y x ( O r / R 0 ) by numerical
differentiation :
d
vx(Or/R0) = ~ Vx(Or/R0).
Odometry also gives an estimate of the heading
velocity r of the robot. The centrifugal acceleration
can then be approximated by :
~(Or/RO) - Vx(Or/Ro).r.
The angular acceleration is obtained by numerical
differentiation :
d But this representation is not used in the differential
Q(Rr/R0) = [ i)dl f ]T = ~ ~(Rr/R0)"
Using (8), the x and y components of the non-
gravitationalaccelerationcan be expressedby :
Yx(Oa/R0)=Vx(Or/R0)+q dz-Ldy-~(p.dy-q.dx)
-r(r.dx-p.dz)
7y(Oa/R0)f~y(Or/R0)+f.dx-~.dz+r(q.dz-r.dy)
The components of gravity become :
gx = Ax '/x(Oa/R0)
-
gy = Ay- 7y(Oa/R0).
Roll and pitch angles can then be determined using
(6) and (7).
It can be noted that the accelerometers by
themselves are not sufficient to determine the
attitude. Gyrometric and odometric measurements
are necessary to estimate y(Oa/R0).
4. ESTIMATING THE ATITFUDE WITH
GYROMETRIC MEASUREMENTS
A gyrometer delivers an output voltage proportional
to the projection on its sensitivity axis of the
absolute angular velocity applied to it. Consider
three gyrometers attached to the vehicle such that
their sensivity axes define an orthogonal frame Rg.
The gyrometric output is :
283
Q [Q (Rg/R0)]g. (9)
Identically, the gyrometers are attached to the robot
so that the transformation between Rr and Rg is
reduced to a translation (no alignment error). In this
case, the gyros give a direct measure of the robot's
absolute angular velocity. The gyrometric output (9)
can be expressed as follows :
o = [p q r ]T= [O(Rr/RO)]r. (10)
The attitude can be determined using these
gyrometric measurements. This method also allows
estimation of the heading (yaw), which is not
possible with the accelerometers. In this case, a
differential equation relating the attitude and the
instantaneous angular velocity has to be integrated.
The form of this equation depends on the choice
made in representing the attitude (Radix, 1980;
Grubin, 1973). Roll and pitch angles are used as
outputs of the system to define the robot's attitude,
because they have a direct physical interpretation.
equation. Quaternions have been chosen, mainly
because they never lead to singularities. Once the
quatemion is obtained, the direction cosines matrix
is computed. Roll, pitch, and yaw are then extracted
(1,2,3) so that the singularity problem is eliminated
from the integration process. Using quatemions, the
differential equation to be solved takes the form :
1
u m
m m
0 -p q -r Qo
Q1 1
p 0 r -q Q1
q -r 0 p Q2
Q3 r q -p 0
m
Q3
m
where:
Q = Q 0 + Q I . i + Q 2 . J + Q 3 . k is the quaternion
associated with the attitude of the robot,
O=[p q r]T is its instantaneous angular velocity.
This differential equation has an analytical solution
only when the robot rotates around a fixed axis in
Rr and R0 (Radix, 1980), which is generally not the
case. A numerical integration method must then be
used. Grubin (1973) compared different integration
algorithms (rectangular, trapezoidal, third-order
Runge-Kutta) and found that the best results were
given by a third-order algorithm derived by Edwards
284 J. Vaganayand M.J. Aldon

(1971), after minor changes. This algorithm is used and the quatemions :
to solve the attitude equation.
= a 1 .i+a2.j+a3.k
= al.i+a2.J+a3.k
4.1.~
r. a a 2 1 a.a.)l"
Q(t) = Q(t-at) (a*.a -
Euler's theorem states that an arbitrary sequence of
rotations with one point fixed applied to a rigid
body leads to a final attitude that can be obtained 1
The analytical solution of Q = ~ Q l'~ (with the
using a single rotation about an axis which passes
restriction concerning the rotation axis) is :
through the fixed point. Considering the rotation t
from the absolute reference frame to the robot's
frame, let uffi[UxUy Uz]T expressed in the absolute Q(O = Qo.exp(S-~- d'C)= Qo.exp@)
reference frame be the rotation axis, and let 13be the o
rotation angle about u. The attitude quaternion a 1 a 2 1 a 3
associated to this rotation is:

Q=Qo+Q 1.i+Q2.J+Q3.k It appears that the solution given by this algorithm

corresponds to a third-order approximation of the
where:
analytical solution. The term ~1 (~t*.a - o r . a * )

Q0=c°s 2~ ' Ql=Ux'sin ~2' Q2=Uy"sin [~ corresponds to a correction applied to account for the
2'
fact that the rotation axis is not fixed.
Q3fuz.sin ~2
At each step, a normalization of the updated
quatemion is perfumed :
and where i, j, k are such that"

ii =jj = kk =-1
ij = -ji =k, jk = -kj = i, ki = -ik = j Q -'->Q/IQ.

Once the quaternion is determined, the corresponding

The direction cosines matrix can be expressed in
direction cosines matrix is computed (11) and the
terms of the quatemion components by :
roll-pitch-yaw angles are extracted (1,2,3).

Q(t0) is initialized by accelerometric measurement

of gravity when the robot is static at the initial
position.
t.. ~ Q ~ ~t~) 2(~')-1
(11) 5, FUSING ACCELEROMETRIC AND
GYROMETRIC ATrrrUDE ESTIMATIONS
4.2. Integration Aleorithm
The fusion concerns only roll and pitch estimations
since yaw cannot be estimated with the
Integrated gym outputs on time intervais [t-2At;t-
aecelerometers.
At] and [vAt;t] are needed to compute Q(O knowing
Accelerometers allow a long-term control of the
Q(t-At). Define :
attitude because this attitude estimation has a good
t-At t-At t-At mean value. On the other hand, the instantaneous
~# 10g estimate can be completely wrong because robot
a l ffi ~p.dx a2 = ~q.d~ a3 = ~r.dx vibrations are converted into angles and because
t-2At t-2At t-2At numerical differentiations are used. Oyrometers give
a good short-term evolution of the attitude but this
t t t estimation is subject to drift with time (gyrometer
0tl ffi Ip.dx a2 = Iq.dx a3 = I r'dx drift, integration process). A fusion by extended
t-At t-At t-At Kalman filtering is performed to take advantage of
both methods.
 Attitude Estimation
In order to correct the gyrometric estimation of the
attitude, it is necessary to estimate the mean drifts
on pitch and roll, due to the sensor's drifts and to the
integration process. Measurements on the
experimental site tend to show that the following
model can be used :
Vg(O = v(t) + d~t).t (12)
#g(O = *(0 + dt(O.t (13)
where y g and Og are determined by the gyrometric
method, ¥ and ¢~define the real attitude, and d~ and
d~ are the mean drifts between the initial time and
time instant t.
The state vector X is made up of the pitch and roll
angles as well as their drifts. Since the angular
movement of the robot is unknown, the state
lransition equation is :
= +vk <=> Xk+ 1 = Xk + Vk
k+l k linear phases. During the first linearpart,at time
where Vk is the state noise vector presumed
~lated, zero-mean, and Gaussian with variance
Qk.
The observation vector Z, given below, contains the
x and y components of gravity and the gyrometric
attitude estimate. Using equations (6,7,12,13), the
observation equation can be written :
[g l r-gsins' "1
gy[ /gcos~sinO/
Vg| =| ¥+dv.t / + wk <=>Zk=h(Xk'k)+wk
@gJk L@+d@.t Jk
where w k is the observation noise vector presumed
uncorrelated, zero-mean, and Gaussian with variance
Rk.
A
Given the inital state estimate XO/0 with its
associated variance P0/0, the attitude is calculated at
step k+l using the following formulas :
~k+I/k = ~klk
A A A Fig. 1. Roll estimation
Xk+I/k÷l = Xk+I/k + Kk+l.(Zk+l - Zk+l)
Pk+l/k = Pk/k+Qk
T T _ interval is 0.15s. With a 64.8 cm tread, the
Kk+l = Pk+I/kHl~+1(Hk+lPk+l/kHk+l+Rk+1) 1
Pk÷I/k+l = (I - Kk+iHk+l)Pk+i/k
285
with Hk+l i)h(Xk+1)I
= ~Xk+ 1 JXk+1=~k+l/k
6. EXPERIMENTAL RESULTS
The system described above has been implemented
and tested on the two-differential-wheels mobile
platform VEGA available in the laboratory. All
software (robot controland inertial measurement
processing) is programmed in C and executed on a
T805 transputer network. The real-time
implementation allows the attitude estimation to be
updated every I0 ms. Three gyrometers and a three-
axis accelerometer are fixed on a precision-made
support to ensure a good alignment of the
sensitivity axes. This support is attached to the
robot.
In the experiment, the robot moves in a swaight line
for 5 meters, ttm~ 180° with zero radius, and moves
again in a straight line for 5 meters. This motion is
executed on flat ground, except during a part of the
t-,Ts, the right wheel climbs onto a plank which is
2 cm thick and 150 cm long, using a small inclined
plane. At time t-12s,/he wheel gets back onto flat
ground. After the half turn, in the second linear part,
the robot's left wheel climbs onto the plank (t-32s
and t~37s). All inertial measurements are filtered by
a second-order Butterworth with a 10Hz cut
frequency.
![ "'!". i
-I
-2 "'"'; , ..... by lymmems
-3
,o ,5 20 2", ~ ;~ ,~ ,'s ~0
tecoa~
Figure 1 shows the estimations of the roll angle
throughout the motion. The graphic sampling
theoretical roll is -1.76° during the fwst linear phase
and +1.76 ° during the second. It can be seenthatthe
accelerometric estimates have a good mean value
286
(0 °) when the robot moves on fiat ground, and
around +1.5 ° when one wheel of the robot is on the
plank), but that instantaneous values can be
completely wrong because of shocks or vibrations
(example: ,=-4.5° at t=8s). It can also be verified that
the gyrometric estimates give a precise short-term
evolution of roll. For example, between t=7s and
t=12s, the variation of roll is very well represented.
On the other hand, the values of this angle get
worse with time, because of the drift due to the
gyros and the integration process. The roll
estimation obtained by extended Kalman filtering
gives a precise evolution and value of the angle at
the same time. The variation of roll observed
between t-22s and t,,26s, corresponds to the rotating
phase. Before the rotation, the gyrometric roll
estimation is about -2 ° . Since this angle is
calculated by integration, after a +180 ° rotation, it
becomes +2 ° . Everything happens as if the robot
had turned on a 2 ° inclined plane. This is why a +2 °
variation can be observed on the pitch estimation
(Fig. 2).
' ~i ~ i !i
ii
2
I Doctorate Thesis, UPS, Toulouse.
.8
-I
-2 :
-3
; - -
-4
0 J I0 15 20 25 30
seconds
Fig. 2. Pitch estimation
Figure 2 shows the estimations of the pitch angle
throughout the motion. The same comments are
valid concerning the accelerometric and gyrometric
estimations. Variations in the pitch can be observed
when the robot climbs on the plank (negative pitch
around t=8s and t,~34s) or when it climbs down from
it (positive pitch around t,,14s and t,-39s).
7. CONCLUSION
This paper describes an attitude estimation system
based on inertial measurements. Even with low-cost
inertial sensors, this system appears to be very
sensitive and accurate when used on an indoor
mobile roboL The method takes advantage of both
types of sensors by fusing the attitude estimations
they provide via a Kalman filter. Real-time
J. Vaganay and M.J. Aldon
implementation of the system on the experimental
robot VEGA demonstrated that it works well, but
the variation domain of the attitude being very
restricted, it would be interesting to test it on other
types of vehicles. To employ this system on other
types of vehicles (all-terrain, under-water), an
estimation of the absolute acceleration of the vehicle
should be available to extract the gravity
components from the accelerometric measurements.
The sensors' full scales and resolutions should also
be adapted to the vehicle's dynamics.
8. REFERENCES
Bhanu, B., B. Roberts and J. Ming (1990). Inertial
navigation sensor integrated motion analysis for
obstacle detection. In: IEEE Int. Conf. on
Robotics and Automation, pp. 954-959,
Cincinnati, Ohio.
Edwards, A. (1971). The state of strapdown inertial
guidance and navigation. In: Navigation, Journal
of the Institute of Navigation, vol. 18, n°4.
i Fen'and, A. (1991). Conception et mise en oeuvre
d'un syst~me de capteurs proprioceptifs destin6
la loealisation relative des robots mobiles.
Frappier, G. (1990). Syst~mes inertiels de
navigation pour robots mobiles. In: Sdminaires
Robotique Mobile', Paris.
Grubin, C. (1973). Attitude determination for a
b y ftt.~ion strapdown inertial system using the Euler
35 40 45 50 axis/angle and quatemion parameters. In: Aj.A.A.
paper, n°73-900.
Le Corre, J.F., and G. Garcia. (1992). Real time
determination of the location and speed of mobile
robots running on non-planar surfaces. In: IEEE
nt. Conf. on Robotics and Automation, pp. 2594-
2599, Nice, France.
Radix, J.C. (1980). Syst~mes inertiels /~
composants li6s 'strap-down'. In: Sup'Adro, Ecole
Nationale Sup6rieure de l'A6ronautique et de
rEspace, ed. Cepadues.
Turpin, D.R. (1986). Inertial guidance : is it a
viable guidance system for AGVS ?. In: #th Int.
Conf. on AGVS, pp. 301-320.
Vaganay, J., M. J. Aldon and A. Fournier (1993a).
Mobile robot localization by fusing odometric and
inertial measurements. In: IEEE Int. Conf. on
Robotics and Automation, Vol. I., pp. 277-282,
Adanta.
 AttitudeEstimation
Vaganay, J. (1993b). Conception d'un syst~me
multisensoriel de localisation dynamique 3D pour
robot mobile. Doctorate Thesis, UM II,
Montpellier.
Vi6ville, T., and O. Faugeras (1989a). Cooperation
of the inertial and visual systems. In: NATO
ARW Traditional and Non Traditional Robotic
Sensors.
287
~16viile, T., and O. Faugeras (1989b). Computation
of inertial information on a robot. In: 5th Int.
Sympo~um of Robotics Research, Tokyo, Japan.
Wang, J., and W. J. Wilson (1992). 3D relative
position and orientation estimation using Kalman
filter for robot control. In: IEEE Int. Conf. on
Robotics and Automation, pp. 2638-2645, Nice,
France.