1346 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO.

7, JULY 2006

Quaternion-Based Extended Kalman Filter for

Determining Orientation by Inertial and Magnetic
Sensing
Angelo M. Sabatini, Senior Member, IEEE

Abstract—In this paper, a quaternion based extended Kalman noise. Hence, the estimation errors tend to grow unbounded.
filter (EKF) is developed for determining the orientation of a rigid Another drawback is that inertial sensors are not well-suited
body from the outputs of a sensor which is configured as the inte- for determining absolute location: indeed, the integration has
gration of a tri-axis gyro and an aiding system mechanized using a
tri-axis accelerometer and a tri-axis magnetometer. The suggested to be started from initial conditions, which inertial sensors
applications are for studies in the field of human movement. In the cannot help establishing (position, velocity), or disambiguating
proposed EKF, the quaternion associated with the body rotation completely (orientation). Other technologies, such as earth’s
is included in the state vector together with the bias of the aiding magnetic field sensing, can help either mitigate the integration
system sensors. Moreover, in addition to the in-line procedure of errors [3] or specify the absolute orientation [4]. In principle,
sensor bias compensation, the measurement noise covariance ma-
trix is adapted, to guard against the effects which body motion earth’s magnetic field sensing is externally referenced, although
and temporary magnetic disturbance may have on the reliability of it can be used in practice as though it is internally referenced.
measurements of gravity and earth’s magnetic field, respectively. This is because the earth’s magnetic field is virtually available
By computer simulations and experimental validation with human anywhere, and environmental modifications are not necessary
hand orientation motion signals, improvements in the accuracy to measure it.
of orientation estimates are demonstrated for the proposed EKF,
as compared with filter implementations where either the in-line Research is currently being carried out in many laborato-
calibration procedure, the adaptive mechanism for weighting the ries for tracking human body motion with the use of gyros, ac-
measurements of the aiding system sensors, or both are not imple- celerometers and magnetometers. Beside the research efforts for
mented. high-grade, yet inexpensive sensors, advanced signal processing
Index Terms—Earth’s magnetic field sensing, extended Kalman methods are intensely investigated to improve the performance
filter, human motion tracking, inertial sensing, quaternion. of existing sensing hardware [5]. The basic idea behind com-
plementary filtering is that orientation drift errors resulting from
gyro output errors can be bounded by aiding the gyros with addi-
tional sensors, the information from which allows correcting the
I. INTRODUCTION gyro orientation solution. The tradition of using linear Kalman
filters (KFs) and their extended (EKF) version for nonlinear

T HE DETERMINATION of position and orientation of

moving objects is involved in several fields: among
them, navigation of man-made vehicles and, of interest here,
models is well established when complementary filters are de-
veloped to blend the outputs from gyros and the sensors of an
aiding system. This is also the case when applications that re-
human motion tracking and analysis. Most motion sensing quire inertial sensing of human motion are considered. In this
technologies in use need an external source (optical, magnetic, paper, an EKF is developed for estimating the orientation of
and acoustic) to determine location information. Usually, the a rigid body using sensor devices which are composed of one
source is transmitted over limited distances; interference and gyro, one accelerometer and one magnetometer, each of them,
shadowing are additional problems associated with current endowed with three perpendicular sensitive axes—henceforth,
technologies. Inertial sensors make it possible to determine this configuration is referred to as a multi-sensor orientation de-
location by measuring physical quantities (acceleration, angular termination system (MODS).
velocity) which are directly related to the motion of the body An inertial measurement unit (IMU) composed of one tri-
part where they are placed. Being internally referenced and axis accelerometer is developed in [6] for measuring inclina-
immune to interference and shadowing, inertial sensing can tion during dynamic tasks (gravimetric tilt sensing). Usually,
track body motion, in principle, without restrictions [1], [2]. accelerometers are sensitive to both gravity and bodily accel-
In inertial systems, the main problem is that location is erations, which makes their discrimination difficult, especially
computed by time-integrating the signals from gyros and as the speed of movement increases. The developed KF is de-
accelerometers, including any superimposed sensor drift and signed to separate these components, including accelerometer
bias vector, in accordance with an analytical model based on
Manuscript received March 9, 2005; revised January 5, 2006. This work was realistic assumptions about the dynamics of human movement.
supported in part by the Fondazione Cassa di Risparmio di Pisa under the Kid- However, since the accelerometer signals are not affected by a
rollbot Project, and in part by the Italian Ministry of University and Research. rotation around the vertical, information from additional sensors
The author is with the ARTS Lab, Scuola Superiore Sant’Anna, Piazza Martiri
della Libertà, Pisa 56127, Italy (e-mail: A.Sabatini@arts.sssup.it). is necessary to compensate for the heading drift in the proposed
Digital Object Identifier 10.1109/TBME.2006.875664 in-line calibration procedure.
0018-9294/$20.00 © 2006 IEEE

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
SABATINI: DETERMINING ORIENTATION BY INERTIAL AND MAGNETIC SENSING 1347

