Sensors 2011, 11, 9182-9206; doi:10.

3390/s111009182
OPEN ACCESS

sensors
ISSN 1424-8220
www.mdpi.com/journal/sensors

Article

Kalman-Filter-Based Orientation Determination Using

Inertial/Magnetic Sensors: Observability Analysis and
Performance Evaluation
Angelo Maria Sabatini

The BioRobotics Institute, Scuola Superiore Sant’Anna, Piazza Martiri della Libertà 33, 56124 Pisa,
Italy; E-Mail: [email protected]; Tel.: +39-050-883-415; Fax: +39-050-883-101

Received: 4 August 2011; in revised form: 21 September 2011 / Accepted: 23 September 2011 /
Published: 27 September 2011

Abstract: In this paper we present a quaternion-based Extended Kalman Filter (EKF) for
estimating the three-dimensional orientation of a rigid body. The EKF exploits the
measurements from an Inertial Measurement Unit (IMU) that is integrated with a tri-axial
magnetic sensor. Magnetic disturbances and gyro bias errors are modeled and compensated
by including them in the filter state vector. We employ the observability rank criterion
based on Lie derivatives to verify the conditions under which the nonlinear system that
describes the process of motion tracking by the IMU is observable, namely it may provide
sufficient information for performing the estimation task with bounded estimation errors.
The observability conditions are that the magnetic field, perturbed by first-order
Gauss-Markov magnetic variations, and the gravity vector are not collinear and that the
IMU is subject to some angular motions. Computer simulations and experimental testing
are presented to evaluate the algorithm performance, including when the observability
conditions are critical.

Keywords: ambulatory human motion tracking; orientation determination; inertial

measurement unit; Extended Kalman filter; Lie derivatives; observability of nonlinear
systems

1. Introduction

Accurate tracking of the orientation of rigid bodies moving in a three-dimensional (3D) space is
important in several applications, including navigation of man-made vehicles, robotics, machine
Sensors 2011, 11 9183

interaction and, of main interest to us in this paper, ambulatory human motion analysis. Several
approaches are available to create trackers that determine orientation based on measurements from,
e.g., acoustic, mechanical, optical, inertial and magnetic sensors [1]. One popular approach is based on
the principles behind inertial/magnetic sensing.
A state-of-the-art Inertial Measurement Unit (IMU) consists of a tri-axial accelerometer, a tri-axial
gyro, and a tri-axial magnetic sensor, henceforth referred to as an integrated IMU. The 3D orientation
can be computed by time-integrating the gyro output from known initial conditions. Due to
low-frequency gyro bias drift this operation is subject to errors that tend to grow unbounded over time.
On the other hand, gyros may help achieving accurate orientation estimates for highly dynamic
motions. The initial conditions needed for gyro integration are usually given by the accelerometer used
in combination with the magnetic sensor. The accelerometer can provide drift-free inclination
estimates by sensing the gravity vector; the magnetic sensor, which would sense the earth magnetic
field vector, helps providing drift-free heading estimates. Serious limitations affect their operation
when applied to ambulatory human motion tracking [2-4]. First, the difficulty of correctly interpreting
the acceleration signals exists, when the component due to the gravity field (vertical reference)
coexists with the component related to the body motion [5]. Second, ferromagnetic materials nearby
the IMU are critically disturbing sources when the magnetic sensor output is used to build the
horizontal reference for heading estimation [6,7].
Accurate estimates of the three-dimensional (3D) orientation of a rigid body by inertial/magnetic
sensing require that the complementary properties of gyros, accelerometers and magnetic sensors are
exploited [8]. Sensor fusion techniques are needed in order that the aiding sensors (accelerometer and
magnetic sensor) help mitigating the low-frequency gyro bias errors, while, in turn, the signals from
the aiding sensors, which are prone to relatively high-frequency errors, are smoothed using gyro data.
Different approaches are available to design sensor fusion algorithms [8]. In [9] a simple time-domain
first-order complementary filter is designed: it performs low-pass filtering on the signals from the
accelerometer-magnetic sensor pair and high-pass filtering on the signals from the gyro. Another
possible approach is represented by the use of deterministic single-frame estimation techniques [8].
They all are based on the concept of vector matching, which requires, in principle, the measurements
of constant reference vectors (e.g., gravity and the Earth’s magnetic field) [10]. Mostly, these
techniques have been employed in gyro-free systems for tracking static or slowly moving bodies in
environments containing only small magnetic sources [3,4,11]. In their original formulation, they are
unable to provide sequential estimates of a time-varying orientation and of other parameters than the
orientation, such as sensor biases [12]. In contrast with the previous approaches, which bypass at all
statistical modeling in the estimator design, extended stochastic linear estimation techniques use a
model for predicting aspects of the time behavior of a system (dynamic model) and a model of the
sensor measurements (measurement model). There appears to be wide consensus that the Kalman filter
is “perhaps the perfect tool for elegantly combining multisensory fusion, filtering, and motion
prediction in a single fast and accurate framework” [13]. Since orientation determination is
intrinsically a non-linear problem, Extended Kalman filters (EKFs) are the tools to work with [14].
In [15] an indirect-state formulation of the EKF for an inertial head-tracker is described; it operates
on errors in the primary variables of Euler angles, angular velocity and gyro bias. The indirect-state
formulation is chosen to provide fast response, i.e., low latency due to computational demands of the
Sensors 2011, 11 9184

algorithm. However, no expedient is provided to guard against the effects of body accelerations and
magnetic disturbances. Very interesting is the work in [16], where the authors use one tri-axial
accelerometer to measure inclination during dynamic tasks without requiring additional sensors, be
they gyros or magnetic sensors. A KF-based algorithm is designed to estimate the different
acceleration components, namely gravity and inertial acceleration, plus the accelerometer bias, based
on few reasonable assumptions about the properties of human motion. In a later paper by the same
group, an integrated IMU is used to estimate the full orientation matrix [17]. The KF for body segment
orientation in [16] is thus extended with models of gyro and magnetic sensor biases, in the attempt to
prevent heading drift and to compensate magnetic disturbances. A direct-state EKF is developed
in [18], where the quaternion and the angular velocity are included in the vector state. Dynamic models
of human limb motion in terms of first-order Gauss-Markov stochastic processes are used in
developing the filter equations. The linear measurement equations include the components related to
gyro measurements and components that are formulated in the quaternion space; this latter part of the
measurement equations uses a deterministic single-frame estimation technique, i.e., a Gaussian
Newton optimizer, that computes the corresponding quaternion for each set of accelerometer and
magnetic sensor measurements. No compensation of sensor errors or protection against magnetic
effects is provided in this work. A direct-state quaternion-based formulation of the EKF is developed
in [2], where angular velocity is considered a control input and active compensation (gyro bias and
magnetic effects) is achieved by using state-augmentation techniques. Since the angular velocity is not
part of the state vector, models of human motion dynamics are not necessary in the development of the
filter equations [19]. The non-linear measurement equations are formulated by rotating the reference
vectors in the body-frame using the estimated orientation matrix. The EKF is made adaptive
by introducing vector selection schemes that work on the measured gravity and magnetic field
vectors [4,20,21]: the measurement noise variances are increased in value, thus giving more weight to
the filter predictions, when variations are found between the measured acceleration and the gravity, or
between the measured magnetic field and the earth magnetic field.
The use of state-augmentation techniques is the preferred approach within the KF framework to
take sensor errors and magnetic disturbances into account. At one point or another, however, the
dimension of the state vector may create problems of system observability. Issues of system
observability have sometimes been addressed, e.g., [21,22], without a formal analysis being carried
out. A primary contribution of this paper is to elucidate under which conditions a KF-based algorithm
for orientation determination using an integrated IMU is observable; in other words, the conditions
when sufficient information is available for estimating a state vector that includes, in the present case,
the quaternion of rotation, the magnetic variation superimposed to the magnetic reference vector and
the gyro bias vector. To the best of our knowledge, this is the first time an observability analysis based
on tools from the nonlinear control theory, namely the Lie derivatives, is performed in this regard. We
take inspiration from the work described in [23], where the observability analysis is applied to a
problem of IMU-camera calibration. The observability conditions are that the magnetic field vector,
perturbed by first-order Gauss-Markov magnetic variations, and the gravity vector are not collinear,
and that the IMU is subject to some angular motions. Computer simulations and experimental testing
are presented to evaluate the algorithm performance, including when the observability conditions are
critical.
Sensors 2011, 11 9185

2. Theoretical Background

2.1. Using Quaternions to Represent the Orientation