The EKF developed in [7] allows estimating the orientation of forts to make an EKF adaptive in applications that require in-
a body segment using a MODS; these authors represent the ori- ertial sensing of human motion [11], [15], [16]. Because of the
entation with quaternions. Albeit they may suffer from problems inherent inability of accelerometers to provide accurate heading
of interpretation in terms of meaningfully clinical or anatom- estimates, magnetic sensing is argued to effectively complement
ical angles, quaternions are interesting mathematical entities, inertial sensing for applications in the field of human body mo-
since they require less computing time and avoid the singularity tion tracking. The main contribution of this paper is the design
problems inherent in using other orientation descriptors, e.g., and testing of an orientation filter which allows the in-line cal-
Euler angles [8], [9]. In [7], the quaternion measurement is ob- ibration of aiding sensors, both accelerometers and magnetic
tained by vector matching earth’s gravitational and magnetic sensors—feature which is lacking in most orientation filters we
fields, resolved by the aiding system in the body frame, with are aware of. By computer simulations and experimental val-
their known representation in the absolute reference frame [10]. idation with human hand orientation motion signals, improve-
The gyro information is integrated to track high-frequency ori- ments in the accuracy of orientation estimates are demonstrated,
entation components, while quaternion measurements are used as compared with filter designs, where the in-line calibration
to track low-frequency components and stabilize the gyro be- procedures, the adaptive mechanism for weighting the aiding
havior. The linearization process ensuing from vector matching sensor measurements, or both are not implemented.
by a reduced-order Gauss-Newton optimizer and the fast and ro-
bust convergence of the latter allow implementing an EKF with II. METHOD
moderate computational overhead and good stability, although
these features accrue to the filter design provided that quater- A. Orientation Representation and Determination
nion measurements are not biased. The orientation of a rigid body in space is determined when
The adaptive EKF developed in [11] is embedded in the axis orientation of a coordinate frame attached to the body
the MODS marketed by InterSense Inc. [12]. Gravimetric (the body frame ) is specified with respect to an absolute co-
tilt sensing and earth’s magnetic field sensing are used for ordinate system, usually named the navigation frame .
alignment and gyro stabilization; the gyro outputs are then The transformation between the representations, relative to
integrated to yield the orientation changes between successive and , of a 3 1 column-vector , whose components are
measurements. To guard against the effects of body motion, generally functions of time , is expressed as
the acceleration magnitude is further tested for significant de-
viations from gravity [13]. If these deviations are not detected, (1)
gravimetric tilt sensing is used to correct pitch and roll drift;
otherwise, the accelerometer measurements receive a lesser Henceforth, the argument will be omitted for the sake of
weighting [14]. The level of yaw compensation is specified simplicity. The direction cosine matrix (DCM) for the transfor-
by hand-crafting the weight of magnetic measurements during mation from to is given in terms of the orientation quater-
filter initialization. To guard against the effects of temporary nion , as shown in (2) at the bottom of the page,
magnetic disturbances, a screening technique similar to that where is the vector part and is the scalar
described above for acceleration measurements may be based part of the quaternion [8].
on testing the sensed magnetic field strength for significant The rigid body angular motion obeys the vector differential
deviations from the local earth’s magnetic field strength [15], equation
[16]. In-line calibration procedures to compensate for the bias
originated from exposing the MODS to magnetically disturbed (3)
environments are not fully pursued in [11]–[15].
In this paper, we develop a quaternion based EKF with the where
following features: a state augmentation technique is applied
(4)
in the process model and a specific measurement model is for-
mulated, in the attempt to capture the bias vectors of both ac-
celerometer and magnetometer (in-line calibration)—this part is the angular velocity of relative to ,
of the work is related to previously reported in-line calibration resolved in . is a 4 4 skew symmetric matrix and the
efforts for motion tracking and analysis in the medical field [7], operator
[16], robotics [17], and virtual environment systems [18]; the
measurement noise covariance matrix is adapted at run-time, to (5)
guard against the effects of body motion and temporary mag-
netic disturbances on the reliability of an aiding system sensor
measurements—this part of the work is related to previous ef- represents the standard vector cross-product [7].

(2)

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
1348 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006

The discrete-time model corresponding to (3) is keeping the accelerometers from moving as described in [25],
are more difficult to implement and may require specific ma-
neuvers to work properly [6], [26]. Zero attitude or velocity
(6) updates may rely on the stereotypical feature of human move-
ments to alternate movements of limb segments, the rotation
where is the system sampling interval. The quaternion is de- of which can be measured using gyros, with rest periods, in
termined at time instants , starting from initial conditions preparation for the next movement of these segments. It is
that are assumed to be known or measurable during alignment. indeed during these rest periods that the aiding sensors can be
The validity of (6) is subject to the assumption that the angular used to compute the initial conditions from which the gyro
velocity measured at time instants is constant in the in- integration will start [23], [24]. It would also be important to
terval . Equation (2) is used to update the DCM detect these rest periods in order to dynamically reset the gyro
expression, once that the solution of (6) has progressed in time. outputs. Henceforth, a zero attitude update, similar to the one
implemented in [24], is performed just at the beginning of each
B. Sensor Model simulated and experimental run.
Usually, the magnetometer error is described by taking into
The gyro, the accelerometer and the magnetometer are sensor
account the effects of so-called hard irons and soft irons [3].
triplets with perpendicular sensitivity axes. Their output in re-
Hard iron errors are those due to unwanted fields, either sta-
sponse to the angular velocity , total acceleration (gravity
tionary or time-varying, that are generated by ferromagnetic ma-
and acceleration ), earth’s magnetic field are expressed,
terials nearby the magnetometer. The effect of their superpo-
respectively, by
sition to the sensed earth’s magnetic field is to bias the mag-
netometer output [16]. Soft iron errors are due to the fields
which, in response to externally applied fields, are generated by
(7) some materials, which include clothes and accessories worn by
the person carrying the magnetometer [15]. In response to the
earth’s magnetic field, soft irons generate a magnetic field that
where , , and are the scale factor matrices (ideally, will be superimposed on the magnetometer output. Since the
they are equal to the 3 3 identity matrix ); , , and orientation of the earth’s magnetic field vector relative to the
are the bias vectors (ideally, they are null); , , and are soft irons change during unrestrained movements, the resulting
assumed uncorrelated white Gaussian measurement noise, with effect on the magnetic field is highly complex, and would not be
null mean and covariance matrix , , and easily modeled. However, because of the strong dependence of
. Equation (7) is a simplified model which does not parasitic magnetic fields on the distance from the source, the
account for cross-axis sensitivity, cross-coupling, and misalign- impact of either hard or soft irons, e.g., mobile phones, can
ment [16], [19]–[21]. be minimized taking care during sensor placement. The auto-
The bias and scale factor of inertial sensors are functions of matic procedure of in-line calibration is intended to detect and
environmental conditions, in particular the ambient tempera- correct situations when the deviation in the compass is pro-
ture; this is especially true for gyros, while the temperature co- duced by external magnetic interference which act in the sense
efficients of accelerometers are of relatively lower quantitative to bias the magnetometer output: while hard irons tend to act
relevance and the temperature coefficients of magnetometers as static bias vectors, the effects of either static or time-varying
have insignificant effects on their behavior across the temper- external magnetic interferences are modeled as trajectory-de-
ature variations that they may encounter in practice [17], [22]. pendent time-varying bias vectors. Finally, since the objective
Moreover, scale factor drifts of inertial sensors are known to af- here is to get a reference orientation from the MODS within a
fect the accuracy of the measurement process to a much lesser relatively small tracking area, the errors due to the difference be-
extent than the bias drifts of these sensors [1]. In the case of tween the compass reading and the geographic north direction
gyros, temperature variations are considered the most important on earth, i.e., variation and declination [3], are not considered
element to determine the bias drift. The bias drift is significant in the sensor model (7c).
especially after power is applied to gyros, as a result of device
self-heating [22]. Provided that gyros are allowed warm-up and C. Filter Design
thermal stabilization for few minutes, then their biases tend to The state vector is composed of the rotation quaternion, aug-
change quite slowly with time. mented by the tri-axis accelerometer and magnetometer bias
Henceforth, we assume that scale factor and bias errors vectors, the components of which are modeled as random walk.
of gyros in (7a) are constant; in practice, bias errors can be The state transition vector equation is
calibrated and compensated effectively by bias capture pro-
cedures based on so-called “zero attitude updates,” which
require keeping the gyros from rotating [23], [24]. As for the
accelerometer, we assume that scale factor and bias error in
(7b) are constant; in practice, bias errors can be calibrated and
compensated effectively, although the bias capture procedures (8)
based on so-called “zero velocity updates”, which require

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
SABATINI: DETERMINING ORIENTATION BY INERTIAL AND MAGNETIC SENSING 1349

where is the 3 3 null matrix and In magnetically perturbed environments, the deviation of the
sensed magnetic field magnitude from the local earth’s mag-
(9) netic field magnitude can be so large, and the dip angle, namely
the angle formed by the earth’s magnetic field relative to the
and are zero-mean white noise processes, with co- horizontal
variance matrix and , respec-
tively. The part of the sensor model in (8), (9) describing the (14)
quaternion time-evolution is a first-order approximation in
and of the exact process (6) [27]. Underlying this approxi-
mation is the consideration that the true angular velocity vector can deviate so much from the local value , to raise a serious
to be used in (6) is not known in practice, but it is rather mea- concern about the reliability of the magnetic information [14];
sured. The gyro measurement noise vector is assumed small the term in (14) is the predicted gravity (The
enough that a first order approximation of the “noisy” transition superscript—stands for “a priori estimate at time , before
matrix is possible, yielding (9). Because of the assumption that the current measurement is used in the computation of
, and are not correlated with one another, the the a posteriori estimate” [28]). The following validation test
process noise covariance matrix will have the following ex- is implemented:
pression:

otherwise
(15)
(10) Rather than dealing with acceleration and magnetic distur-
bances as time-varying components of the bias vectors, the
The measurement model is constructed by stacking the ac- proposed validation tests aim at precluding the measurements
celerometer and magnetometer measurement vectors from influencing the filter behavior, when detected disturbances
are characterized by high magnitude or duration. In this regard,
the tests implement a sort of gating technique, which can be
used alone, as in the present filter design, or in combination
eventually with any of the gating techniques reported in the
literature on Kalman filters [28].
Because of the nonlinear nature of (11), the EKF approach re-
(11) quires that a first-order Taylor-Mac Laurin expansion is carried
out around the current state estimate by computing the Jacobian
The covariance matrix of the measurement model is matrix:

(12) (16)

Underlying (12) is the assumption that the accelerometer and

magnetometer measurement noise and are un- As an example, the element is given by
correlated zero-mean white noise processes, the covariance ma-
trices of which are and , re-
spectively. (17)
Before the current measurement is incorporated in the
filtering process to drive the state vector update, a mechanism As it is evident from inspection of (8)–(11), the statistical
of adaptation of the measurement noise covariance matrix is im- models which are considered in this EKF development are
plemented. The part of (11) which describes the accelerometer state-dependent. However, the expression of the true state is
measurement model differs from the sensor model (7b) in that not known; it is common practice to handle this dependence by
the acceleration component related to body motion cannot be substituting the best estimate of the state available, namely the
present. In our approach, the measured acceleration magnitude a priori state estimate, instead of the true state. Another subtle
is tested in advance for the absence of significant deviations detail is in the use of the quaternion as part of the state vector.
from gravity [11], [13], [14]; if persistent for some specified Since, to represent a valid rotation, a quaternion must have
time interval, this absence is considered the sign that the body is unit-norm, the four quaternion components are interdependent.
at rest. If not so, the observation variance is set to extremely Although the state transition matrix is orthogonal,
high values, so as to force the filter to rely on the magnetic in- the unit-norm property of the a posteriori quaternion must be
formation for carrying the state vector update [14] preserved by a normalization step, which is common practice
to perform by dividing it by its Euclidean norm [27].
otherwise For the sake of reader’s convenience, the EKF equations are
(13) summarized below [28].

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
1350 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006

TABLE I
VALUES OF BIAS, SENSITIVITY, AND MEASUREMENT NOISE STANDARD
DEVIATION USED TO SIMULATE THE SENSOR BEHAVIOR. THESE VALUES WERE
TYPICAL OF THE CALIBRATION RESULTS OBTAINED WHEN AN INERTIACUBE2
ORIENTATION SENSOR WAS SUBMITTED TO STANDARD CALIBRATION
PROCEDURES IN OUR LAB