The 3D orientation of a rigid body is represented using two right-handed coordinate systems: the
earth-fixed frame E = {e1 e2 e3} and the body-fixed frame B = {e1' e '2 e3' } . In applications of motion
tracking the motion of the E-frame due to the earth’s rotation is usually negligible; the E-frame is
therefore an inertial frame. In the following, we also assume that the sensitivity axes of the IMU
sensors are aligned with the axes of the B-frame, and the physical location of the accelerometer is
where the origin of the B-frame is located, i.e., no centripetal acceleration effects exist.
An arbitrary vector x in the 3D space can be represented in terms of the coordinates (or
components) with respect to either of the frames:
xB = EB C xE . (1)

The subscripts E, B denote in which frame the vector x is represented. The columns of the
orientation matrix EB C are the representations of the ei , i = 1,...,3 with respect to B, while its rows are
the representations of the e'i , i = 1,...,3 with respect to E. The orientation matrix and its transpose allow
moving vector representations from (to) E to (from) B, respectively. The orientation matrix is a 3 × 3
orthogonal matrix with unit determinant, which belongs to the 3D special orthogonal group SO(3) of
rotation matrices. The quaternion is a more parsimonious representation of orientation than the
orientation matrix [24]; it is derived by formulating the orientation matrix as a homogeneous quadratic
function of the Euler symmetric parameters qi, i = 1,...,4:
C ( q ) = I 3×3 − 2q4 [q ×] + 2 [q ×] ,
B 2
E (2)

where the operator:

⎡ 0 −q3 q2 ⎤
[q ×] = ⎢⎢ q3 0 − q1 ⎥⎥ (3)
⎢⎣ − q2 q1 0 ⎥⎦

is the standard vector cross product and In×n is the n × n identity matrix. It is commonplace to refer to q
T
as the vector part and to q4 as the scalar part of the quaternion q = ⎡⎣ qT q4 ⎤⎦ :

q = [ q1 q2 q3 ] = sin (θ / 2 ) n T ,
T
q4 = cos (θ / 2 ) . (4)

Here, the axis and angle of rotation (n,θ ) are another valid representation of the orientation, closely
related to the rotation vector θ = θ n [24]. As implied by Equation (4), in order to represent a valid
rotation the quaternion must comply with the normalization constraint:
4

∑q 2 2
q= i = q + q42 = 1. (5)
i =1

In the quaternion space the multiplication between two generic quaternions q and q' is defined as
follows:
Sensors 2011, 11 9186

⎣( )
q ⊗ q ' = ⎡ q4' q + q4 q ' + [q ×] q ' q4 q4' − q ⋅ q ' ⎤ ,
T

⎦ (6)

where the symbol ⋅ denotes the standard vector dot product.

T
Given the vector quaternion xE = ⎡⎣ xET 0 ⎤⎦ , namely a quaternion with null scalar part associated
with the vector xE , the vector quaternion:
T
xB = ⎡⎣ xBT 0 ⎤⎦ = q −1 ⊗ xE ⊗ q (7)

is associated with the vector xB , rotated about the n-axis through an angle θ, see Equation (4). The
inverse quaternion q −1 in Equation (7) is given by:

T ⎡⎣ −qT q4 ⎤⎦
q ⊗ q = ⎡⎣0 1⎤⎦
−1 T
→ q = −1
2
. (8)
q

When the unit norm constraint Equation (6) is enforced, a quaternion is left with the three degrees
of freedom consistent with the SO(3) dimensionality. The four-component unit quaternion has thus
the lowest dimension of any globally non-singular orientation parameterization. However, the
representation is redundant, since the quaternion −q represents the same rotation as q.

2.2. Kinematic Equations

These describe the relations between the temporal derivatives of an orientation representation such
as the quaternion q and the angular velocity ωB, namely the angular velocity of the B-frame relative to
the E-frame, as it is measured by a tri-axial gyro fixed to the body:
d 1 T 1 ⎡ − [ ωB ×] ωB ⎤
q = q ⊗ ⎡⎣ωB T 0 ⎤⎦ = ⎢ ⎥ q = Ω ( ωB ) q, (9)
dt 2 2 ⎣ −ωB T 0 ⎦

In the following the time argument will always be dropped to make the notation a little easier.
Ω(ωB) is a 4 × 4 skew symmetric matrix. The kinematic Equation (9) do not involve nonlinear
computationally expensive trigonometric functions and are not affected by the presence of singularity
points, in contrast to the formulation using representations of orientation such as the Euler angles.
The discrete-time equivalent of Equation (9) is given by:
q(tk ) = Φ q(tk −1 ), (10)
with:
sin ( uB / 2 )
Φ = cos ( uB / 2 ) I 4×4 + Ω ( ωB (tk −1 ) ) . (11)
uB / 2

If the angular velocity is assumed constant in the sampling interval Ts = tk − tk−1 , we have:
tk
uB = ∫ ωB (τ ) dτ ≈ ωB (tk −1 ) Ts . (12)
tk −1
Sensors 2011, 11 9187

2.3. Observability Analysis

A system is observable when, for any possible sequence of state and control input vectors, the
current state can be inferred by measuring the output variables. In other words, the output variables
contain all the information needed to determine the behavior of the system. For continuous time-
varying nonlinear systems the observability analysis, namely the analysis needed to verify whether a
system is observable or not, requires the development of specific tools [25].
Consider a nonlinear system Σ described, in state space form, by a set of equations of the following
form:
⎧d l

⎪ x = f ( x , u ) = f 0 ( x ) + ∑ fi ( x ) ui
⎨ dt i =1 (13)
⎪y = h ( x )
⎩
The time argument in x, u, and y is dropped. x = [ x1 , x2 ,..., xn ] T ∈ X ⊂ R n , u = [u1 , u2 ,..., ul ] T ∈ R l ,
y = [ y1 , y2 ,..., ym ] T ∈ R m are, respectively, the state vector, the control input vector and the measurement
vector. The process function f is input-linear, i.e., it can be decomposed into a sum of independent
functions fi, each one corresponding to a different component ui of the control input vector u. The
process function f and the measurement function h are assumed smooth in their arguments, namely
they possess continuous partial derivatives of any order.
A suitable tool for studying the observability properties of Σ is the observability rank condition
based on the Lie derivatives of the measurement functions hk (x), k = 1,..., m. The zero-order
Lie derivative of a scalar function is, by definition, the function itself, L 0 hk (x) = hk (x), k = 1,..., m. The
first-order Lie derivative of the measurement function hk (x) with respect to the component fi of the
process function f is given by:
n
∂hk
Lf1i hk ( x ) = ∇hk ( x ) ⋅ fi ( x ) = ∑ ( x ) fij ( x ) , i = 0,..., l , (14)
j =1 ∂x j

where ∇ denotes the gradient operator. Since the first-order Lie derivative is a scalar function itself, the
second-order Lie derivative of hk (x) with respect to fi is defined in a similar fashion to (14):

Lfi2 hk ( x ) = ∇Lf1i hk ( x ) ⋅ fi ( x ) . (15)

The definition of higher-order and mixed Lie derivatives with respect to different components fi, fj
of the process function f follows the same route as above. For instance, the second-order Lie
derivatives with respect to fj and fi, given their first-order derivatives with respect to fi, are given by
Lf2j fi hk ( x ) = ∇Lf1i hk ( x ) ⋅ f j ( x ) , i, j = 0,..., l ; k = 1,..., m. (16)

The observability matrix is constructed by stacking the gradients of the Lie derivatives:

{
O ( x ) = ∇Lfsi ...f j hk ( x ) | i, j = 0,..., l ; k = 1,..., m; s ∈ N . } (17)

The number of columns of the observability matrix is equal to the dimension n of the state vector x,
while the number of rows may grow unbounded with the order of the computed Lie derivatives. Since
the input and measurement functions are smooth, the number of rows of O can be thus virtually infinite.
Sensors 2011, 11 9188

For nonlinear systems the observability property is related to the concept of indistinguishability of
states with respect to the control inputs. Two states x1 and x2 are said indistinguishable when, for every
admissible control input, the system produces the same outputs in a given interval of time when
starting from either x1 or x2. The system Σ is weakly observable at the state x0 if for every open
neighborhood U of x0, x0 can be distinguished from states in U(x0), or weakly observable if it is so for
every x ∈ X. The system Σ is locally weakly observable at the state x0 if x0 can be distinguished from
states in an open neighborhood V(x0) ⊂ U(x0) when admissible control inputs lead to paths within
U(x0), or locally weakly observable if it is so for every x in X. A sufficient condition for the system Σ
to be locally weakly observable at x0 is that O(x0) is full rank, namely the gradients of the Lie
derivatives of the measurement functions computed at x = x0 must span the state space X. If it is so for
every x ∈ X the system Σ is locally weakly observable. It should be noted that the full-rank condition
of the observability matrix O(x) is only sufficient to say that the system is locally weakly observable.
In the case that the observability matrix O(x) is rank-deficient, the system’s behavior has to be verified
through computer simulations and experimental testing.

3. Method

3.1. Sensor Modeling

In response to the time-variant body angular velocity ωbody, acceleration (constant gravity g and
time-variant body acceleration abody), and local magnetic field (constant earth’s magnetic field h and
any time-variant magnetic variation h b acting nearby the sensor) the measured outputs of the gyro, the
accelerometer and the magnetic sensor from an integrated IMU can be written as follows:
⎧ωm = g K ωbody + g b + g v
⎪
⎪
⎨a m = K E C ( −g + a body ) + b + v
a B a a
(18)
⎪
⎪⎩h m = K E C ( h + b ) + b + v,
m B h m m

g
K , a K , m K are the matrices of the scale factors (ideally, they are equal to I3×3); g b, a b , m b are the bias
vectors (ideally, they are null); g v , a v , m v are assumed independent white Gaussian measurement
noises, with null mean and covariance matrix Σ g = σ g2 I 3×3 , Σ a = σ a2 I 3×3 and Σ m = σ m2 I 3×3 . Equation (18)
is a simplified model that does not account for additional error sources, such as cross-axis sensitivity,
gyro g-sensitivity, nonlinearity, hysteresis and misalignment [26]. In the context of MEMS sensors, the
component in the gyro output due to the Earth’s rotation can be neglected as compared to sensor
errors, and therefore it does not appear in Equation (18).
The scale factor and bias of inertial and magnetic sensors are functions of environmental conditions,
in particular ambient temperature. Across the thermal variations typically encountered in practice,
thermal effects on accelerometers are of relatively lower quantitative relevance than on gyros, and they
are usually negligible on magnetic sensors. Moreover, scale factor drifts of inertial and magnetic
sensors usually affect the accuracy of the measurement process to a much lesser extent than the bias
drifts of these sensors, in particular gyros. Scale factor and bias errors of accelerometers can be
compensated on time scales up to few hours, using the procedure described in [26]; the procedure
described in [27] can be used to calibrate scale factors and hard iron offsets m b , on similar time scales.
Sensors 2011, 11 9189

A more serious problem concerns the bias errors of gyros and how they develop over time. This
problem is well received in the literature, where orientation estimators are oftentimes developed with
built-in devices for gyro-bias compensation. A random-walk vector random process with statistically
independent components is widely used for carrying the compensation task in a Kalman filtering
framework [28]:
d g
b = wg , (19)
dt
where wg is white Gaussian noise, with null mean and covariance matrix b Σ g = bσ g2 I3×3. We consider the
implementation of these devices essential for a proper operation of the filter, especially when the
opportunity to perform on-line bias capture are precluded by the nature of the tracked motions [29].
For slowly moving bodies in magnetically clean environments, gyro-free orientation determination
systems have been developed with mixed success [3,11]. In these systems a further simplification is
made in the sensor model Equation (18), where the only acceleration measured by the accelerometer is
gravity. Henceforth, we make the simplifying assumption that a body ≈ 0.

3.2. Modeling the Magnetic Variation

A magnetic sensor measures the Earth’s magnetic field plus any other magnetic field superimposed
to it. For the purpose of orientation determination, an accurately known homogeneous magnetic field
in the environment surrounding the tracked body is needed as the horizontal reference for heading
estimation. Especially indoors, magnetic homogeneity is difficult to achieve, due to construction iron
in floors, walls and ceilings, or to various equipment. The magnetic distortions occur in both the
horizontal and vertical plane [6,7]. Suppose that, albeit not homogeneous, the magnetic field existing
in a given region of space is at least time-invariant. In this case, an interesting possibility to deal with
the problem of magnetic distortions would consist, in principle, of calibrating and mapping the
measurement volume. Besides being complicated and time-consuming, this approach is also in contrast
with the development of ambulatory sensor systems, whose prior knowledge of existence and location
of disturbances cannot be taken for granted: for instance, current IMUs do not produce position
information, which makes critical using the magnetic maps of the measurement volume.
IMUs are exposed to magnetic fields that can rapidly change in direction and magnitude, when
they move relative to their surroundings in a magnetically non-homogeneous environment. These
h
magnetic distortions, generically denoted with the term magnetic variation b, can be expressed in the
earth-fixed frame. In order to prevent heading drift, the orientation estimator has to be developed with
a built-in device for compensation of magnetic variations. A first-order Gauss-Markov vector random
process with statistically independent components is chosen to model the magnetic variation:
d h
b = −α h b + w h , (20)
dt
where α is a positive constant, and wh is white Gaussian noise, with null mean and covariance matrix
b
Σ h = bσ h2 I3×3 . This model is the same considered in [17], and generalizes the random-walk model
adopted in our previous research [2]. The random-walk model is obtained by Equation (20) by taking
α = 0.
Sensors 2011, 11 9190

3.3. Filter Implementation

The main difficulty of using quaternion-based state vector components in an EKF lies in the
formulation of the filter equations. For a review of these equations with a generic state-space model
see [30]. The origin of the problem lies in the lack of independence of the four components of a
quaternion, since they are related by the unit-norm constraint. Constraints imposed on the estimated
state variables cannot be preserved by EKFs in their standard development [19]. A popular method to
preserve the quaternion unit-norm property is to normalize the a posteriori estimate after the
measurement update stage (“brute-force” approach). Even though it is neither elegant nor optimal [31],
the "brute-force" approach is proven to work generally well [32]. The EKF developed in this paper
enforces the unit-norm constraint by the “brute-force” approach [2].
Since the angular velocity is measured from a body-fixed tri-axial gyro, the kinematic equations of
a rigid body can be used to obtain the orientation state [19]. Gyro data are treated as external inputs to
the filter rather than as measurements, and gyro measurement noise and bias enter the filter as process
noise rather than as measurement noise. An advantage of this choice is the reduction in the dimension
of the state vector, which may lead to minimal-order, computationally efficient filter implementations.
An important feature of EKFs is the possibility they offer to estimate unknowns by state vector
augmentation techniques. In this paper, we concentrate on the self-compensation of gyro bias and
magnetic variation, both accounted for in the filter as additional state vector components.
The continuous-time system model combines (9)-(19)-(20) together:
⎧d
⎪ dt q = Ω ( ω ) q
⎪
⎪d h
⎨ b = −α h b + w h (21)
⎪ dt
⎪d g
⎪ dt b = w g .
⎩
T
The state vector in Equation (21) is x = ⎡⎣qT h b T g b T ⎤⎦ . The equations for propagating the state
estimates in the model are obtained by applying the expectation operator to Equation (21).
By rearranging the equations in a format suitable for computing the Lie derivatives [23], we obtain:

(22)

ˆ = ωm − g bˆ is treated as a control input, and the matrix Ξ (q ) is given by:

where the angular velocity ω

⎡ q4 −q3 q2 ⎤
⎢ − q1 ⎥⎥
⎡ q4 I 3×3 + [q ×]⎤ ⎢ q3 q4
Ξ (q ) = ⎢ ⎥ = ⎢ −q = ⎡s s2 s3 ⎤⎦ (23)
⎣ −qT ⎦ 2 q1 q4 ⎥ ⎣ 1
⎢ ⎥
−
⎣ 1 q − q2 − q3 ⎦
Sensors 2011, 11 9191

In the following we drop the caret, which denotes the expectation of a random variable, to avoid
unnecessary cluttering in the notation. The discrete-time model allows employing the sampled
measurements of the IMU for state propagation:

(24)

where:

(25)

The process noise component q w ( k − 1) describes how the gyro measurement noise enters the state
model through a quaternion-dependent linear transformation, as follows:
Ts
q
w (k − 1) = − Ξ ( q(k − 1) ) g v (k − 1) (26)
2
The process noise components q w (k − 1), h w (k − 1), g w (k − 1) are assumed uncorrelated; hence, the
process noise covariance matrix Q(k − 1) is shown to have the block-diagonal structure:
⎡σ g2 (Ts / 2 )2 ( trace(M o ) I 4×4 − M o ) 0 3×3 0 3×3 ⎤
⎢ ⎥
⎢ 1 − exp ( −2α Ts ) ⎥
Q(k − 1) = ⎢ 0 3×3 σ h2 I 3×3 0 3×3 ⎥ (27)
2α
⎢ ⎥
⎢
⎣
0 3×3 0 3×3 ( σ gTs ) I3×3 ⎥⎦
b 2

where:
M o = q ( k − 1)q ( k − 1)T + P q ( k − 1) (28)
q ( k − 1) and Pq (k −1) denote, respectively, the expectation and the covariance matrix of the
quaternion component of the state vector [32].
Equation (18) is used to model the sensor measurements. Each reference vector component needed
for vector matching is given a specific equation:
⎡ a m (k ) ⎤ ⎡ E C ( q(k ) ) 03×3 ⎤ ⎡
B
g ⎤ ⎡ a v(k ) ⎤
=
⎢h ( k ) ⎥ ⎢ 0 ⎥⎢ ⎥ + ⎢m ⎥ (29)
E C ( q ( k ) ) ⎦ ⎣h + b ( k ) ⎦
B h
⎣ m ⎦ ⎣ 3×3 ⎣ v(k ) ⎦

as done in [2]. The measurement equations are nonlinear, which forces to compute their Jacobian
matrices when carrying out the linearization process implied by the EKF. They will be shown in the
next Section, in connection with the problem of computing the Lie derivatives of the nonlinear system
of Equation (22). The measurement noise covariance matrix can be expressed directly in terms of the
statistics of the measurement noise affecting each sensor:
⎡σ 2 I 0 ⎤
R (k ) = ⎢ a 3×3 ⎥ (30)
⎣ 0 σ m I 3×3 ⎦ .
2

No vector selection scheme is introduced in the filter developed in this paper [2].
Sensors 2011, 11 9192

The initialization procedure is carried out as follows: the processing of gyro data does not include
the procedure of bias capture; an initialization error is thus introduced by setting the gyro bias
component of the state vector to zero. The magnetic field is not taken from a magnetic field model, but
is rather computed as follows. The aiding sensor measurements are averaged during the initial rest
period (averaging time: 1s). Inclination is estimated by processing the acceleration average vector. The
magnetic average vector is then projected in the horizontal plane using the estimated inclination, which
allows estimating h in the earth-fixed frame. Finally, we apply the TRIAD method to the acceleration
and magnetic average vectors, using the gravity vector g and the estimated value of h as reference
vectors [3,33]. The initial quaternion and its covariance matrix are then computed.

3.4. Filter Observability

The observability test consists of demonstrating that the state space of the system with input-linear
process function described by Equation (22) is spanned by the gradients of the Lie derivatives of the
following measurement functions:
ϕ1 (x) = EB C ( q ) ( −g )
ϕ 2 (x) = EB C ( q ) ( h + h b ) (31)
ϕ3 ( x ) = q q − 1
T

The measurement function ϕ3 (x) is introduced to account for the quaternion normalization
constraint [23]. For its relevance in performing the observability analysis, we introduce, for a generic
vector p, the function:
∂ B
ψ ( q, p ) = E C ( q ) p. (32)
∂q

Using Equation (2) we obtain:

ψ ( q, p ) = ⎡⎣ 2q4 [p ×] + 2 ([p ×][q ×] − 2 [q ×][p ×]) −2 [q ×] p ⎤⎦ (33)

Since the zero-order Lie derivatives are the measurement functions themselves, the gradients of the
zero-order Lie derivatives are the Jacobian matrices of the measurement functions:
∇L 0ϕ1 (x) = ⎡⎣ψ ( q, −g ) 03×3 03×3 ⎤⎦
∇L 0ϕ 2 (x) = ⎡⎣ψ ( q, h + h b ) B
E C ( q ) 03×3 ⎤⎦ (34)
∇L 0ϕ3 (x) = ⎣⎡ 2qT 01×3 01×3 ⎦⎤

The first-order Lie derivatives of ϕ1 and ϕ2 (with respect to f0) can be computed as:
1
Lf10 ϕ1 (x) = ∇L 0 ϕ1 (x) ⋅ f 0 = − ψ ( q, −g ) Ξ ( q ) b g
2
(35)
1
Lf10 ϕ 2 ( x) = ∇L 0 ϕ 2 (x) ⋅ f0 = − ψ ( q, h + h b ) Ξ ( q ) b g + EB C ( q ) ( −α h b )
2
The gradients of the first-order Lie derivatives of ϕ1 and ϕ2 (with respect to f0) are then taken:
Sensors 2011, 11 9193

⎡ 1 ⎤
∇Lf10 ϕ1 (x) = ⎢ X1 03×3 − ψ ( q, −g ) Ξ ( q ) ⎥
⎣ 2 ⎦
(36)
⎡ 1 ⎤
∇Lf10 ϕ 2 (x) = ⎢ X2 X2 − ψ ( q, h + h b ) Ξ ( q ) ⎥
⎣ 2 ⎦

The next Lie derivative of interest is the first-order Lie derivative of ϕ2 with respect to f1:
1
Lf11 ϕ 2 (x) = ∇L 0 ϕ 2 (x) ⋅ f1 = ψ ( q, h + h b ) Ξ ( q ) (37)
2

The gradient of the first-order Lie derivative of ϕ2 with respect to f1 is obtained by expanding the
Lie derivatives as column vectors. These column vectors are stacked together as follows:
∇Lf11 ϕ 2 ( x) = [ Γ ϒ 09×3 ]
⎡ ∂
(
⎢ ∂q ψ ( q, h + b ) Ξ ( q ) e1
h
) ⎤⎥ ⎡ ∂
(
⎢ ∂ h b ψ ( q, b ) Ξ ( q ) e1
h
) ⎤⎥
⎡ Γ1 ⎤ ⎢ ⎥ ⎡ ϒ1 ⎤ ⎢ ⎥
1⎢ ∂ ⎥ 1⎢ ∂ (38)
⎢ ⎥
Γ = ⎢Γ 2 ⎥ = ⎢
2 ⎢ ∂q
(
ψ ( q, h + h b ) Ξ ( q ) e 2 ) ⎥
⎢
ϒ = ⎢ϒ2 ⎥ = ⎥
2 ⎢∂ hb
(
ψ ( q, h b ) Ξ ( q ) e 2 ) ⎥
⎥
⎥
⎣⎢ Γ 3 ⎦⎥ ⎢⎣ ϒ 3 ⎥⎦ ⎢ ⎥
⎢ ∂ ⎥ ⎢ ∂ ψ ( q, h b ) Ξ ( q ) e3
( )
⎢
⎣ ∂ q
(
ψ ( q, h + h b ) Ξ ( q ) e3 ) ⎥
⎦ ⎣⎢ ∂ b
h
⎥
⎦⎥

where e1 = [1 0 0] T , e2 = [01 0] T , e3 = [0 01] T .

Finally, we obtain the 22 × 10 observability matrix O(x):
⎡ ∇L0 ϕ1 (x) ⎤
⎢ ∇L ϕ ( x ) ⎥
⎢ 0 2 ⎥
⎢ ∇L f0 ϕ1 ( x) ⎥ ⎡ C6×7 06×3 ⎤
O( x) = ⎢ ⎥ = ⎢ D6×7 A 6×3 ⎥⎥ (39)
⎢ f0 2 ⎥ ⎢
∇ L ϕ ( x )
⎢ ⎥ ⎢⎣ B10×7 010×3 ⎥⎦
∇ L ϕ
⎢ f1 2 ⎥ ( x )
⎢ ∇L ϕ ( x ) ⎥
⎣ 0 3 ⎦

where the following matrices are introduced:

⎡ 1 ⎤
− ψ ( q, −g ) Ξ ( q ) ⎥
⎡ A11 ⎤ ⎢ 2 ⎡ Γ ϒ ⎤
A=⎢ ⎥=⎢ 1 ⎥ B=⎢ T
A
⎣ 21 ⎦ ⎢ − ψ q, h + h b Ξ ( q ) ⎥ ⎣ 2q 01×3 ⎦⎥
⎣⎢ 2
( ) ⎦⎥ (40)
⎡ ψ ( q, −g ) 03×3 ⎤ ⎡X 03×3 ⎤
C=⎢ ⎥ D=⎢ 1
⎢⎣ ψ ( q, h + b )
h B
E C ( q ) ⎥⎦ ⎣ X2 X3 ⎥⎦

The salient steps of the observability proof are reported in Appendix A, where the matrices X1, X2,
X3, and hence the matrix D, do not need to be computed explicitly. In short, the system of
Equation (22) is certainly locally weakly observable when: (a) the magnetic field has components in
the horizontal plane, which is necessary to have the horizontal reference for heading estimation; (b) at
least one degree of rotational freedom is excited. Remind that the magnetic dip angle, also called
magnetic inclination, is the angle the Earth’s magnetic field makes with the surface of the Earth; the
observability condition (a) fails when the dip angle is ±90° (e.g., close to the Poles). Running the
Sensors 2011, 11 9194

estimation algorithm during periods when the observability is not guaranteed—when the magnetic dip
angle is close to ±90° or the IMU is in quasi-static conditions—is likely to degrade seriously the state
estimation accuracy. This fact has to be verified by assessing the system’s behavior by computer
simulations and experimental testing.

3.5. Filter Assessment

The performance metrics are based on computing the error quaternion Δq = q t−1 ⊗ q , where q t and
q are the true and the estimated quaternion, respectively. In the present context the term true implies
the use of synthetic or experimental motion data to carry out the test procedure. An obvious advantage
of working with synthetic signals is that the errors incurred in estimating the state vector components
can be compared with the bounds that are predicted by the error covariance matrix produced by the
EKF. This is a useful feature to assess the filter convergence and to diagnose a number of potential
problems arising in its numerical implementation. Experimental motion data can be captured, e.g.,
from an optoelectronic motion tracker, so as to define the ground truth reference for filter assessment.
The error quaternion represents the rotation that brings the estimated body frame onto the true body
frame. Its scalar component can be used to derive the orientation error Δθ, according to the equation
Δθ = 2arccos(Δq4) The performance metrics are expressed in terms of the root-mean-square-value of
the orientation error (RMSEq), averaged over the number of either the Monte Carlo simulation runs or
the experimental trials available. Alternatively, a set of estimated and reference Euler angles (roll,
pitch, yaw or heading) can be computed from q t and q using standard conversion formulas, and the
filter performance can be summarized by presenting the RMSEs of the Euler angles, again averaged
over the number of either the Monte Carlo simulation runs or the experimental trials available.
Although the calculations for orientation are not performed using Euler angles, the results can be
presented in this way for better interpretation.

3.6. Computer Simulations

The body-fixed frame has a fixed origin, and it is aligned with the Earth-fixed frame at the initial
time of each simulation trial. Our choice of the earth-fixed frame follows the North-East-Down (NED)
convention: the gravity vector is g = [0 0 g]T (g = 9.81 m/s2), and the earth magnetic field is
h = [hx 0 hz]T (hx = 0.26 Gauss; hz = 0.37 Gauss). The simulated field strength is approximately 0.45
Gauss, with a magnetic dip angle δ ≈ 55°, situations typical of our latitudes. A magnetically perturbed
environment is simulated by adding h with the output of a routine that generates samples from a
Gauss-Markov vector random process with statistically independent components.
The angular velocity time functions are given as input to the simulation program. The true
quaternion time functions are obtained by time integrating the kinematic Equation (9) at the sampling
frequency of fs = 4 kHz, which is high enough to make integration errors negligible; the quaternion
time functions are then time-decimated down to fs = 100 Hz (the sampling frequency used in many
popular orientation trackers). Two distinct conditions are tested: static conditions, in which case the
angular velocity is null, and then the IMU does not change the initial orientation; dynamic conditions,
i.e., following an initial period of rest, a pure sinusoidal rotation around the vertical axis is simulated
(amplitude: 100°/s; frequency: 1 Hz). In either case simulation trials last 10 min.
Sensors 2011, 11 9195

The magnetic field and the gravity vector can be expressed in the body-frame using the computed
quaternions. White Gaussian noise is then injected into the body-referenced sensor time functions to
simulate the effect of specified amounts of measurement noise: σa = 1 − 5 mg; σh = 1 mGauss. In
dynamic conditions the value of σa is chosen relatively high as compared with values measured during
on-bench calibration tests of MEMS accelerometers, which are less than 1 mg; this is to account for
the minute motions that affect on-body IMUs, even when the body is quasi-static. No scale factor or
bias errors are considered for the IMU sensors, with the exception of the gyro: the gyro time functions
are corrupted by addition of a Gaussian white measurement noise component with standard deviation
σg = 0.4°/s and a fixed offset (e.g., b g = [1 − 0.5 0.75] T °/s).
The different treatment of IMU sensors is motivated by the weight an uncompensated gyro bias has
on the error budget of an inertial orientation sensor and consequently by our desire to always enable
the compensation of gyro bias in our filter. The following two EKF implementations are considered:
EKF-MA and EKF-MB, which differ in enabling the compensation of magnetic disturbances or not;
in order to disable the compensation of magnetic disturbances, bσ h = 0 and α = 0 are plugged in
Equation (27). The two methods are tested in static and dynamic conditions, when either the magnetic
variation is markovian, namely it follows the Gauss-Markov model of Equation (20) (magnetically
perturbed environment, MPE), or when the magnetic variation is null (magnetically clean environment,
MCE). For each combination of method and condition, N = 10 Monte Carlo runs are performed, and
the values of orientation RMSEq are reported (mean ± standard deviation, SD). Another set of
simulations is carried for different values of the magnetic inclination, from 55° to 90° (dynamic
conditions—method MA). The aim is to verify the filter behavior in difficult observability conditions.
The filter parameter setting is given in Table 1.

Table 1. EKF parameter setting (Monte Carlo simulations).

MPE-MA MCE-MA MPE-MB MCE-MB

σg , °/s 0.4 0.4 0.4 0.4
2
b
σg , °/s 0.01 0.01 0.01 0.01
b
σh , mGauss/s 10 1 0 0
−1
α,s 1 1 0 0
σa , mg 5 5 5 5
σh , mGauss 1 1 1 1

3.7. Experimental Validation

The MTx orientation tracker by Xsens Technologies B.V. (Enschede, The Netherlands) is used for
carrying out the experimental validation. The raw sensor data are delivered through a USB interface to
the host computer at a rate of 100 Hz, together with the Euler angle time functions estimated by the
proprietary EKF, henceforth called the Xsens-EKF. The IMU sensors are calibrated before starting the
experimental session as described in [26,27]; by contrast, no bias capture is considered for the gyro.
The initial orientation of the sensor frame relative to the marker frame is found by taking data during
the rest period when the IMU is still.
Sensors 2011, 11 9196

The tests for validation in static and dynamic conditions are carried out within one of our lab rooms.
The static test, whose duration is 10 min, consists of leaving the IMU still on a table that is placed far
from current wires, computer appliances, and ferromagnetic materials. A magnetic disturbance is
induced at the time when a cell phone is placed close to the IMU; the cell phone is left in place for
a while, then it is removed, so as to recover the initial magnetic field. The test for validation in
dynamic conditions is performed as follows. The IMU is fastened to a 50 cm × 50 cm wooden
plate using double-side adhesive tape. The plate is raised by hand slightly over the table and then it is
freely moved around; toward the end of the trial, which lasts about 137 s, the plate is replaced
approximately in the same pose. The working volume visited by the plate during the motion trial is
100 cm × 50 cm × 60 cm. The plate orientation is recorded using a six-camera Vicon optical motion
capturing system with a sampling rate of fs = 100 Hz. The IMU and Vicon data streams are
electronically time-synchronized. The Vicon system measures the position of four reflective markers
(diameter: 14 mm) arranged at the corners of the plate to form a square with side 45 cm. The accuracy
of the marker frame is determined by analyzing the relative motion of two markers: the RMS of the
Vicon orientation error Δθ is assumed in the same order as the RMS distance variation divided by the
distance between two markers (Δθ < 0.5°). The RMSEs of the Euler angles and the RMSEq of the
orientation are presented using the Euler angles time functions computed by the Vicon system as the
truth reference. The filter parameter setting we choose for the experimental validation is reported in
Table 2. The orientation sensor delivers the data from the magnetic sensor in arbitrary units.

Table 2. EKF parameter setting (experimental validation).

Static Dynamic
σg , °/s 0.4 0.4
b
σg , °/s2 0.01 0.01
−3
b
σh , u.a./s (×10 ) 10 10
−1
α,s 10 10
σa ,
mg 1 5
σ h , mGauss 1 1

4. Results

4.1. Computer Simulations

The Monte Carlo results are reported in Table 3. The EKF-MA performs better than the EKF-MB in
a magnetically markovian environment, according to a paired-sample t-test (statistically significant
difference, SSD at p < 0.001 and p < 0.01, in dynamic and static conditions). In the case of null
magnetic variations, the EKF-MA performs as the EKF-MB. Figure 1 depicts the typical behavior in
dynamic conditions. As for the second condition of observability, the EKF-MA is tested in the same
conditions leading to the data in Figure 1, for different values of the magnetic dip angle, Figure 2.
Sensors 2011, 11 9197

Table 3. Monte Carlo simulation results (mean ± SD); ** SSD at p < 0.01; *** SSD at p < 0.001.
Condition: MPE
EKF-MA
Static test 0.93 ± 0.24 ***
Dynamic 1.05 ± 0.24 **
EKF-MB
Static test 1.27 ± 0.18 ***
Dynamic 1.53 ± 0.21 **
Condition: MCE
EKF-MA
Static test 0.29 ± 0.11
Dynamic 0.32 ± 0.08
EKF-MB
Static test 0.22 ± 0.11
Dynamic 0.24 ± 0.15

Figure 1. (a)–(e) State errors from the EKF. (f) Heading estimation error. (a)–(c)
Quaternion components q1, q2, q3. (d) Magnetic variation Y-component. (e) Gyro bias
Z-component. The black lines show the 3 standard deviation bounds estimated by the filter.

(a) (b)

(c) (d)

(e) (f)
Sensors 2011, 11 9198

Figure 2. Orientation RMSE vs. magnetic dip angle.

4.2. Experimental Validation: Static Test

In Figure 3 we plot, in blue, the component of the magnetic field along the X-direction (earth-fixed
frame), as is measured by the magnetic sensor. Since the test environment is almost magnetically
unperturbed before placing the cell phone nearby the IMU at time t1 = 476 s, the measured magnetic
field is the true magnetic field existing in the room with very good approximation. The EKF-MA is
tested using three different values of the process noise standard deviation bσ h .

Figure 3. The time function of the X-component of the magnetic field, estimated by the
EKF-MA. In blue the time function of the measured X-component of the magnetic field
(see text).

All the other parameters being the same as in Table 2, these values are: bσ h = 0.001 a.u./s (red);
b
σ h = 0.01 a.u./s (black); bσ h = 0.1 a.u./s (magenta). The RMS value of the difference between
estimated and measured X-components of the magnetic field up to time t1 increases from 15 × 10−5 a.u.
( bσ h = 0.001 a.u./s) to 8.4 × 10−3 a.u. ( bσ h = 0.1 a.u./s). The magnetic disturbance from t1 to when the
cell phone comes to a stop at time t2 = 480 s is best tracked with higher values of bσ h . However, when
the magnetic field is not time variant anymore from time t2 on, the filter behavior is unsatisfactory. The
estimated heading angle is reported in Figure 4.
Sensors 2011, 11 9199

Figure 4. Heading angle time function estimated by the EKF-MA and by the Xsens-EKF
( σ h = 0.01 a.u./s).
b

4.3. Experimental Validation: Dynamic Test

The norm of the acceleration magnitude is 1 g ± 60 mg throughout the trial, and the coefficient of
variation of the norm of the measured magnetic field is about 1%. The RMSE of the Euler angles are
reported in Table 4 for the EKF-MA and the EKF-MB. As a matter of comparison, we also report the
errors statistics in three other cases: (a) when only the gyro angular velocities are used and the aiding
sensors are disabled; (b) when only the accelerometer and magnetic sensor data are involved in
determining the orientation using the TRIAD method; (c) when the orientation is estimated by the
Xsens-EKF. The Euler angle time functions estimated by the Vicon system are reported with
superimposed the errors incurred by the EKF-MA in Figure 5.

Table 4. RMSE statistics—EKF vs. competing filter implementations.

EKF-MA EKF-MB Only gyro TRIAD Xsens-EKF
Roll, ° 0.89 1.02 2.14 2.15 0.92
Pitch, ° 0.97 0.93 3.85 4.49 0.71
Yaw, ° 1.14 1.18 7.62 5.81 10.48
Orientation,° 1.63 1.69 8.68 7.44 10.60

Figure 5. Euler angles time functions (truth reference and EKF-MA estimation errors).
Sensors 2011, 11 9200

Figure 5. Cont.

4. Discussion and Concluding Remarks

The conditions for the observability of the KF-based algorithm for IMU orientation tracking are
based on the rank-condition of the observability matrix Equation (39). These conditions require that
the local magnetic field, which may include magnetic variations superimposed to the earth magnetic
field, and the gravity vector are not collinear; moreover, at least one rotational degree of freedom
would excite the system. The computer simulations clearly show the relevance of the first condition.
Indeed, when the magnetic dip angle becomes too large, the heading estimation accuracy undergoes an
impressive degradation. Conversely, the computer simulations, and experimental testing as well,
demonstrate that the condition concerning the IMU motion is not critical. In static conditions, the filter
is stable and its performances are similar to those achieved in dynamic conditions. In general, we
observe some problems of filter consistency. In particular, difficulties exist in picking up the third
component of the quaternion and the horizontal magnetic states, as shown in Figure 1(c–d). The errors
in these estimated state vector components turn out to be highly correlated in time, which is not
reflected in the expression of the computed error state covariance matrix; moreover, while the
compensation of gyro bias is generally fast, some possible problems exist for its Z-component,
Figure 1(e). These behaviors are not unexpected: the estimated values of heading, horizontal
component of the acting magnetic field and gyro bias component in the vertical direction must
necessarily interact in the attempt to explain the information available to the filter. In other words, the
observability conditions can never be too good. However, a safe start in a magnetic homogeneous
Sensors 2011, 11 9201

area [7], and standing still few seconds to carry out the IMU alignment [8] are useful expedients to
ensure the validity of the linear approximation of the IMU orientation tracking process. Some
disadvantages of the linearization procedure implemented in an EKF are indeed sensitivity to initial
conditions, biases in the estimation errors, and critical problems of convergence and filter stability,
especially when the sampling interval is too large [28].
The results of the experimental validation support the results of the Monte Carlo study, either in the
static case or in dynamic conditions. The importance of implementing sensor fusion algorithms is
stressed by the results offered in Table 5. Presently, the reason why the yaw estimates from the
Xsens-EKF start to diverge after about 40 s is unknown. The advantage of the compensation of the
magnetic disturbances is shown in a situation where the level of contamination is low. It should be
noted the importance of providing the filter with some sort of adaptability to the magnetic
environment. In this regard, the results of the validation test in static conditions gives rise to several
interesting considerations. Since the magnetic variations are quite small, the mechanism of magnetic
disturbance compensation is on, with a small value of the process noise standard deviation. This helps
making the filter stable; however, its capability of dealing with the magnetic disturbance, when it
occurs, is very limited. If, in the effort to deal with the magnetic disturbance, we increase the process
noise, the ability to follow the magnetic variations when they occur is remarkable, however the filter
may lose track when the magnetic variations are small. On one hand, the injection of process noise is
known as a useful recipe to account for mismodeling problems, and even to make the filter response
faster [30]; on the other hand, excess process noise is also responsible for less accurate state estimates
in benign tracking conditions.
The importance of implementing sensor fusion algorithms is further outlined by the results reported
in Table 4. Two further implementations are considered: “only gyro” refers to the case when the EKF
is unaided by the accelerometer and the magnetic sensor, and the orientation information is obtained
exclusively by time-integrating noisy gyro signals from initial conditions we assume, for simplicity, to
be known at the same level of accuracy as in EKF-MA and EKF-MB (we ignore the details of the
Xsens’ initialization procedure). This can be obtained by plugging high values of the accelerometer
and magnetic sensor measurement noise standard deviation into the filter matrix R. On the contrary,
the column named “gyro-free” in Table 4 refers to the case when a gyro-free measurement unit is
considered. In this case, only the accelerometer and the magnetic sensor are used: their information is
combined while solving a specific instance of the Wahba’s problem using the TRIAD algorithm,
which is often used to combine measured gravity and Earth magnetic field vectors, for slow motions
occurring in magnetically clean environments. Either for the “only gyro” or the “gyro-free” systems, it
is worth noting that the stabilization of inclination (roll and pitch) and heading (yaw) is achieved just
to a limited extent. In the “only gyro” system, the limited stabilization is a consequence of the random-
walk integration of wideband gyro measurement noise, while, in the “gyro-free” system, it is a
consequence of the fact that, being a single-frame algorithm for orientation determination, the TRIAD
per se does not smooth the estimates it produces: even small tremulous movements affecting the
accelerometer and any magnetic disturbance in the environment combine their effects to produce noisy
orientation estimates. Although the “only gyro” system seems to be almost equivalent to the
“gyro-free” system in this particular experimental trial, it must be remembered that random-walk
errors tend to grow unbounded over time, while the high-frequency noise affecting the “gyro-free”
Sensors 2011, 11 9202

system is virtually drift-free. However, the “gyro-free” system is completely blind and vulnerable to
abrupt changes in the body motion and to magnetic variations in the environment. On the other hand,
the EKF is capable of making the best use of the available information, and provides outstanding
performance as compared with the “only gyro” and the “gyro-free” systems. Although having a limited
ability to overcome magnetic variations, as shown in the static test results, the EKF we have developed
may be protected against the effects of body motion using vector selection schemes such as those
implemented in [2], which make it adaptive to some extent (R-adaptation, or adaptation of the
measurement noise covariance matrix R is involved in [2]). These schemes are not considered here. An
interesting avenue of research will concern how to make the EKF Q-adaptive, namely how the process
noise covariance matrix Q may be changed at run-time to reflect varying magnetic conditions. These
considerations stress the importance of developing adaptive EKFs: this is beyond the scope of this
paper and it is the subject of our current research. At the present time, we are investigating the
rank-condition of observability matrices built from gradients of the Lie derivatives in our research on
adaptive EKFs that aims at the compensation of magnetic disturbances in strongly perturbed
environments.
The main contribution of this paper concerns the presentation of a method for testing the
observability of KFs applied to a problem of orientation determination using inertial/magnetic sensors.
Oftentimes, KFs are developed exploiting the considerable freedom in choosing the state and
measurement equations, without paying proper consideration to possible problems of observability.
This is the first time the observability rank criterion based on Lie derivatives is applied to test the
observability of a quaternion-based EKF that is developed for determining the orientation of a rigid
body using inertial/magnetic sensors. The formal demonstration of the conditions of observability and,
in addition to that, the results of a set of Monte Carlo computer simulations and few experimental trials
are offered in support of our approach.

Acknowledgments

This work has been supported by funds from the Italian Ministry of University and Research. The
author is indebted to Andrea Mannini for his assistance in performing the experiments.

References

1. Welch, G.F.; Foxlin, E. Motion tracking: No silver bullet, but a respectable arsenal. IEEE
Comput. Graph. Appl. 2002, 22, 24-38.
2. Sabatini, A.M. Quaternion-based extended Kalman filter for determining orientation by inertial
and magnetic sensing. IEEE Trans. Biomed. Eng. 2006, 53, 1346-1356.
3. Yun, X.; Bachmann, E.R.; McGhee, R.B. A simplified quaternion-based algorithm for orientation
estimation from earth gravity and magnetic field measurements. IEEE Trans. Instrum. Meas.
2008, 57, 638-650.
4. Lee, J.K.; Park, E.J. A fast quaternion-based orientation optimizer via virtual rotation for human
motion tracking. IEEE Trans. Biomed. Eng. 2009, 56, 1574-1582.
Sensors 2011, 11 9203

5. Veltink, P.H.; Bussmann, H.B.J.; de Vries, W.H.K.; Martens, W.L.J.; Van Lummel, R.C.
Detection of static and dynamic activities using uniaxial accelerometers. IEEE Trans. Rehabil.
Eng. 1996, 4, 375-385.
6. Bachmann, E.R.; Yun, X.; Peterson, C.W. An investigation of the effects of magnetic variations
on inertial/magnetic orientation sensors. In Proceedings of IEEE International Conference on
Robotics and Automation (ICRA '04), New Orleans, LA, USA, 26 April–1 May 2004;
pp. 1115-1122.
7. de Vries, W.H.K.; Veeger, H.E.J.; Baten, C.T.M.; van der Helm, F.C.T. Magnetic distortion in
motion labs, implications for validating inertial magnetic sensors. Gait Posture 2009, 29,
535-541.
8. Sabatini, A.M. Estimating three-dimensional orientation of human body parts by inertial/magnetic
sensing. Sensors 2011, 11, 1489-1525.
9. Gallagher, A.; Matsuoka, Y.; Wei-Tech, A. An efficient real-time human posture tracking
algorithm using low-cost inertial and magnetic sensors. In Proceedings of IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS 2004), Sendai, Japan, 28 September–2
October 2004; pp. 2967-2972.
10. Wahba, G. A least-square estimate of spacecraft attitude. SIAM Rev. 1965, 7, 409.
11. Gebre-Egziabher, D.; Elkaim, G.H.; Powell, J.D.; Parkinson, B.W. A gyro-free quaternion-based
attitude determination system suitable for implementation using low cost sensors. In Proceedings
of IEEE Position Location and Navigation Symposium (PLANS '00), San Diego, CA, USA, 13–16
March 2000; pp. 185-192.
12. Markley, F.L. Attitude determination and parameter estimation using vector observations: Theory.
J. Astronaut. Sci. 1989, 37, 41-58.
13. Welch, G.F. The use of the Kalman filter for human motion tracking in virtual reality. Presence
2009, 18, 72-91.
14. Brown, R.G.; Hwang, P.Y.C. Introduction to Random Signals and Applied Kalman Filtering;
John Wiley & Sons: New York, NY, USA, 1997.
15. Foxlin, E. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter.
In Proceedings of IEEE Virtual Reality Annual International Symposium, Santa Clara, CA, USA,
30 March–3 April 1996; pp. 185-194.
16. Luinge, H.J.; Veltink, P.H. Inclination measurement of human movement using a 3-D
accelerometer with autocalibration. IEEE Trans. Neur. Syst. Rehabil. Eng. 2004, 12, 112-121.
17. Roetenberg, D.; Luinge, H.J.; Baten, C.T.M.; Veltink, P.H. Compensation of magnetic
disturbances improves inertial and magnetic sensing of human body segment orientation. IEEE
Trans. Neur. Syst. Rehabil. Eng. 2005, 13, 395-405.
18. Yun, X.; Bachmann, E.R. Design, implementation, and experimental results of a quaternion-based
Kalman filter for human body motion tracking. IEEE Trans. Robot. 2006, 22, 1216-1227.
19. Lefferts, E.J.; Markley, F.L.; Shuster, M.D. Kalman filtering for spacecraft attitude estimation.
J. Guid. 1982, 5, 417-429.
20. Harada, T.; Mori, T.; Sato, T. Development of a tiny orientation estimation device to operate
under motion and magnetic disturbance. Int. J. Rob. Res. 2007, 26, 547-559.
Sensors 2011, 11 9204

21. Tomé, P.; Yalak, O. Improvement of orientation estimation in pedestrian navigation by

compensation of magnetic disturbances. J. Navig. 2008, 55, 179-190.
22. Foxlin, E.; Harrington, M.; Altshuler, Y. Miniature 6-DOF inertial system for tracking HMDs.
SPIE Proc. 1998, 3362, 214-228.
23. Mirzaei, F.M.; Roumeliotis, S.I. A Kalman-filter-based algorithm for IMU-camera calibration:
Observability analysis and performance evaluation. IEEE Trans. Robot. 2008, 24, 1143-1156.
24. Shuster, M.D. A survey of attitude representations. J. Astronaut. Sci. 1993, 41, 439-517.
25. Isidori, A. Nonlinear Control Systems; Springer Verlag: London, UK, 1995.
26. Ferraris, F.; Grimaldi, U.; Parvis, M. Procedure for effortless in-field calibration of three-axis rate
gyros and accelerometers. Sens. Mater. 1995, 7, 311-330.
27. Gebre-Egziabher, D.; Elkaim, G.H.; Powell, J.D.; Parkinson, B.W. A non-linear, two-step
estimation algorithm for calibrating solid-state strapdown magnetometers. In Proceedings of 8th
International Conference on Navigation Systems, St. Petersburg, Russia, 28–30 May 2001.
28. Crassidis, J.L.; Markley, F.L. Unscented filtering for spacecraft attitude estimation. J. Guid.
Contr. Dynam. 2003, 26, 536-542.
29. Sabatini, A.M. Quaternion-based strap-down integration method for applications of inertial
sensing to gait analysis. Med. Biol. Eng. Comput. 2005, 43, 94-101.
30. Maybeck, P.S. Stochastic Models, Estimation and Control; Academic Press: New York, NY,
USA, 1982.
31. Markley, F.L. Attitude error representations for Kalman filtering. J. Guid. Contr. Dynam. 2003,
26, 311-317.
32. Choukroun, D.; Bar-Itzhack, I.Y.; Oshman, Y. Novel quaternion Kalman filter. IEEE Trans.
Aerosp. Electron. Syst. 2006, 41, 174-190.
33. Shuster, M.D.; Oh, S. Three-axis attitude determination from vector observations. J. Guid. Contr.
1981, 4, 70-77.

Appendix

We prove the conditions under which the 6 × 3 matrix A is full rank:

⎡ ⎡ q4 [p1 ×] + ([p1 ×][q ×] − 2 [q ×][p1 ×]) − [q ×] p1 ⎤ ⎡ s1 s 2 s3 ⎤ ⎤
⎣ ⎦⎣ ⎦⎥
A=⎢ (A1)
⎣ ⎣ 4 [ 2 ] ([ 2 ][ ] [ ][ 2 ])
⎢ ⎡q p × + p × q × − 2 q × p × − [q ×] p 2 ⎦⎤ ⎡⎣ s1 s 2 s3 ⎤⎦ ⎥
⎦
T
Be p1 = [ 0 0 g z ] and p 2 = ⎡⎣ hx hy hz ⎤⎦ . Let us consider elementary rotations about the three principal
T

axes (roll, pitch and yaw). A script written using the symbolic computation software MAPLE helps
proving that minors of order three can always be found with determinant values
roll: cos ϕ hx ⋅ g z2 , sin ϕ hx ⋅ g z2 , cos ϕ hy ⋅ g z2 , sin ϕ hy ⋅ g z2
pitch: cos ϑ hx ⋅ g z2 , sin ϑ hx ⋅ g z2 , cos ϑ hy ⋅ g z2 , sin ϑ hy ⋅ g z2 (A2)
yaw: (cosψ hx + sinψ hy ) g , (− sinψ hx + cosψ hy ) g
2
z
2
z
Sensors 2011, 11 9205

The determinant values cannot be simultaneously zero for any elementary rotation, provided that the
local magnetic field has components in the horizontal plane, i.e., hx, hy or both are different from zero,
condition that can be considered always true, for all practical purposes.
Next, we prove the rank conditions for the 10 × 7 matrix B, for which we have:
⎡ 0 0 0 0 ⎤ 1⎫
⎢ 2(−q p + q p − q p ) ⎪
⎢ 1 3 3 1 4 2 2(−q2 p3 + q3 p2 + q4 p1 ) 2(+ q1 p1 + q2 p2 + q3 p3 ) 2(−q1 p2 + q2 p1 + q4 p3 ) ⎥⎥ 2 ⎬ ωx
⎢ 2(+ q1 p2 − q2 p1 − q4 p3 ) 2(−q1 p1 − q2 p2 − q3 p3 ) 2(−q2 p3 + q3 p2 + q4 p1 ) 2(−q1 p3 + q3 p1 − q4 p2 ) ⎥ 3⎪
⎭
⎢ ⎥
⎢ 2(+ q1 p3 − q3 p1 + q4 p2 ) 2(+ q2 p3 − q3 p2 − q4 p1 ) 2(−q1 p1 − q2 p2 − q3 p3 ) 2(+ q1 p2 − q2 p1 − q4 p3 ) ⎥ 4⎫
⎪
Γ ( q, p ) = ⎢ 0 0 0 0 0⎥ 5⎬ωy
(A3)
⎢ ⎥
⎢ 2( + q p
1 1 + q 2 p2 + q3 p3 ) 2(+ q1 p2 − q2 p1 − q4 p3 ) 2(+ q1 p3 − q3 p1 + q4 p2 ) 2(−q2 p3 + q3 p2 + q4 p1 ) ⎥ 6⎪
⎭
⎢ 2(−q p + q p + q p ) 2(+ q1 p1 + q2 p2 + q3 p3 ) 2(+ q2 p3 − q3 p2 − q4 p1 ) 2(+ q1 p3 − q3 p1 + q4 p2 ) ⎥ 7⎫
⎢ 1 2 2 1 4 3
⎥ ⎪
⎢ 2(−q1 p1 − q2 p2 − q3 p3 ) 2(−q1 p2 + q2 p1 + q4 p3 ) 2(−q1 p3 + q3 p1 − q4 p2 ) 2(+ q2 p3 − q3 p2 − q4 p1 ) ⎥ 8 ⎬ ωz
⎢ ⎥
⎣ 0 0 0 0 ⎦ 9⎪
⎭

⎡ 0 0 0 ⎤ 1⎫
⎢ 2(q q + q q ) 2(−q q + q q ) 1 − 2q 2 − 2q 2 ⎥ ⎪
2 ⎬ ωx
⎢ 1 3 2 4 1 4 2 3 1 2 ⎥
⎢ 2(−q 1 q2 + q3 q4 ) 1 − 2q22 − 2q42 −2(q 1 q4 + q2 q3 ) ⎥ ⎪
3⎭
⎢ ⎥
⎢ 2(−q 1 q3 − q2 q4 ) 2(q 1 q4 − q2 q3 ) 1 − 2q32 − 2q42 ⎥ 4⎫
⎪
ϒ (q) = ⎢ 0 0 0 ⎥ 5⎬ωy
(A4)
⎢ ⎥
6⎪
2 2
⎢ 1 − 2q2 − 2q3 2(q1q2 + q3 q4 ) 2(q1q3 − q2 q4 ) ⎥ ⎭
⎢ 2(q q − q q ) 1 − 2q12 − 2q32 2(q 1 q4 + q2 q3 ) ⎥ 7⎫
⎢ 1 2 3 4
⎥ ⎪
⎢ 1 − 2q1 − 2q4 −2(q1q2 + q3 q4 ) 2(−q1q3 + q2 q4 ) ⎥ 8 ⎬ ωz
2 2

⎢ ⎥
⎣ 0 0 0 ⎦ 9⎪
⎭

The variables shown right next to the row numbers indicate the control input that is involved in using
the corresponding row numbers: when a row is retained in the observability matrix (e.g., the 4th row),
the underlying assumption is that the corresponding control input (i.e., ωy) is non-zero. Note that, for
either Γ or ϒ , rows 2 and 4; 3 and 7; 6 and 8 are equal, apart the sign. Rows 3, 4, 8 (or any other
combination of rows that involve, at least, one control input) can be extracted from ϒ to form the
reduced matrix
⎡ 2 ( −q 1 q2 + q3q4 ) 1 − 2q22 − 2q42 −2 ( q 1 q4 + q2 q3 ) ⎤
⎢ ⎥
ϒ red ( q ) = ⎢ 2 ( −q 1 q3 − q2 q4 ) 2 ( q 1 q4 − q2 q3 ) 1 − 2q32 − 2q42 ⎥ (A5)
⎢ 1 − 2q12 − 2q42 −2 ( q 1 q2 + q3q4 ) 2 ( −q 1 q3 + q2 q4 ) ⎥⎦
⎣
It should be noted that:
ϒ red ( q ) ϒ Tred ( q ) = I 3×3 (A6)
Hence ϒ red (q) is seen to belong to SO(3). It is immediate to conclude that the matrix extracted from B
by retaining rows 3, 4, 8 and 10, namely:
⎡ 2(+ q1 p2 − q2 p1 − q4 p3 ) 2(−q1 p1 − q2 h2 − q3 p3 ) 2( −q2 p3 + q3 p2 + q4 p1 ) 2(− q1 p3 + q3 p1 − q4 p2 ) ⎤
⎢ 2(+ q p − q p + q p ) 2( + q p − q p − q p ) 2(− q p − q p − q p ) 2(+ q p − q p − q p ) ⎥
ϒ ( q )
B red =⎢ 1 3 3 1 4 2 2 3 3 2 4 1 1 1 2 2 3 3 1 2 2 1 4 3 red ⎥
(A7)
⎢ 2( −q1 p1 − q2 p2 − q3 p3 ) 2( −q1 p2 + q2 p1 + q4 p3 ) 2(− q1 p3 + q3 p1 − q4 p2 ) 2(+ q2 p3 − q3 p2 − q4 p1 ) ⎥
⎢ ⎥
⎣ 2q1 2q2 2q3 2q4 0 0 0⎦

is rank 4 if the quaternion is not identically equal to zero, which is true if the quaternion has to
represent a valid rotation. We conclude that the motion would be one-dimensional at least, to excite all
rows needed for the matrix Bred to be rank 4; hence the matrix B is rank 4 under the same conditions.
Sensors 2011, 11 9206

By performing Gaussian elimination on the corresponding rows of O and eliminating all the
elements of the involved columns we obtain:
⎡ 03×4 03×3 03×3 ⎤
⎢0 B
C (q ) 03×3 ⎥⎥
⎢ 3×4 E

⎢0 03×3 A11 ⎥
O = ⎢ 3×4 ⎥ (A8)
⎢ 03×4 X3 A12 ⎥
⎢ I 4×4 04×3 04×3 ⎥
⎢ ⎥
⎣⎢06×4 06×3 06×3 ⎦⎥

By performing Gaussian elimination on the rows of the matrix A, which is of rank 3, we get:
⎡03×4 03×3 03×3 ⎤
⎢0 B
C (q) 03×3 ⎥⎥
⎢ 3×4 E

⎢0 03×3 I 3×3 ⎥
O = ⎢ 3×4 ⎥ (A8)
⎢03×4 03×3 03×3 ⎥
⎢ I 4×4 04×3 04×3 ⎥
⎢ ⎥
⎣⎢03×4 03×3 03×3 ⎦⎥

Since a valid DCM is non-singular, we conclude that the observability matrix is full rank when: (a)
the magnetic field and the gravity vectors are not collinear; (b) at least one degree of rotational
freedom is excited.

© 2011 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article
distributed under the terms and conditions of the Creative Commons Attribution license
(http://creativecommons.org/licenses/by/3.0/).