was acquired from an InertiaCube2 orientation sensor (Inter-

Sense Inc., Burlington, MA), interfaced to a PC via a serial
communication interface (RS232 communication protocol,
baud rate: 115 200 bps), [12].
Fig. 1. EKF structure. The block “project ahead” computes the a priori state To construct the HAND set, the tracking device was snugly
estimate and error covariance matrix. The DCM estimation is also carried within
this block, and used to implement the measurement validation test (13), see (14), strapped to the subject’s wrist using a Velcro strap. In the initial
and to compute the Jacobian matrix, see (16), (17), which is also needed for posture, the subject was seated at a distance of about 40 cm from
updating the expression of the Kalman gain. The block “update estimate” com- a table, the palm of the instrumented hand lying on the thigh; the
putes the a posteriori state estimate and error covariance matrix. The measure-
ment validation tests implement (13)–(15), in preparation for the adaptation of MODS sensitive axes were aligned with the body axes: sagittal,
the measurement noise covariance matrix, see (12). The iterative nature of the frontal, and longitudinal. The subject was asked to move the
discrete-time EKF allows exploiting the statistics available at the k -th time-step arm at freely selected speed, as though he had to laterally reach
to start the computations at the next time-step, when a new set of measurements
from the MODS sensors becomes available. a target area on the tabletop with the wrist rotated about 90 ,
then to move the hand to the mouth in the attempt to mimic
drinking, before back-tracing the hand path to the initial posture.
• Compute the a priori state estimate The recorded movement lasted about 18 s. The time functions of
the quaternion components were delivered by the InertiaCube2
at a sampling rate of 100 Hz. Data were off-line processed using
Matlab v. 6.0.
The truth reference data-set for algorithm testing was ob-
• Compute the a priori error covariance matrix tained from filtering the quaternion components with a second-
order forward-backward low-pass Butterworth filter (cut-off fre-
quency: 5 Hz).
Standard conversion formulas allowed constructing the orien-
tation vector ; its time-derivative was obtained by applying
• Compute the Kalman gain the central difference technique to the orientation vector sam-
ples. and were used to estimate the angular velocity vector
that generated that specified orientation as stated in [29]. The
sensed gravitational and magnetic fields were computed from
• Compute the a posteriori state estimate resolving gravitational and geomagnetic fields—
and , respectively—into the
body frame via the estimated DCM.
The sensor orientation was not referred to the true magnetic
or geographic north; instead yaw was estimated relative to the
• Compute the a posteriori error covariance matrix initial hand posture. Before moving, the hand was at rest for few
seconds, so as to estimate the earth’s magnetic field in condi-
tions of null yaw. Random Gaussian noise was injected into the
sensor time functions to simulate the effect of specified amounts
of measurement noise.
The flow-chart of computations performed by the proposed EKF
For the purpose of algorithm testing, the time interval [0, 18]
is summarized in Fig. 1.
s was divided in four subintervals: the subinterval ,
where perfectly calibrated sensors were simulated; in the other
D. Computer Simulations
subintervals, namely , , and
One data-set of human hand orientation motion was used , sensor bias and scale factor were set to
in our study as a representative example of human motion dy- values different from initial calibration setting, Table I.
namics which may be frequently encountered during daily-life The conditions within the subinterval were further mod-
functional activities. The data-set, called HAND set henceforth, ified by introducing motion and magnetic disturbances: the

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
SABATINI: DETERMINING ORIENTATION BY INERTIAL AND MAGNETIC SENSING 1351

recorded using a nine-camera Vicon optical motion capturing

system with a sampling rate of 100 Hz. The system measured
the position of three reflective markers (diameter: 14 mm) ar-
ranged to form an equilateral triangle on top of the plate (tri-
angle side: 20 cm); an additional marker was placed on top of the
InertiaCube2 (not used here). This configuration allowed con-
structing a marker frame, the accuracy of which was obtained
by analyzing the relative motion of two markers. The RMS of
the Vicon orientation error was assumed to be in the same order
as the RMS distance variation divided by the distance between
two markers.
The basic idea behind orientation determination using the
Vicon system was the fact that three noncollinear points in
space define a plane, the orientation of which could be easily
computed relative to the predefined reference frame. The cor-
responding roll, pitch, and yaw time functions computed from
Fig. 2. The displacement of the body frame origin which occurs in the local
level plane is responsible for a linear acceleration component the magnitude of
the position data provided by Vicon were considered the truth
ms
which is up to 2 = (solid line). The dipole-like magnetometer disturbance is reference for the purpose of error estimation. The orientation
oriented in the direction of the local earth’s magnetic field, as though a metallic of the sensor frame relative to the marker frame was found by
mass is moving nearby the magnetometer (dashed line).
holding the plate still for few seconds at the beginning of the
trial, so as to compute the calibration quaternion which brought
the sensor frame into coincidence with the marker frame. Gyro
sensor was linearly displaced in a plane perpendicular to gravity bias capture was done during heading reset. The quaternions
at a time when it was also exposed to a time-varying magnetic produced by Method A—the context winner of the simulation
field, oriented in the direction of the earth’s field, Fig. 2. runs—and Method E were converted in Euler angles using
Four EKF implementations were studied, henceforth called standard conversion formulas. The time functions of the roll,
Method A, B, C, and D. Method A and Method B incorporated pitch and yaw estimation errors were off-line constructed by
the in-line calibration of the aiding system sensors; Method computing the difference between the roll, pitch, and yaw esti-
A adapted the measurement noise covariance matrix , while mates produced by each tested filtering algorithm and the truth
Method B did not adapt it. Method C and Method D did not in- reference. Hence, beside the value, RMSEs of roll,
corporate the in-line calibration of the aiding system sensors; pitch and yaw estimates were computed using three different
Method C adapted , while Method D did not adapt it. Filter approaches: Method A, Method E, and the native InertiaCube2
initialization was performed by gravimetric tilt and earth’s mag- filtering algorithm (default parameter initialization), henceforth
netic field sensing at first contact. An additional method, called named Method F.
Method E, was based on quaternion time-propagation by inte-
grating (6) from initial conditions which were externally pro- III. RESULTS
vided to the algorithm. The five methods were tested at three
The quaternion time functions of the HAND data-set are
different sampling rates: 100 Hz, 50 Hz, and 25 Hz. Ten Monte
shown in Fig. 3.
Carlo trials were conducted for each condition (method and
To process the HAND data-set noisy sensor data, constructed
sampling rate). In the simulations concerning the performance
as stated in Section II-D, the filter parameters are those indicated
assessment of Method A and Method C, ,
in Table II.
, , and .
The statistics of the orientation are reported in
The performance metrics were based on computing
Tables III and IV. The results in Table IV are obtained by
, where and were truth-reference and estimated
setting values of the random walk model parameters which
quaternions, respectively. The quaternion represented the
achieve the best performance after extensive testing when the
rotation which brought the estimated body frame onto . The
simulation is carried out in the absence of motion and magnetic
orientation error was obtained from the scalar component of
disturbances (this optimal filter parameter initialization is
. The performance metrics were
shown in Table II as row A1–A2).
given by the root-mean-square error of , com-
Figs. 5–7 show the time functions of the Euler angles as they
puted over the four subintervals.
are measured from the Vicon system during the experimental
validation trial, together with the corresponding error time func-
E. Experimental Validation
tions produced by Method A. The RMS of the Vicon orientation
The experimental validation was carried out in the course of error is almost the same for roll, pitch and yaw, and turns out
an experiment which consisted of moving a plastic plate, raised to have, in this experiment, a standard deviation (SD) of about
from ground, by hand, at freely selected speed for 120 s within a 0.4 . Typical errors made by Method A in estimating the quater-
measurement space of about . The InertiaCube2 nion components are sketched in Fig. 4 (motion and magnetic
was placed on top of the plate and fastened using double-side disturbances not inserted in the subinterval ; optimal filter pa-
adhesive tape. As a truth reference, the plate orientation was rameter initialization).

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
1352 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006

accelerometers from moving. The in-line calibration procedure

described in [26] is an interesting variant of zero velocity up-
dates, where a tri-axis accelerometer is exposed to the influence
of gravity in several static orientations in order to estimate ac-
celerometer bias. The in-line procedure of bias compensation is
embedded in the improved EKF to perform this compensation
automatically.
It should be pointed out that an element of complication in the
performance analysis of a filtering algorithm for inertial navi-
gation is the effect that the sensed trajectory may have on its
behavior: certain errors of the inertial sensors are in fact trajec-
tory-dependent, e.g., scale factor errors [22]. Rather than on a
wide variety of trajectories in the three-dimensional space, the
methods described in this paper are exercised on a synthetic tra-
jectory [21]; because of the way it is constructed, the synthetic
trajectory sketched in Fig. 3 in quaternion space is intended to
replicate important features of human hand orientation motion
Fig. 3. The four time functions related to the unit-norm quaternion of rotation dynamics.
from the HAND data-set. The quaternion components are dimensionless. First
component: solid line; second component: dotted line; third component: dashed
The filter parameter initialization reported in Table II is found
line; fourth component: dash-dot line. to work well after running an extensive number of tests in the
presence of the simulated disturbances. Of course, for different
trajectories and time-varying disturbances, different sets of filter
TABLE II
FILTER PARAMETER INITIALIZATION parameters would be probably better. In general, it should be
outlined the important role played by the process noise covari-
ance matrix in tailoring the EKF responsiveness. A good rule
of thumb is to increase the process noise in the effort to com-
pensate for the disturbing effects of inaccurate modeling and
to improve the tracking ability of the filtering algorithm [28].
Of course, this may be at the expense of the estimation accu-
racy when “calmness is restored.” In our implementation the
quite high value of the gyro measurement SD is entered in the
process noise covariance matrix to counteract the effects of gyro
imperfect calibration. The SD assumed for the random walk
The filter parameter initialization for Method A is included in model which describes the time-evolution of the accelerometer
Table II as row A (experimental). The accuracy of the methods and magnetometer bias vectors must also be sufficiently large:
tested during the experimental validation trial is reported in a careful look at the sensor model (7) shows that the effect of
Table V. scale factor errors and constant null-shift errors is to produce
measurement errors which look like trajectory-dependent tem-
porary bias errors. If the in-line compensation procedure is dis-
IV. DISCUSSION AND CONCLUSION
abled, the natural thing to do is to increase the SDs in the mea-
The simulation trials are assumed to start with perfectly cali- surement noise covariance matrix, see the filter parameter ini-
brated sensors, namely unit scale factor and null bias. The values tialization in Table II used for Method C and D; the latter means
of the sensor model parameters in Table I, which are enforced that the a priori state estimates, which are driven by gyro mea-
from time on in the simulations, are typical of the range surements, as indicated in (8) and (9), are assumed to be more
of values obtained by calibrating the InertiaCube2 in different reliable than the measurements from the sensors of the aiding
parts of a laboratory room using the procedures described in [19] system.
and [20]. The results reported in Table III show that it is the com-
Zero attitude and velocity updates are not implemented in the bination of in-line bias compensation and adaptation of the
simulation runs to perform gyro and accelerometer bias capture. measurement noise covariance matrix to give the best perfor-
Since gyro bias is defined as the output produced by gyros at mance. The measurement validation tests detect the presence of
rest, bias can be estimated anytime the gyro is detected to be at a time-varying disturbance with some delay, dependent on both
rest: it should be pointed out that it is a stereotypical feature of the disturbance rise time and the threshold setting. In spite that
most human motions to exhibit rest periods of some duration, disturbance spectral components may generally have some-
at almost regular time intervals, e.g., walking [24], or sporad- what higher-frequency content than human motion quaternion
ically, e.g., as during hands or head movements, which makes components, the need to prevent frequent false alarms forces
gyro calibration and compensation by zero attitude update quite to choose threshold values in (13) and (15) which cannot be
effective and simple. As for the accelerometer, bias capture pro- too low. Before time-varying disturbances are detected, the
cedures use zero velocity updates, which require keeping the linearization procedure within the EKF is, thus, susceptible to

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
SABATINI: DETERMINING ORIENTATION BY INERTIAL AND MAGNETIC SENSING 1353

TABLE III
6
ORIENTATION ESTIMATE RMSE ( )—MEAN SD—PRODUCED BY THE DIFFERENT FILTERING METHODS IN THE COURSE OF THE MONTE CARLO PERFORMANCE
ANALYSIS. THE FILTER PARAMETER INITIALIZATION IS THAT REPORTED IN Table II. THE ACCELEROMETER AND MAGNETOMETER DISTURBANCES ARE
INTRODUCED WITHIN THE INTERVAL T3, SEE TEXT

TABLE IV
ORIENTATION ESTIMATE RMSE ( )—MEAN 6 SD—PRODUCED BY METHOD A, EITHER IN THE ABSENCE OF SIMULATED MOTION AND MAGNETIC
DISTURBANCES (A1) OR IN THEIR PRESENCE (A2)

TABLE V
QUATERNION ORIENTATION, ROLL, PITCH, AND YAW ESTIMATION RMSE ( ),
PRODUCED BY METHOD A, METHOD E, AND METHOD F (EXPERIMENTAL
VALIDATION)

induce undesirable effects such as biases in the state estimation

errors, which may take a quite long time to recover depending
on how long the disturbances themselves have affected the
filter behavior; the in-line compensation procedure proves to be
effective in driving this recovering process. Finally, provided
that the time-varying disturbances are detected, the affected
measurements from the aiding system are not assimilated in
the filtering process, and the orientation sensor relies entirely Fig. 4. State errors from the EKF for the four quaternion components (HAND
on the gyro outputs until the next measurement update. An data-set sampled at 100 Hz—method A). The solid lines are the errors and the
dotted lines represent the three SD bounds. (a)–(d) First, second, third, fourth
interesting approach which would deserve further investigation quaternion component, respectively.
is the introduction of a rule to smoothly shift the measurement
noise SDs from the initial value to infinity, according to the
perceived reliability of the sensed information. For the purpose An additional set of simulations, the results of which are
of this paper, however, we prefer to deflate the adaptation of shown in Table IV, is useful to assess the sensitivity of method
the measurement noise covariance matrix toward an implemen- A to the choice of the random walk model parameters; not sur-
tation of gating. prisingly, it is easy to find other filter parameter initializations,

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
1354 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006

Fig. 5. Time function of the roll angle measured from the Vicon system (truth Fig. 7. Time function of the yaw angle measured from the Vicon system (truth
reference) and the roll angle estimation error, computed as the difference be- reference) and the yaw angle estimation error, computed as the difference be-
tween the roll angle estimate produced by the EKF (Method A) and the truth tween the yaw angle estimate produced by the EKF (Method A) and the truth
reference. Dashed line: truth reference, solid line: estimation error. reference. Dashed line: truth reference, solid line: estimation error.

timation task, in the presence of serious measurement errors as

investigated in this paper, is extended from orientation compo-
nents to position components, which require double integration
of gravity-compensated acceleration components, or when the
estimation task is connected with potentially more challenging
human motion dynamics, such as walking. In our recent work on
foot inertial sensing for estimating temporo-spatial parameters
of gait [21], [24], it is shown that limitations in the sampling
frequency can be tolerated down to 25–50 Hz, provided that
strap-down integrals can be reliably reset once in a gait cycle
[21]; we also demonstrate that the effect of low-frequency er-
rors, e.g., bias and scale factor drifts, is more relevant than the ef-
fect of high-frequency errors, e.g., sensor jolting, impact spikes
and so forth, which can be accommodated by careful sensor
placement and signal filtering [24]. Using relatively slow hand
motion signals, as done in the present paper, presents a chal-
lenge to the developed quaternion-based EKF, mainly because
Fig. 6. Time function of the pitch angle measured from the Vicon system (truth of the duration of the strap-down integration.
reference) and the pitch angle estimation error, computed as the difference be-
tween the pitch angle estimate produced by the EKF (Method A) and the truth
Within the limits of the experimental validation, the perfor-
reference. Dashed line: truth reference, solid line: estimation error. mances of inertial/magnetic sensing are remarkable as com-
pared with those of a state-of-the-art motion capture system.
The performance characteristics of the InertiaCube2 are spec-
characterized in particular by smaller values of the random walk ified by the manufacturer as 1 RMS (static accuracy) and 3
SD, which yield better performance in the absence of the distur- RMS (dynamic accuracy). These specifications are in the same
bances (row named A1); however, when the disturbances are order as those reported for other commercially available MODS
introduced in the simulation (row named A2), this parameter products. It comes as no surprise that, in dynamic conditions,
setting turns out to be not well suited, because of the reduced the accuracy is generally worse than in static conditions: this
ability of the algorithm to protect against destabilizing effects is partly because accelerometers, exposed to both gravity and
of the disturbances before their detection occurs. body accelerations, can estimate inclination with limited ac-
Within the limits of our analysis, the effect of the sampling curacy. Irrespective of the measurement protocols devised by
rate is not prominent. This is interesting, since the EKF lin- manufacturers to specify the accuracy of their products, which
earization may lead to filter instability unless suitably small would also include environment and motion trajectories, iner-
sampling intervals are used [28], [30]. It is likely that human tial sensing for orientation determination does not have a com-
motion dynamics is not particularly challenging to track, as dis- petitive edge over digital optical systems, when the latter tech-
cussed at length in [30]. On the other hand, the analysis in [30] nology is not subject to, e.g., line-of-sight or working volume
is carried out without dealing with the problem of sensor accu- limitations [31]. For the purpose of the experimental validation
racy. It remains to see whether the claim is valid when the es- performed in this paper, the orientation information provided

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
SABATINI: DETERMINING ORIENTATION BY INERTIAL AND MAGNETIC SENSING 1355

by a digital optical system such as Vicon can be considered the [2] C. Verplaetse, “Inertial proprioceptive devices: self-motion-sensing
truth reference for filtering algorithm assessment. toys and tools,” IBM Syst. J., vol. 35, no. 3–4, pp. 639–650, 1996.
[3] M. J. Caruso, “Applications of magnetic sensors for low cost compass
Although all gyros in our sensing hardware appear to work systems,” in Proc. IEEE Position, Location and Navigation Symp., San
properly, the pitch angle estimation accuracy provided by Diego, CA, March 13–16, 2000, pp. 177–184.
Method E is unexpectedly poorer as compared with the esti- [4] B. Kemp, A. J. M. W. Janssen, and B. van der Kamp, “Body posi-
tion can be monitored in 3D using accelerometers and earth-magnetic
mation accuracy of other Euler angles provided by the same field sensors,” Electroencephalogr. Clin. Neurophysiol., vol. 109, pp.
method: it is arguable that, because of the time-integration 484–488, 1998.
of wideband noise superimposed to gyro signals, the pitch [5] A. M. Sabatini, “Inertial sensing in biomechanics: a survey of computa-
tional techniques bridging motion analysis and personal navigation,” in
angle estimation accuracy happens to be penalized in this Computational Intelligence for Movement Sciences: Neural Networks,
particular experiment. In full compliance with the principles of Support Vector Machines and Other Emerging Techniques, R. K. Begg
complementary filtering, however, gravimetric tilt sensing and and M. Palaniswami, Eds. Hershey, PA: Idea Group Inc., 2006, ch. 2,
pp. 70–100.
earth’s magnetic field sensing are well suited to stabilize the [6] H. J. Luinge and P. H. Veltink, “Inclination measurement of human
gyro orientation. It is worthy noting that, while the accuracy of movement using a 3-D accelerometer with autocalibration,” IEEE
pitch and roll estimates produced by Method A and Method F Trans. Neural Syst. Rehab. Eng., vol. 12, no. 1, pp. 112–121, Jan.
2004.
is similar, the yaw estimate is better when Method A is used. [7] J. L. Marins, X. Yun, E. R. Bachmann, R. B. McGhee, and M. J. Zyda,
It is not our goal here to compare the developed EKF with “An extended Kalman filter for quaternion-based orientation estima-
the native algorithm within the InertiaCube2, which is simply tion using MARG sensors,” in Proc. IEEE/RSJ Int. Conf. Intelligent
Robots and Systems , Mani, HI, Oct. 29–Nov. 03 2001, pp. 2003–2011.
used here with its default parameter setting. We argue that [8] J. C. K. Chou, “Quaternion kinematic and dynamic differential equa-
the improvement of yaw estimate is mainly due to the in-line tions,” IEEE Trans. Robot. Autom., vol. 8, no. 1, pp. 53–64, 1992.
calibration procedure, and indeed the yaw estimate produced [9] C. Kirtley, “Summary: Quaternions vs. Euler angles,” BIOMCH-L Dis-
cussion May 3, 2001 [Online]. Available: http://isb.ri.ccf.org/biomch-l/
by Method A is about the same order of accuracy as the yaw archives/biomch-l-2001-05
estimate produced by Method E. A key factor for exploiting the [10] D. Gebre-Egziabher, G. H. Elkaim, J. D. Powell, and B. W. Parkinson,
improvement provided by Method A is that the gyro performs “A gyro-free quaternion-based attitude determination system suitable
for implementation using low cost sensors,” in Proc. IEEE Position,
reasonably well. The improved EKF promotes a sort of sensor Location and Navigation Symp., San Diego, CA, Mar. 13–16, 2000,
bootstrap: the sensor is forced to rely on itself in order to pp. 185–192.
compensate for the bias drift, since external sensors, e.g., GPS [11] E. Foxlin, M. Harrington, and Y. Altshuler, “Miniature 6-DOF inertial
system for tracking HMDs,” Proc. SPIE, vol. 3362, Aerosense 98, pp.
are not used to make the system error states observable. It is, 1–15, 1998.
thus, important, for instance, that the gyro bias is captured by [12] InterSense, Inc., [Online]. Available: http://www.isense.com/products/
zero attitude updates, before bias becomes a dominant factor in prec/ic2/inde x.htm
[13] H. Rehbinder and X. Hu, “Drift-free attitude estimation for accelerated
determining the orientation uncertainty, which may happen in rigid bodies,” in Proc. IEEE Int. Conf. Robotics Automation, Seoul,
practice in less than few minutes with commercial grade gyros Korea, May 21–26, 2001, pp. 4244–4249.
[1]. Provided that the bias capture feature is active, it is then [14] T. Harada, H. Uchino, T. Mori, and T. Sato, “Portable absolute orien-
tation estimation device with wireless network under accelerated situ-
important that the filter mechanization is studied to prevent ation,” in Proc. IEEE Int. Conf. Robotics Automat., New Orleans, LA,
the errors incurred by the aiding sensors from degrading the Apr. 2004, pp. 1412–1417.
orientation estimator performance. [15] Q. Ladetto and B. Merminod, “In step with INS—navigation for the
blind, tracking emergency crews,” GPS World, pp. 30–38, Oct. 2002.
We have developed an improved quaternion based EKF for [16] D. Roetenberg, H. J. Luinge, C. T. M. Baten, and P. H. Veltink, “Com-
estimating the orientation of a rigid body using a MODS en- pensation of magnetic disturbances improves inertial and magnetic
dowed with one tri-axis gyro, one tri-axis accelerometer and sensing of human body segment orientation,” IEEE Trans. Neural
Syst. Rehab. Eng., vol. 13, no. 3, pp. 395–405, Mar. 2005.
one tri-axis magnetometer. The EKF incorporates an in-line cal- [17] R. Smith, A. Frost, and P. Probert, “Aspects of heading determination
ibration procedure for modeling time-varying biases which may via fusion of inclinometer and magnetometer data,” in Proc. Int. Conf.
affect sensors like accelerometers and magnetometers, and a Advanced Robotics, Monterrey, CA, Jul. 7–9, 1997, pp. 739–744.
[18] B. Hoff and R. Azuma, “Autocalibration of an electronic compass in
mechanism for adapting their measurement noise covariance an outdoor augmented reality system,” in Proc. IEEE/ACM Int. Symp.
matrix in the presence of motion and magnetic disturbances. Augmented Reality, Münich, Germany, Oct. 5–6, 2000, pp. 159–164.
These two features in combination prove helpful to enhance the [19] F. Ferraris, U. Grimaldi, and M. Parvis, “Procedure for effortless
in-field calibration of three-axis rate gyros and accelerometers,”
accuracy of orientation estimates, as demonstrated by computer Sensors Mater., vol. 7, pp. 311–330.
simulations and experimental validation in connection with the [20] D. Gebre-Egziabher, G. H. Elkaim, J. D. Powell, and B. W. Parkinson,
use of state-of-the-art MODS. “A non-linear, two-step estimation algorithm for calibrating solid-state
strapdown magnetometers,” in Proc. Int. Conf. Integrated Navigation
Systems, St. Petersburg, Russia, May 28–30, 2001, pp. 290–297.
[21] A. M. Sabatini, “Quaternion based strap-down integration method
ACKNOWLEDGMENT for applications of inertial sensing to gait analysis,” Med. Biol. Eng.
Comput., vol. 42, pp. 97–105, 2005.
The author would like to thank Dr. L. Grimaldi for performing [22] E. Abbott and D. Powell, “Land-vehicle navigation using GPS,” Proc.
the experimental validation with the Vicon system, and Dr. A. IEEE, vol. 87, no. 1, pp. 145–162, Jan. 1999.
Ravaschio for the assistance in operating this system. [23] R. Williamson and B. J. Andrews, “Detecting absolute human knee
angle and angular velocity using accelerometers and rate gyroscopes,”
Med. Biol. Eng. Comp., vol. 39, pp. 294–302, 2001.
REFERENCES [24] A. M. Sabatini, C. Martelloni, S. Scapellato, and F. Cavallo, “Assess-
ment of walking features from foot inertial sensing,” IEEE Trans.
[1] E. Foxlin, “Motion tracking requirements and technologies,” in Hand- Biomed. Eng., vol. 52, no. 3, pp. 486–494, Mar. 2005.
book of Virtual Environment Technologies, K. Stanney, Ed.. Hills- [25] J. Elwell, “Inertial navigation for the urban warrior,” Proc. SPIE, vol.
dale, NJ: Lawrence Erlbaum, 2002, ch. 8, pp. 163–210. 3709, pp. 196–204, 1999.

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.
1356 IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006

[26] J. C. Lötters, J. Schipper, P. H. Veltink, W. Olthius, and P. Bergveld, Angelo M. Sabatini (M’90–SM’06) received the Dr.
“Procedure for in-use calibration of triaxial accelerometers in medical Eng. degree in electrical engineering from the Uni-
applications,” Sensors Actuators A, vol. 68, pp. 221–228, 1998. versity of Pisa, Pisa, Italy, in 1986, and the Ph.D. de-
[27] D. Choukroun, “Novel methods for attitude determination using vector gree in biomedical robotics from Scuola Superiore
observations,” Ph.D. thesis, Technion, Israel Inst. Technol., Haifa, Is- Sant’Anna, Pisa, in 1992.
rael, 2003. From 1987–1988 he was with Centro E. Piaggio,
[28] P. S. Maybeck, Stochastic Models, Estimation and Control. New Faculty of Engineering, University of Pisa. During
York: Academic, 1979. the summer of 1988, he was a Visiting Scientist at the
[29] J. E. Bortz, “A new mathematical formulation for strapdown inertial Artificial Organ Lab, Brown University, Providence,
navigation,” IEEE Trans. Aerosp. Elec. Syst., vol. AES-7, no. 1, pp. RI. From 1991–2001, he was an Assistant Professor
61–66, Jan. 1971. of Biomedical Engineering at Scuola Superiore
[30] J. J. LaViola, Jr., “A comparison of unscented and extended Kalman Sant’Anna, where he has been an Associate Professor of Biomedical Engi-
filtering for estimating quaternion motion,” in Proc. American Control neering since 2001. His main research interests are the design and validation
Conf., Denver, CO, June 4–6, 2003, pp. 2435–2440. of intelligent assistive devices and wearable sensor systems, biomedical signal
[31] G. Welch and E. Foxlin, “Motion tracking: no silver bullet, but a re- processing, and quantification of human performance.
spectable arsenal,” IEEE Comput. Graphic. Applicat., vol. 22, no. 6,
pp. 24–38, Nov.–Dec. 2002.

Authorized licensed use limited to: INSTITUTO FEDERAL DO ESPIRITO SANTO. Downloaded on July 29,2020 at 14:35:54 UTC from IEEE Xplore. Restrictions apply.