309 views

Uploaded by Moatasem Momtaz

save

- Aviation Legal Terms
- Visnav Lecture Notes
- 10.1.1.119
- Full Text 01
- Module 12E
- Error Reduction Techniques for a MEMS Accelerometer-based Digital Input Device
- syllabi5th6Sep2016
- Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing
- JEC2 paper 5-1
- APM Autopilot setup using simulink
- Am or Us Op a Per 1
- UAV_Sim.pdf
- Quadcopter Dynamics, Simulation, And Control
- EN-SET-116(2011)-03
- A Efficient Orientation Filter for IMUs and MARG Sensor Arrays
- Lifting Analysis for Heavy Ship Hull Blocks Using 4 Cranes
- USLI Presentation - Task 7
- 01338525
- Orientation Histogram
- Single Shooting and Multiple Shooting
- Reflex Gyro Brochure
- Aula 04 Din & Cont Vei Espaciais_English
- II CM _SY_110112115544
- IRI Introduction
- U Frame Support User Documentation
- 333
- caplib
- tmpA038
- Design Reference Manual - Creating the Model
- Movie Monitor
- 6 an Integrated GPS-MEMS-IMU Navigation System for an Autonomous Helicopter
- Development of Autonomous Unmanned Aerial
- Rugged Computing
- Mtcr Handbook

You are on page 1of 32

**inertial/magnetic sensor arrays
**

Sebastian O.H. Madgwick

April 30, 2010

Abstract

This report presents a novel orientation ﬁlter applicable to IMUs consisting of

tri-axis gyroscopes and accelerometers, and MARG sensor arrays that also include

tri-axis magnetometers. The MARG implementation incorporates magnetic distortion

and gyroscope bias drift compensation. The ﬁlter uses a quaternion representation,

allowing accelerometer and magnetometer data to be used in an analytically derived

and optimised gradient-descent algorithm to compute the direction of the gyroscope

measurement error as a quaternion derivative. The beneﬁts of the ﬁlter include: (1)

computationally inexpensive; requiring 109 (IMU) or 277 (MARG) scalar arithmetic

operations each ﬁlter update, (2) eﬀective at low sampling rates; e.g. 10 Hz, and (3)

contains 1 (IMU) or 2 (MARG) adjustable parameters deﬁned by observable system

characteristics. Performance was evaluated empirically using a commercially available

orientation sensor and reference measurements of orientation obtained using an optical

measurement system. A simple calibration method is presented for the use of the

optical measurement equipment in this application. Performance was also benchmarked

against the propriety Kalman-based algorithm of orientation sensor. Results indicate

the ﬁlter achieves levels of accuracy exceeding that of the Kalman-based algorithm;

< 0.6

◦

static RMS error, < 0.8

◦

dynamic RMS error. The implications of the low

computational load and ability to operate at low sampling rates open new opportunities

for the use of IMU and MARG sensor arrays in real-time applications of limited power

or processing resources or applications that demand extremely high sampling rates.

1

Contents

1 Introduction 3

2 Quaternion representation 4

3 Filter derivation 6

3.1 Orientation from angular rate . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.2 Orientation from vector observations . . . . . . . . . . . . . . . . . . . . . . 6

3.3 Filter fusion algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.4 Magnetic distortion compensation . . . . . . . . . . . . . . . . . . . . . . . . 11

3.5 Gyroscope bias drift compensation . . . . . . . . . . . . . . . . . . . . . . . 12

3.6 Filter gains . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4 Experimentation 14

4.1 Equipment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4.2 Orientation from optical measurements . . . . . . . . . . . . . . . . . . . . . 14

4.3 Calibration of frame alignments . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.4 Experimental proceedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5 Results 19

5.1 Typical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

5.2 Static and dynamic performance . . . . . . . . . . . . . . . . . . . . . . . . 21

5.3 Filter gain vs. performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.4 Sampling rate vs. performance . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.5 Gyroscope bias drift . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

6 Discussion 24

7 Conclusions 25

A IMU ﬁlter implementation optimised in C 29

B MARG ﬁlter implementation optimised in C 30

2

1 Introduction

The accurate measurement of orientation plays a critical role in a range of ﬁelds includ-

ing: aerospace [1, 2, 3], robotics [4, 5], navigation [6, 7] and human motion analysis [8, 9]

and machine interaction [10]. Whilst a variety of technologies enable the measurement of

orientation, inertial based sensory systems have the advantage of being completely self con-

tained such that the measurement entity is constrained neither in motion nor to any speciﬁc

environment or location. An IMU (Inertial Measurement Unit) consists of gyroscopes and

accelerometers enabling the tracking of rotational and translational movements. In order to

measure in three dimensions, tri-axis sensors consisting of 3 mutually orthogonal sensitive

axes are required. A MARG (Magnetic, Angular Rate, and Gravity) sensor is a hybrid IMU

which incorporates a tri-axis magnetometer. An IMU alone can only measure an attitude

relative to the direction of gravity which is suﬃcient for many applications [4, 2, 8, 1]. MARG

systems, also known as AHRS (Attitude and Heading Reference Systems) are able to provide

a complete measurement of orientation relative to the direction of gravity and the earth’s

magnetic ﬁeld.

A gyroscope measures angular velocity which, if initial conditions are known, may be inte-

grated over time to compute the sensor’s orientation [11, 12]. Precision gyroscopes, ring laser

for example, are too expensive and bulky for most applications and so less accurate MEMS

(Micro Electrical Mechanical System) devices are used in a majority of applications [13]. The

integration of gyroscope measurement errors will lead to an accumulating error in the calcu-

lated orientation. Therefore, gyroscopes alone cannot provide an absolute measurement of

orientation. An accelerometer and magnetometer will measure the earth’s gravitational and

magnetic ﬁelds respectively and so provide an absolute reference of orientation. However,

they are likely to be subject to high levels of noise; for example, accelerations due to motion

will corrupt measured direction of gravity. The task of an orientation ﬁlter is to compute

a single estimate of orientation through the optimal fusion of gyroscope, accelerometer and

magnetometer measurements.

The Kalman ﬁlter [14] has become the accepted basis for the majority of orientation ﬁlter

algorithms [4, 15, 16, 17] and commercial inertial orientation sensors; xsens [18], micro-strain

[19], VectorNav [20], Intersense [21], PNI [22] and Crossbow [23] all produce systems founded

on its use. The widespread use of Kalman-based solutions are a testament to their accuracy

and eﬀectiveness, however, they have a number of disadvantages. They can be complicated to

implement which is reﬂected by the numerous solutions seen in the subject literature [3, 4, 15,

16, 17, 24, 25, 26, 27, 28, 29, 30, 31, 32]. The linear regression iterations, fundamental to the

Kalman process, demand sampling rates far exceeding the subject bandwidth; for example, a

sampling rate between 512 Hz [18] and 30 kHz [19] may be used for a human motion caption

application. The state relationships describing rotational kinematics in three-dimensions

typically require large state vectors and an extended Kalman ﬁlter implementation [4, 17, 24]

to linearise the problem.

These challenges demand a large computational load for implementation of Kalman-

based solutions and provide a clear motivation for alternative approaches. Many previous

approaches to address these issues have implemented either fuzzy processing [2, 5] or ﬁxed

ﬁlters [33] to favour accelerometer measurements of orientation at low angular velocities and

the integrated gyroscope measurements at high angular velocities. Such an approach is simple

3

but may only be eﬀective under limited operating conditions. Bachman et al [34] proposed

an alternative approach where the ﬁlter achieves an optimal fusion of measurements data at

all angular velocities. However, the process requires a least squares regression, which also

brings in an associated computational load. Mahony et al [35] developed the complementary

ﬁlter which is shown to be an eﬃcient and eﬀective solution; however, performance is only

validated for an IMU.

This report introduces novel orientation ﬁlter that is applicable to both IMUs and MARG

sensor arrays addressing issues of computational load and parameter tuning associated with

Kalman-based approaches. The ﬁlter employs a quaternion representation of orientation (as

in: [34, 17, 24, 30, 32]) to describe the coupled nature of orientations in three-dimensions and

is not subject to the problematic singularities associated with an Euler angle representation

1

.

A complete derivation and empirical evaluation of the new ﬁlter is presented. Its performance

is benchmarked against an existing commercial ﬁlter and veriﬁed with optical measurement

system. Innovative aspects of the proposed ﬁlter include: a single adjustable parameter

deﬁned by observable systems characteristics; an analytically derived and optimised gradient-

descent algorithm enabling performance at low sampling rates; an on-line magnetic distortion

compensation algorithm; and gyroscope bias drift compensation.

2 Quaternion representation

A quaternion is a four-dimensional complex number that can be used to represent the ori-

entation of a ridged body or coordinate frame in three-dimensional space. An arbitrary

orientation of frame B relative to frame A can be achieved through a rotation of angle θ

around an axis

A

ˆ r deﬁned in frame A. This is represented graphically in ﬁgure 1 where the

mutually orthogonal unit vectors ˆ x

A

, ˆ y

A

and ˆ z

A

, and ˆ x

B

, ˆ y

B

and ˆ z

B

deﬁne the principle axis

of coordinate frames A and B respectively. The quaternion describing this orientation,

A

B

ˆ q,

is deﬁned by equation (1) where r

x

, r

y

and r

z

deﬁne the components of the unit vector

A

ˆ r in

the x, y and z axes of frame A respectively. A notation system of leading super-scripts and

sub-scripts adopted from Craig [37] is used to denote the relative frames of orientations and

vectors. A leading sub-script denotes the frame being described and a leading super-script

denotes the frame this is with reference to. For example,

A

B

ˆ q describes the orientation of

frame B relative to frame A and

A

ˆ r is a vector described in frame A. Quaternion arithmetic

often requires that a quaternion describing an orientation is ﬁrst normalised. It is therefore

conventional for all quaternions describing an orientation to be of unit length.

A

B

ˆ q =

q

1

q

2

q

3

q

4

=

cos

θ

2

−r

x

sin

θ

2

−r

y

sin

θ

2

−r

z

sin

θ

2

(1)

The quaternion conjugate, denoted by

∗

, can be used to swap the relative frames described

by an orientation. For example,

B

A

ˆ q is the conjugate of

A

B

ˆ q and describes the orientation of

frame A relative to frame B. The conjugate of

A

B

ˆ q is deﬁned by equation (2).

A

B

ˆ q

∗

=

B

A

ˆ q =

q

1

−q

2

−q

3

−q

4

(2)

1

Kuipers [36] oﬀers a comprehensive introduction to this use of quaternions.

4

ˆ y

A

ˆ y

B

A

ˆ r

ˆ x

B

ˆ x

A

θ

ˆ z

A

ˆ z

B

Figure 1: The orientation of frame B is achieved by a rotation, from alignment with frame

A, of angle θ around the axis

A

r.

The quaternion product, denoted by ⊗, can be used to deﬁne compound orientations.

For example, for two orientations described by

A

B

ˆ q and

B

C

ˆ q, the compounded orientation

A

C

ˆ q

can be deﬁned by equation (3).

A

C

ˆ q =

B

C

ˆ q ⊗

A

B

ˆ q (3)

For two quaternions, a and b, the quaternion product can be determined using the

Hamilton rule and deﬁned as equation (4). A quaternion product is not commutative; that

is, a ⊗b = b ⊗a.

a ⊗b =

a

1

a

2

a

3

a

4

⊗

b

1

b

2

b

3

b

4

=

a

1

b

1

−a

2

b

2

−a

3

b

3

−a

4

b

4

a

1

b

2

+ a

2

b

1

+ a

3

b

4

−a

4

b

3

a

1

b

3

−a

2

b

4

+ a

3

b

1

+ a

4

b

2

a

1

b

4

+ a

2

b

3

−a

3

b

2

+ a

4

b

1

¸

¸

¸

¸

T

(4)

A three dimensional vector can be rotated by a quaternion using the relationship de-

scribed in equation (5) [36].

A

v and

B

v are the same vector described in frame A and frame

B respectively where each vector contains a 0 inserted as the ﬁrst element to make them 4

element row vectors.

B

v =

A

B

ˆ q ⊗

A

v ⊗

A

B

ˆ q

∗

(5)

The orientation described by

A

B

ˆ q can be represented as the rotation matrix

A

B

R deﬁned

by equation (6) [36].

A

B

R =

2q

2

1

−1 + 2q

2

2

2(q

2

q

3

+ q

1

q

4

) 2(q

2

q

4

−q

1

q

3

)

2(q

2

q

3

−q

1

q

4

) 2q

2

1

−1 + 2q

2

3

2(q

3

q

4

+ q

1

q

2

)

2(q

2

q

4

+ q

1

q

3

) 2(q

3

q

4

−q

1

q

2

) 2q

2

1

−1 + 2q

2

4

¸

¸

(6)

Euler angles ψ, θ and φ in the so called aerospace sequence [36] describe an orientation

of frame B achieved by the sequential rotations, from alignment with frame A, of ψ around

5

ˆ z

B

, θ around ˆ y

B

, and φ around ˆ x

B

. This Euler angle representation of

A

B

ˆ q is deﬁned by

equations (7), (8) and (9).

ψ = Atan2

2q

2

q

3

−2q

1

q

4

, 2q

2

1

+ 2q

2

2

−1

(7)

θ = −sin

−1

(2q

2

q

4

+ 2q

1

q

3

) (8)

φ = Atan2

2q

3

q

4

−2q

1

q

2

, 2q

2

1

+ 2q

2

4

−1

(9)

3 Filter derivation

3.1 Orientation from angular rate

A tri-axis gyroscope will measure the angular rate about the x, y and z axes of the senor

frame, termed ω

x

, ω

y

and ω

z

respectively. If these parameters (in rads

−1

) are arranged into

the vector

S

ω deﬁned by equation (10), the quaternion derivative describing the rate of

change of orientation of the earth frame relative to the sensor frame

S

E

˙ q can be calculated

[38] as equation (11).

S

ω =

0 ω

x

ω

y

ω

z

(10)

S

E

˙ q =

1

2

S

E

ˆ q ⊗

S

ω (11)

The orientation of the earth frame relative to the sensor frame at time t,

E

S

q

ω,t

, can

be computed by numerically integrating the quaternion derivative

S

E

˙ q

ω,t

as described by

equations (12) and (13) provided that initial conditions are known. In these equations,

S

ω

t

is the angular rate measured at time t, ∆t is the sampling period and

S

E

ˆ q

est,t-1

is the previous

estimate of orientation. The sub-script ω indicates that the quaternion is calculated from

angular rates.

S

E

˙ q

ω,t

=

1

2

S

E

ˆ q

est,t-1

⊗

S

ω

t

(12)

S

E

q

ω,t

=

S

E

ˆ q

est,t-1

+

S

E

˙ q

ω,t

∆t (13)

3.2 Orientation from vector observations

A tri-axis accelerometer will measure the magnitude and direction of the ﬁeld of gravity in the

sensor frame compounded with linear accelerations due to motion of the sensor. Similarly, a

tri-axis magnetometer will measure the magnitude and direction of the earth’s magnetic ﬁeld

in the sensor frame compounded with local magnetic ﬂux and distortions. In the context

of an orientation ﬁlter, it will initially be assumed that an accelerometer will measure only

gravity, a magnetometer will measure only the earth’s magnetic ﬁeld.

6

If the direction of an earth’s ﬁeld is known in the earth frame, a measurement of the

ﬁeld’s direction within the sensor frame will allow an orientation of the sensor frame relative

to the earth frame to be calculated. However, for any given measurement there will not be

a unique sensor orientation solution, instead there will inﬁnite solutions represented by all

those orientations achieved by the rotation the true orientation around an axis parallel with

the ﬁeld. In some applications it may be acceptable to use an Euler angle representation

allowing an incomplete solution to be found as two known Euler angles and one unknown [5];

the unknown angle being the rotation around an axis parallel with the direction of the ﬁeld.

A quaternion representation requires a complete solution to be found. This may be achieved

through the formulation of an optimisation problem where an orientation of the sensor,

S

E

ˆ q,

is that which aligns a predeﬁned reference direction of the ﬁeld in the earth frame,

E

ˆ

d,

with the measured direction of the ﬁeld in the sensor frame,

S

ˆ s, using the rotation operation

described by equation (5). Therefore

S

E

ˆ q may be found as the solution to (14) where equation

(15) deﬁnes the objective function. The components of each vector are deﬁned in equations

(16) to (18).

min

S

E

ˆ q∈

4

f(

S

E

ˆ q,

E

ˆ

d,

S

ˆ s) (14)

f(

S

E

ˆ q,

E

ˆ

d,

S

ˆ s) =

S

E

ˆ q

∗

⊗

E

ˆ

d ⊗

S

E

ˆ q −

S

ˆ s (15)

S

E

ˆ q =

q

1

q

2

q

3

q

4

(16)

E

ˆ

d =

0 d

x

d

y

d

z

(17)

S

ˆ s =

0 s

x

s

y

s

z

(18)

Many optimisation algorithms exist but the gradient descent algorithm is one of the

simplest to both implement and compute. Equations (19) describes the gradient descent

algorithm for n iterations resulting in an orientation estimation of

S

E

ˆ q

n+1

based on an ‘initial

guess’ orientation

S

E

ˆ q

0

and a step-size µ. Equation (20) computes the gradient of the solution

surface deﬁned by the objective function and its Jacobian; simpliﬁed to the 3 row vectors

deﬁned by equations (21) and (22) respectively.

S

E

q

k+1

=

S

E

ˆ q

k

−µ

∇f(

S

E

ˆ q

k

,

E

ˆ

d,

S

ˆ s)

∇f(

S

E

ˆ q

k

,

E ˆ

d,

S

ˆ s)

, k = 0, 1, 2...n (19)

∇f(

S

E

ˆ q

k

,

E

ˆ

d,

S

ˆ s) = J

T

(

S

E

ˆ q

k

,

E

ˆ

d)f(

S

E

ˆ q

k

,

E

ˆ

d,

S

ˆ s) (20)

f(

S

E

ˆ q

k

,

E

ˆ

d,

S

ˆ s) =

2d

x

(

1

2

−q

2

3

−q

2

4

) + 2d

y

(q

1

q

4

+ q

2

q

3

)+

2d

x

(q

2

q

3

−q

1

q

4

) + 2d

y

(

1

2

−q

2

2

−q

2

4

)+

2d

x

(q

1

q

3

+ q

2

q

4

) + 2d

y

(q

3

q

4

−q

1

q

2

)+

2d

z

(q

2

q

4

−q

1

q

3

) −s

x

2d

z

(q

1

q

2

+ q

3

q

4

) −s

y

2d

z

(

1

2

−q

2

2

−q

2

3

) −s

z

¸

¸

(21)

7

J(

S

E

ˆ q

k

,

E

ˆ

d) =

2d

y

q

4

−2d

z

q

3

2d

y

q

3

+ 2d

z

q

4

−2d

x

q

4

+ 2d

z

q

2

2d

x

q

3

−4d

y

q

2

+ 2d

z

q

1

2d

x

q

3

−2d

y

q

2

2d

x

q

4

−2d

y

q

1

−4d

z

q

2

−4d

x

q

3

+ 2d

y

q

2

−2d

z

q

1

−4d

x

q

4

+ 2d

y

q

1

+ 2d

z

q

2

2d

x

q

2

+ 2d

z

q

4

−2d

x

q

1

−4d

y

q

4

+ 2d

z

q

3

2d

x

q

1

+ 2d

y

q

4

−4d

z

q

3

2d

x

q

2

+ 2d

y

q

3

¸

¸

(22)

Equations (19) to (22) describe the general form of the algorithm applicable to a ﬁeld

predeﬁned in any direction. However, if the direction of the ﬁeld can be assumed to only

have components within 1 or 2 of the principle axis of the global coordinate frame then

the equations simplify. An appropriate convention would be to assume that the direction of

gravity deﬁnes the vertical, z axis as shown in equation (23). Substituting

E

ˆ g and normalised

accelerometer measurement

S

ˆ a for

E

ˆ

d and

S

ˆ s respectively, in equations (21) and (22) yields

equations (25) and (26).

E

ˆ g =

0 0 0 1

(23)

S

ˆ a =

0 a

x

a

y

a

z

(24)

f

g

(

S

E

ˆ q,

S

ˆ a) =

2(q

2

q

4

−q

1

q

3

) −a

x

2(q

1

q

2

+ q

3

q

4

) −a

y

2(

1

2

−q

2

2

−q

2

3

) −a

z

¸

¸

(25)

J

g

(

S

E

ˆ q) =

−2q

3

2q

4

−2q

1

2q

2

2q

2

2q

1

2q

4

2q

3

0 −4q

2

−4q

3

0

¸

¸

(26)

The earth’s magnetic ﬁeld can be considered to have components in one horizontal axis

and the vertical axis; the vertical component due to the inclination of the ﬁeld which is

between 65

◦

and 70

◦

to the horizontal in the UK [39]. This can be represented by equa-

tion (27). Substituting

E

ˆ

b and normalised magnetometer measurement

S

ˆ m for

E

ˆ

d and

S

ˆ s

respectively, in equations (21) and (22) yields equations (29) and (30).

E

ˆ

b =

0 b

x

0 b

z

(27)

S

ˆ m =

0 m

x

m

y

m

z

(28)

f

b

(

S

E

ˆ q,

E

ˆ

b,

S

ˆ m) =

2b

x

(0.5 −q

2

3

−q

2

4

) + 2b

z

(q

2

q

4

−q

1

q

3

) −m

x

2b

x

(q

2

q

3

−q

1

q

4

) + 2b

z

(q

1

q

2

+ q

3

q

4

) −m

y

2b

x

(q

1

q

3

+ q

2

q

4

) + 2b

z

(0.5 −q

2

2

−q

2

3

) −m

z

¸

¸

(29)

8

J

b

(

S

E

ˆ q,

E

ˆ

b) =

−2b

z

q

3

2b

z

q

4

−4b

x

q

3

−2b

z

q

1

−2b

x

q

4

+ 2b

z

q

2

2b

x

q

3

+ 2b

z

q

1

2b

x

q

2

+ 2b

z

q

4

2b

x

q

3

2b

x

q

4

−4b

z

q

2

2b

x

q

1

−4b

z

q

3

−4b

x

q

4

+ 2b

z

q

2

−2b

x

q

1

+ 2b

z

q

3

2b

x

q

2

¸

¸

(30)

As has already been discussed, the measurement of gravity or the earth’s magnetic ﬁeld

alone will not provide a unique orientation of the sensor. To do so, the measurements and

reference directions of both ﬁelds may be combined as described by equations (31) and

(32). Whereas the solution surface created by the objective functions in equations (25) and

(29) have a minimum deﬁned by a line, the solution surface deﬁne by equation (31) has a

minimum deﬁne by a single point, provided that b

x

= 0.

f

g,b

(

S

E

ˆ q,

S

ˆ a,

E

ˆ

b,

S

ˆ m) =

¸

f

g

(

S

E

ˆ q,

S

ˆ a)

f

b

(

S

E

ˆ q,

E

ˆ

b,

S

ˆ m)

(31)

J

g,b

(

S

E

ˆ q,

E

ˆ

b) =

¸

J

T

g

(

S

E

ˆ q)

J

T

b

(

S

E

ˆ q,

E

ˆ

b)

(32)

A conventional approach to optimisation would require multiple iterations of equation

(19) to be computed for each new orientation and corresponding senor measurements. Ef-

ﬁcient algorithms would also require the step-size µ to be adjusted each iteration to an

optimal value; usually obtained based on the second derivative of the objective function, the

Hessian. However, these requirements considerably increase the computational load of the

algorithm and are not necessary in this application. It is acceptable to compute one iteration

per time sample provided that the convergence rate governed by µ

t

is equal or greater than

the physical rate of change of orientation. Equation (33) calculates the estimated orienta-

tion

S

E

q

∇,t

computed at time t based on a previous estimate of orientation

S

E

ˆ q

est,t-1

and the

objective function gradient ∇f deﬁned by sensor measurements

S

ˆ a

t

and

S

ˆ m

t

sampled at

time t. The form of ∇f is chosen according to the sensors in use, as shown in equation

(34). The sub-script ∇ indicates that the quaternion is calculated using the gradient descent

algorithm.

S

E

q

∇,t

=

S

E

ˆ q

est,t-1

−µ

t

∇f

∇f

(33)

∇f =

J

T

g

(

S

E

ˆ q

est,t-1

)f

g

(

S

E

ˆ q

est,t-1

,

S

ˆ a

t

)

J

T

g,b

(

S

E

ˆ q

est,t-1

,

E

ˆ

b)f

g,b

(

S

E

ˆ q

est,t-1

,

S

ˆ a,

E

ˆ

b,

S

ˆ m)

(34)

An optimal value of µ

t

can be deﬁned as that which ensures the convergence rate of

S

E

q

∇,t

is limited to the physical orientation rate as this avoids overshooting due an unnecessarily

large step size. Therefore µ

t

can be calculated as equation (35) where ∆t is the sampling

period and

S

E

˙ q

ω,t

is the physical orientation rate measured by gyroscopes and α is an aug-

mentation of µ to account for noise in accelerometer and magnetometer measurements.

9

µ

t

= α

S

E

˙ q

ω,t

∆t, α > 1 (35)

3.3 Filter fusion algorithm

An estimated orientation of the sensor frame relative to the earth frame,

S

E

q

est,t

, is obtained

through the fusion of the orientation calculations,

S

E

q

ω,t

and

S

E

q

∇,t

; calculated using equations

(13) and (33) respectively. The fusion of

S

E

ˆ q

ω,t

and

S

E

q

∇,t

is described by equation (36) where

γ

t

and (1 −γ

t

) are weights applied to each orientation calculation.

S

E

q

est,t

= γ

t

S

E

q

∇,t

+ (1 −γ

t

)

S

E

q

ω,t

, 0 ≤ γ

t

≤ 1 (36)

An optimal value of γ

t

can be deﬁned as that which ensures the weighted divergence

of

S

E

q

ω

is equal to the weighted convergence of

S

E

q

∇

. This is represented by equation (37)

where

µt

∆t

is the convergence rate of

S

E

q

∇

and β is the divergence rate of

S

E

q

ω

expressed as

the magnitude of a quaternion derivative corresponding to the gyroscope measurement error.

Equation (37) can be rearranged to deﬁne γ

t

as equation (38).

(1 −γ

t

)β = γ

t

µ

t

∆t

(37)

γ

t

=

β

µt

∆t

+ β

(38)

Equations (36) and (38) ensure the optimal fusion of

S

E

q

ω,t

and

S

E

q

∇,t

assuming that the

convergence rate of

S

E

q

∇

governed by α is equal or greater than the physical rate of change

of orientation. Therefore α has no upper bound. If α is assumed to be very large then

µ

t

, deﬁned by equation (35), also becomes very large and the orientation ﬁlter equations

simplify. A large value of µ

t

used in equation (33) means that

S

E

ˆ q

est,t-1

becomes negligible

and the equation can be re-written as equation (39).

S

E

q

∇,t

≈ −µ

t

∇f

∇f

(39)

The deﬁnition of γ

t

in equation (38) also simpliﬁes as the β term in the denominator

becomes negligible and the equation can be rewritten as equation (40). It is possible from

equation (40) to also assume that γ

t

≈ 0.

γ

t

≈

β∆t

µ

t

(40)

Substituting equations (13), (39) and (40) into equation (36) directly yields equation

(41). It is important to note that in equation (41), γ

t

has been substituted as both as

equation (39) and 0.

S

E

q

est,t

=

β∆t

µ

t

−µ

t

∇f

∇f

+ (1 −0)

S

E

ˆ q

est,t-1

+

S

E

˙ q

ω,t

∆t

(41)

10

Equation (41) can be simpliﬁed to equation (42) where

S

E

˙ q

est,t

is the estimated rate of

change of orientation deﬁned by equation (43) and

S

E

˙

ˆ q

,t

is the direction of the error of

S

E

˙ q

est,t

deﬁned by equation (44).

S

E

q

est,t

=

S

E

ˆ q

est,t-1

+

S

E

˙ q

est,t

∆t (42)

S

E

˙ q

est,t

=

S

E

˙ q

ω,t

−β

S

E

˙

ˆ q

,t

(43)

S

E

˙

ˆ q

,t

=

∇f

∇f

(44)

It can be seen from equations (42) to (44) that the ﬁlter calculates the orientation

S

E

q

est

by numerically integrating the estimated orientation rate

S

E

˙ q

est

. The ﬁlter computes

S

E

˙ q

est

as the rate of change of orientation measured by the gyroscopes,

S

E

˙ q

ω

, with the magnitude

of the gyroscope measurement error, β, removed in the direction of the estimated error,

S

E

˙

ˆ q

,

computed from accelerometer and magnetometer measurements. Figure 2 shows a block

diagram representation of the complete orientation ﬁlter implementation for an IMU.

Accelerometer

S

ˆ a

t

Gyroscope

S

ω

t

S

E

ˆ q

est,t

S

E

˙ q

est,t

1

2

S

E

ˆ q

est,t−1

⊗

S

ω

t

.dt

q

q

J

T

g

(

S

E

ˆ q

est,t−1

)f

g

(

S

E

ˆ q

est,t−1

,

S

ˆ a

t

)

z

−1

z

−1

∇f

∇f

β

Figure 2: Block diagram representation of the complete orientation ﬁlter for an IMU imple-

mentation

3.4 Magnetic distortion compensation

Measurements of the earth’s magnetic ﬁeld will be distorted by the presence of ferromagnetic

elements in the vicinity of the magnetometer. Investigations into the eﬀect of magnetic

distortions on an orientation sensor’s performance have shown that substantial errors may

be introduced by sources including electrical appliances, metal furniture and metal structures

within a buildings construction [40, 41]. Sources of interference ﬁxed in the sensor frame,

termed hard iron biases, can be removed through calibration [42, 43, 44, 45]. Sources of

interference in the earth frame, termed soft iron, cause errors in the measured direction of

11

the earth’s magnetic ﬁeld. Declination errors, those in the horizontal plane relative to the

earth’s surface, cannot be corrected without an additional reference of heading. Inclination

errors, those in the vertical plane relative to the earth’s surface, may be compensated for as

the accelerometer provides an additional measurement of the sensor’s attitude.

The measured direction of the earth’s magnetic ﬁeld in the earth frame at time t,

E

ˆ

h

t

, can

be computed as the normalised magnetometer measurement,

S

ˆ m

t

, rotated by the estimated

orientation of the sensor provided by the ﬁlter,

S

E

ˆ q

est,t-1

; as described by equation (45). The

eﬀect of an erroneous inclination of the measured direction earth’s magnetic ﬁeld,

E

ˆ

h

t

, can

be corrected if the ﬁlter’s reference direction of the earth’s magnetic ﬁeld,

E

ˆ

b

t

, is of the same

inclination. This is achieved by computing

E

ˆ

b

t

as

E

ˆ

h

t

normalised to have only components

in the earth frame x and z axes; as described by equation (46).

E

ˆ

h

t

=

0 h

x

h

y

h

z

=

S

E

ˆ q

est,t-1

⊗

S

ˆ m

t

⊗

S

E

ˆ q

∗

est,t-1

(45)

E

ˆ

b

t

=

0

h

2

x

+ h

2

y

0 h

z

(46)

Compensating for magnetic distortions in this way ensures that magnetic disturbances

are limited to only aﬀect the estimated heading component of orientation. The approach also

eliminates the need for the reference direction of the earth’s magnetic ﬁeld to be predeﬁned;

a potential disadvantage of other orientation ﬁlter designs [17, 24].

3.5 Gyroscope bias drift compensation

The gyroscope zero bias will drift over time, with temperature and with motion. Any

practical implementation of an IMU or MARG sensor array must account for this. An

advantage of Kalman-based approaches is that they are able to estimate the gyroscope bias

as an additional state within the system model [26, 30, 15, 24]. However, Mahony et al [35]

showed that gyroscope bias drift may also be compensated for by simpler orientation ﬁlters

through the integral feedback of the error in the rate of change of orientation. A similar

approach will be used here.

The normalised direction of the estimated error in the rate of change of orientation,

S

E

˙

ˆ q

,

may be expressed as the angular error in each gyroscope axis using equation (47); derived

as the inverse to the relationship deﬁned in equation (11). The gyroscope bias,

S

ω

b

, is

represented by the DC component of

S

ω

**and so may removed as the integral of
**

S

ω

weighted

by an appropriate gain, ζ. This would yield the compensated gyroscope measurements

S

ω

c

,

as shown in equations (48) and (49). The ﬁrst element of

S

ω

c

is always assumed to be 0.

S

ω

,t

= 2

S

E

ˆ q

∗

est,t-1

⊗

S

E

˙

ˆ q

,t

(47)

S

ω

b,t

= ζ

¸

t

S

ω

,t

∆t (48)

S

ω

c,t

=

S

ω

t

−

S

ω

b,t

(49)

The compensated gyroscope measurements,

S

ω

c

, may then be used in place of the of

the gyroscope measurements,

S

ω, in equation (11). The magnitude of the angular error in

12

each axis,

S

ω

**is equal to a quaternion derivative of unit length. Therefore the integral gain
**

ζ directly deﬁnes the rate of convergence of the estimated gyroscope bias,

S

ω

b

, expressed

as the magnitude of a quaternion derivative. As this process requires the use of the ﬁlter

estimate of a complete orientation,

S

E

ˆ q

est

, it is only applicable to a MARG implementation

of the ﬁlter. Figure 3 shows a block diagram representation of the complete ﬁlter implemen-

tation for a MARG sensor array, including the magnetic distortion and gyroscope bias drift

compensation.

Accelerometer

S

ˆ a

t

Magnetometer

S

ˆ m

t

Gyroscope

S

ω

t

S

E

ˆ q

est,t

S

E

˙ q

est,t

1

2

S

E

ˆ q

est,t-1

⊗

S

ω

c,t

.dt

q

q

J

T

g,b

(

S

E

ˆ q

est,t-1

,

E

ˆ

b

t

)f

g,b

(

S

E

ˆ q

est,t-1

,

S

ˆ a,

E

ˆ

b

t

,

S

ˆ m)

z

-1

z

-1

∇f

∇f

S

E

ˆ q

est,t-1

⊗

S

ˆ m

t

⊗

S

E

ˆ q

∗

est,t-1

0

h

2

x

+ h

2

y

0 h

z

2

S

E

ˆ q

∗

est,t-1

⊗

S

E

˙

ˆ q

,t

.dt

β ζ

Group 2

Group 1

Figure 3: Block diagram representation of the complete orientation ﬁlter for an MARG im-

plementation including magnetic distortion (Group 1) and gyroscope drift (Group 2) com-

pensation

3.6 Filter gains

The ﬁlter gain β represents all mean zero gyroscope measurement errors, expressed as the

magnitude of a quaternion derivative. The sources of error include: sensor noise, signal

aliasing, quantisation errors, calibration errors, sensor miss-alignment, sensor axis non-

orthogonality and frequency response characteristics. The ﬁlter gain ζ represents the rate

of convergence to remove gyroscope measurement errors which are not mean zero, also ex-

pressed as the magnitude of a quaternion derivative. These errors represent the gyroscope

bias. It is convenient to deﬁne β and ζ using the angular quantities ω

β

and ˙ ω

ζ

respectively,

where ˜ ω

β

represents the estimated mean zero gyroscope measurement error of each axis and

˙ ω

ζ

represents the estimated rate of gyroscope bias drift in each axis. Using the relation-

ship described by equation (11), β may be deﬁned by equation (50) where ˆ q is any unit

quaternion. Similarly, ζ may be described by equation (51).

13

β =

1

2

ˆ q ⊗

0 ˜ ω

β

˜ ω

β

˜ ω

β

=

3

4

˜ ω

β

(50)

ζ =

3

4

˜

˙ ω

ζ

(51)

4 Experimentation

4.1 Equipment

The ﬁlter was tested using the xsens MTx orientation sensor [18] containing 16 bit resolu-

tion tri-axis gyroscopes, accelerometers and magnetometers. The device and accompanying

software oﬀer a mode of operations where raw sensor data may be logged at a rate of 512 Hz

and then post-processed to provide calibrated sensor measurements. The calibrated sensor

measurements could then be processed by the proposed ﬁlter to provide the estimated ori-

entation of the sensor. The software also incorporates a propriety Kalman-based orientation

ﬁlter to provide an additional estimate orientation. As both the Kalman-based algorithm

and proposed ﬁlter’s outputs could be computed using identical sensor data, the perfor-

mance of each algorithm could be evaluated relative to one-another, independent of sensor

performance.

A Vicon system, consisting of 8 MX3+ cameras connected to an MXultranet server

[46] and Nexus [47] software was used to provide reference measurement of the orientation

sensor’s actual orientation. The system is an array of IR (Infrared) sensitive cameras with

incorporated IR ﬂood lights. The cameras are ﬁxed at calibrated positions and orientations so

that the measurement subject is within the ﬁeld of view of multiple cameras. The Cartesian

positions of IR reﬂective optical markers ﬁxed to the measurement subject may then be

computed in the coordinate frame of the camera array. Cameras were ﬁxed at a height of

approximately 2.5 m, evenly distributed around the perimeter of a 4 m by 4 m enclosure.

Each camera was orientated to face toward the centre of the room, approximately 30

◦

to

60

◦

to horizontal. Experiments were conducted with the measurement subject in the centre

of the room at a height of approximately 1 m. To measure the orientation of the sensor,

it was ﬁxed to an optical orientation measurement platform speciﬁcally designed for this

application. The system was used to log the positions of optical markers at a rate of 120 Hz.

4.2 Orientation from optical measurements

The orientation measurement platform is comprised of 3 500 mm, mutually orthogonal rods,

rigidly connected at the central position along each length. Optical markers were positioned

at both ends of each rod and the orientation sensor ﬁxed to a platform at the point where the

rods coincide. The platform was constructed from an aluminium central hub, carbon ﬁbre

rods and assembled using adhesives to ensure that it had no magnetic properties that may

interfere with the orientation sensor’s magnetometer. Additional optical markers were placed

at arbitrary but dissimilar positions along the lengths of the rods to break the rotational

symmetry and aid the identiﬁcation of each rod within the measurement data. Figure 4

14

shows an annotated photograph of the orientation measurement platform where

C

i

start

,

C

i

end

,

C

j

start

,

C

j

end

,

C

k

start

and

C

k

end

deﬁne the measured position of each marker within the

camera frame. These positions may be used to deﬁne 3 mutually orthogonal unit vectors,

C

ˆ x

M

,

C

ˆ y

M

, and

C

ˆ z

M

within the camera frame representing the direction the x, y and z axes

of the orientation measurement platform coordinate frame; as described by equations (52),

(53) and (54). These vectors deﬁne the rotation matrix describing the orientation of the

measurement platform in the camera frame; as shown in equation (55).

C

x

start

C

x

end

C

j

start

C

j

end

C

z

start

C

z

end

Sensor

Figure 4: Photograph of the orientation measurement platform

C

ˆ x

M

=

C

i

end

−

C

i

start

C

i

end

−

C

i

start

(52)

C

ˆ y

M

=

C

j

end

−

C

j

start

C

j

end

−

C

j

start

(53)

C

ˆ z

M

=

C

k

end

−

C

k

start

C

k

end

−

C

k

start

(54)

C

M

R =

C

ˆ x

M

C

ˆ y

M

C

ˆ z

M

(55)

Due to measurement errors and tolerances in the marker frame construction, the rotation

matrix deﬁned by equation (55) cannot be considered orthogonal and so does not represent a

pure rotation. Bar-Itzhack provides a method [48] where by an optimal ‘best ﬁt’ quaternion

may be extracted from an imprecise and non-orthogonal rotation matrix. The method

requires the construction of the symmetric 4 by 4 matrix, K, deﬁned by equation (56),

where r

mn

corresponds to the element of the m

th

row and n

th

column of

C

M

R. The optimal

quaternion

C

M

ˆ q is found as the normalised Eigen vector corresponding to the maximum

Eigen value of K. Equation (57) deﬁnes the optimal quaternion accounting the alternative

15

quaternion element order convention assumed by the method, where v

1

, v

2

, v

3

and v

4

deﬁne

elements of the normalised Eigen vector.

K =

1

3

r

11

−r

22

−r

33

r

21

+ r

12

r

31

+ r

13

r

23

−r

32

r

21

+ r

12

r

22

−r

11

−r

33

r

32

+ r

23

r

31

−r

13

r

31

+ r

13

r

32

+ r

23

r

33

−r

11

−r

22

r

12

−r

21

r

23

−r

32

r

31

−r

13

r

12

−r

21

r

11

+ r

22

+ r

33

¸

¸

¸

¸

(56)

C

M

ˆ q =

v

4

v

1

v

2

v

3

(57)

4.3 Calibration of frame alignments

In order to compare the optical measurement of the platform orientation in the camera frame,

C

M

ˆ q, and the orientation ﬁlter’s estimated orientation of the earth in the sensor frame,

S

E

ˆ q

est

,

it is necessary to know the alignment of the earth frame relative to the camera frame,

C

E

ˆ q,

and the alignment of the measurement platform relative to the sensor frame,

S

M

ˆ q. Once these

quantities are found the optical measurement of the sensor frame orientation in the earth

frame,

S

E

ˆ q

meas

, may be deﬁned by equation (58). Although the use of optical measurement

equipment in this application is documentaed [26, 24, 41], little discussion is oﬀered on the

calibration of these two quantities.

S

E

ˆ q

meas

=

C

E

ˆ q ⊗

M

C

ˆ q ⊗

S

M

ˆ q (58)

The earth frame x and z axes are deﬁned by the earth’s magnetic and gravitational ﬁelds

respectively. Measurements of these ﬁelds in the camera frame can be used to deﬁne the

alignment

C

E

ˆ q. The direction of gravity was measured using a pendulum constructed from a

1 m length of cotton thread with a small weight ﬁxed to one end. Optical markers where

ﬁxed at either end of the length of cotton and the pendulum left to come to rest. Additional

optical markers were required at arbitrary ﬁxed positions relative to the static pendulum

to break the rotational symmetry of the optical marker constellation. Figure 5 shows an

annotated photograph of the pendulum where

C

p

start

and

C

p

end

deﬁne the position of the

optical markers in the camera frame. The mean position of each marker over a period of

time deﬁnes the direction of the pendulum in the camera frame,

C

ˆ p. This directly deﬁnes

the earth frame z axis in the camera frame,

C

ˆ z

E

; as shown in equation (59).

C

ˆ p =

C

¯ p

end

−

C

¯ p

start

C

¯ p

end

−

C

¯ p

start

=

C

ˆ z

E

=

z

1

z

2

z

3

¸

¸

(59)

The direction of earth’s magnetic ﬁeld was measured using a magnetic compass con-

structed from a 1 m carbon-ﬁbre rod with neodymium magnets ﬁxed to each end; polarising

each end to either magnetic north or south. The compass was hung from a 1 m length

of cotton thread and left to come to rest. Optical markers were ﬁxed to either end of the

rod as well as to an arbitrary position along the length, but oﬀ-set form the rod’s axis, to

beak the rotational symmetry optical marker constellation. Figure 60 shows an annotated

photograph of the compass where

C

c

start

and

C

c

end

deﬁne the position of the optical markers

16

C

p

end

C

p

start

Figure 5: Photograph of the pendulum and optical markers used to measure the direction

of gravity in the camera frame

in the camera frame. The mean position of each marker over a period of time deﬁne the

direction of the compass in the camera frame,

C

ˆ c; as shown in equation (60).

C

c

start

C

c

end

Figure 6: Photograph of the magnetic compass and optical markers used to measure the

direction of the earth’s magnetic ﬁeld in the camera frame

C

ˆ c =

C

¯ c

end

−

C

¯ c

start

C

¯ c

end

−

C

¯ c

start

(60)

Due to measurement errors and the imbalance of the suspended magnetic compass,

C

ˆ c

cannot be assumed to be orthogonal to the direction of gravity deﬁned by

C

ˆ p and so cannot

be used to directly deﬁne the earth x axis. The non-orthogonal component of

C

ˆ c can be

computed as the vector projection of

C

ˆ c on

C

ˆ p. This can then be removed from

C

ˆ c to deﬁne

17

the the earth x axis direction in the camera frame,

C

x

E

; as shown in equation (61). Once

normalised, this deﬁnes the earth x axis;

C

ˆ x

E

, as shown in equation (62).

C

x

E

=

C

ˆ c −

C

ˆ c ·

C

ˆ p

C

ˆ p

2

C

ˆ p (61)

C

ˆ x

E

=

C

x

E

C

x

E

=

x

1

x

2

x

3

¸

¸

(62)

The earth frame y axis in the camera frame,

C

ˆ y

E

, can be calculated as the vector that is

orthogonal to both

C

ˆ x

E

and

C

ˆ z

E

and so be deﬁned by equation (63) where signs are chosen

to satisfy the relative axis direction direction convention. The alignment of the earth frame

may be deﬁned as the rotation matrix

C

E

R, constructed from

C

ˆ x

E

,

C

ˆ y

E

, and

C

ˆ z

E

. The

quaternion representation,

C

E

ˆ q, can then be extracted form this using Bar-Itzhack’s method

[48].

C

ˆ y

E

=

±

1 −x

2

1

−z

2

1

±

1 −x

2

2

−z

2

2

±

1 −x

2

3

−z

2

3

¸

¸

,

C

ˆ x

E

·

C

ˆ y

E

= 0,

C

ˆ z

E

·

C

ˆ y

E

= 0 (63)

C

E

R =

C

ˆ x

E

C

ˆ y

E

C

ˆ z

E

(64)

In order to ﬁnd the alignment

S

M

ˆ q it is assumed that the static error of the orientation

ﬁlter’s Kalman-based algorithm is mean zero. The mean algorithm’s output,

S

E

ˆ

¯ q

Kalman

, was

computed for the measurement platform held stationary for a period of approximate 10

seconds. This was used with the alignment

E

C

ˆ q and optical measurement

C

M

ˆ q to deﬁne the

alignment of the measurement platform in the sensor frame,

S

M

ˆ q, as equation (65).

S

M

ˆ q =

C

M

ˆ q ⊗

E

C

ˆ q ⊗

S

E

ˆ

¯ q

Kalman

(65)

4.4 Experimental proceedure

The optical measurement data and raw orientation sensor data were logged simultaneously.

The raw orientation sensor data was then processed by the accompanying software to provide

the calibrated sensor data and Kalman-based algorithm output. This data then synchronised

the optical measurement data, with the optical measurement data interpolated to match the

higher sampling rate of the orientation sensor data. The calibrated sensor data was then

processed through both the IMU and MARG implementations of the proposed orientation

ﬁlter and calibrated orientation measurements were extracted from the optical measurement

data using the methods described in sections 4.2 and 4.3.

The proposed ﬁlter’s gain β was set to 0.033 for the IMU implementation and 0.041 for

the MARG implementation. Trials summarised in section 5.3, found these values to provide

optimal performance. However, an initial value of 2.5 was used for the ﬁrst 10 seconds of

any experiment to ensure the convergence of algorithm states from initial conditions. The

gain ζ, applicable to the MARG implementation of the proposed ﬁler, was set to 0 as the

calibrated orientation sensor data was not subject to gyroscope bias drift.

18

Data was obtained for a sequence of rotations preformed by hand. The measurement

platform was initially held stationary for 20 to 30 seconds to allow time for algorithm states

to converge to steady-state values. The platform was then rotated 90

◦

around its x axis,

then 180

◦

in the opposite direction, and then 90

◦

to bring the platform back to the starting

position. The platform was held stationary for 3 to 5 seconds between each rotation. This

sequence was then repeated around the y and then z axes. The peak angular rate measured

during each rotation was between 110

◦

/s and 190

◦

/s. The experiment was repeated 8 times

to compile a dataset representative of system performance.

5 Results

It is common [24, 26, 18, 19, 20, 21] to quantify orientation sensor performance as the static

and dynamic RMS (Root-Mean-Square) errors in the decoupled Euler parameters describing

the pitch, roll, and heading components of an orientation. Pitch, φ, roll, θ and heading, ψ

correspond to rotations around the sensor frame x, y, and z axis respectively. An Euler angle

representation has the advantage that the decoupled angles may be more easily interpreted

or visualised. The disadvantage of an Euler representation is that it fails to described the

coupling between each of the parameters and will subject to large and erratic errors if the

Euler angle sequence reaches a singularity.

Euler parameters were computed directly from quaternion data using equations (7), (8)

and (9). A total of 4 sets of of Euler parameters were computed, corresponding to the

calibrated optical measurements of orientation, the Kalman-based algorithm estimated ori-

entation and the proposed ﬁlter estimates orientation for both the MARG and IMU imple-

mentations. The errors of estimated Euler parameters, φ

, θ

and ψ

**, were computed as the
**

diﬀerence between the Euler parameters of the calibrated optical measurements and those

of each of the corresponding estimated values.

5.1 Typical results

Figures 7, 8 and 9 show results typical of the 8 experiments for both the Kalman-based

algorithm and the proposed ﬁlter MARG implementation. In each ﬁgure, the 3 traces of the

upper plot represent the optically measured angle, the Kalman-based algorithm estimated

angle, and the proposed ﬁlter estimated angle. The 2 traces of the lower plot represent the

calculated error in each of the estimated angles.

19

22 24 26 28 30 32 34 36 38 40 42 44

−100

−50

0

50

100

Measured and estimated angle φ

φ

(

d

e

g

r

e

e

s

)

Measured

Kalman

Proposed filter

22 24 26 28 30 32 34 36 38 40 42 44

−3

−2

−1

0

1

2

3

Error

φ

ε

(

d

e

g

r

e

e

s

)

time (seconds)

Kalman

Proposed filter

Figure 7: Typical results for measured and estimated angle φ (top) and error (bottom)

42 44 46 48 50 52 54 56 58 60 62

−100

−50

0

50

100

Measured and estimated angle θ

θ

(

d

e

g

r

e

e

s

)

Measured

Kalman

Proposed filter

42 44 46 48 50 52 54 56 58 60 62

−5

0

5

Error

θ

ε

(

d

e

g

r

e

e

s

)

time (seconds)

Kalman

Proposed filter

Figure 8: Typical results for measured and estimated angle θ (top) and error (bottom)

20

60 65 70 75 80

−100

−50

0

50

100

Measured and estimated angle ψ

ψ

(

d

e

g

r

e

e

s

)

Measured

Kalman

Proposed filter

60 65 70 75 80

−5

0

5

Error

ψ

ε

(

d

e

g

r

e

e

s

)

time (seconds)

Kalman

Proposed filter

Figure 9: Typical results for measured and estimated angle ψ (top) and error (bottom)

5.2 Static and dynamic performance

The static and dynamic RMS values of φ

, θ

, and ψ

**were calculated where a static state
**

was assumed when the measured corresponding angular rate was < 5

◦

/s, and a dynamic

state when ≥ 5

◦

/s. This threshold was chosen to be suitably high enough above the noise

ﬂoor of the data. Each RMS value was calculated for the period of time framing only the

rotation sequence of the corresponding Euler parameter; as indicated in ﬁgures 7, 8 and

9. This was to prevent errors due to initial convergence or singularities in the Euler angle

representation from corrupting results; that is, when θ = ±90. The results are summarised

in table 1. Each value, represents the mean of all 8 experiments. Values of RMS[ψ

] were

not computed for the IMU implementation of the proposed ﬁlter as the ﬁlter cannot, nor is

intended to, compensate for an accumulating error in this parameter.

Euler parameter Kalman-based Proposed ﬁlter Proposed ﬁlter

algorithm (MARG) (IMU)

RMS[φ

] static 0.789

◦

0.581

◦

0.594

◦

RMS[φ

] dynamic 0.769

◦

0.625

◦

0.623

◦

RMS[θ

] static 0.819

◦

0.502

◦

0.497

◦

RMS[θ

] dynamic 0.847

◦

0.668

◦

0.668

◦

RMS[ψ

] static 1.150

◦

1.073

◦

N/A

RMS[ψ

] dynamic 1.344

◦

1.110

◦

N/A

Table 1: Static and dynamic RMS error of Kalman-based algorithm and proposed ﬁlter IMU

and MARG implementations

Results indicate that the proposed ﬁlter achieves higher levels of accuracy than the

Kalman-based algorithm. The manufacturer of orientation sensor specify the typical per-

formance of the Kalman-based algorithm as having a static RMS error of < 0.5

◦

in φ and

21

θ, and < 1

◦

in ψ; and a dynamic RMS error of < 2

◦

in φ, θ, and ψ [18]. These values

do not conform with those listed in table 1. Other studies [49] have shown that accuracy

may far less than that quoted by the manufacture and that quoted levels of accuracy are

only achieved once the devices is recalibrated. The lower levels of accuracy in the heading,

ψ

**, are to due characteristics of the sensor’s measurements of the earth’s magnetic ﬁeld.
**

The inclination of the earth’s magnetic ﬁeld during testing was between 65

◦

and 70

◦

to the

horizontal [39]. As a consequence the component of the magnetic ﬂux vector available as

a reference of heading is relatively small. The larger component of the vector serves as an

additional reference for pitch, φ, and roll, θ, alongside the reference measurement of gravity;

hence errors in pitch, φ

, and roll, θ

, may be expected to be less than those in heading, ψ

.

The magnetometer is speciﬁed [18] as having a bandwidth of 10 Hz which, relative to the

30 Hz and 40 Hz bandwidth of the accelerometer and gyroscope respectively, suggests an

increased heading error, ψ

, in dynamic conditions.

5.3 Filter gain vs. performance

The results of an investigation into the eﬀect of the ﬁlter adjustable parameter, β, on the

proposed ﬁlter performance are summarised in Figure 10. The static and dynamic perfor-

mance is quantiﬁed as the mean of the corresponding RMS values of φ

and θ

**, for the IMU
**

implementation and φ

, θ

and φ

**for the MARG implementation. The experimental data
**

was processed though the separate proposed ﬁlter IMU and MARG implantations, using

ﬁxed values of β between 0 to 0.5. There is a clear optimal value of β for each ﬁlter imple-

mentation; high enough to minimises errors due to integral drift but suﬃciently low enough

that unnecessary noise is not introduced by large steps of gradient descent iterations.

0 0.1 0.2 0.3 0.4

0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

β = 0.033

β

m

e

a

n

[

R

M

S

[

φ

ε

]

,

R

M

S

[

θ

ε

]

]

(

d

e

g

r

e

e

s

)

Effect of β on proposed filter performance (IMU)

Static performance

Dynamic performance

0 0.1 0.2 0.3 0.4

0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

β = 0.041

β

m

e

a

n

[

R

M

S

[

φ

ε

]

,

R

M

S

[

θ

ε

]

,

R

M

S

[

ψ

ε

]

]

(

d

e

g

r

e

e

s

)

Effect of β on proposed filter performance (MARG)

Static performance

Dynamic performance

Figure 10: The eﬀect of the adjustable parameter, β, on the performance of the proposed

ﬁlter IMU (left) and MARG (right) implementations

22

5.4 Sampling rate vs. performance

The results of an investigation into the eﬀect of sampling rate on ﬁlter performance is sum-

marised in Figure11. The experimental data was processed though the separate proposed

ﬁlter IMU and MARG implantations, using the previously deﬁned, optimal values β where

the experimental data was decimated to simulate sampling rates between 1Hz and 512 Hz.

It can be seen from Figure11 that the propsoed ﬁlter acheives similar levels of performance

at 50 Hz as at 512 Hz. Both ﬁlter implemntation are able to acheive a static error < 2

◦

and dynamic error < 7

◦

while sampling at 10 Hz. This level of accracuy may be suﬃcent be

eﬀective for applications such as human motion capture.

10

0

10

1

10

2

0

5

10

15

20

25

30

Sampling rate (Hz)

m

e

a

n

[

R

M

S

[

φ

ε

]

,

R

M

S

[

θ

ε

]

]

(

d

e

g

r

e

e

s

)

Effect of sampling rate on proposed filter performance (IMU)

Static performance

Dynamic performance

10

0

10

1

10

2

0

5

10

15

20

25

30

Sampling rate (Hz)

m

e

a

n

[

R

M

S

[

φ

ε

]

,

R

M

S

[

θ

ε

]

,

R

M

S

[

ψ

ε

]

]

(

d

e

g

r

e

e

s

)

Effect of sampling rate on proposed filter performance (MARG)

Static performance

Dynamic performance

Figure 11: The eﬀect of sampling rate on the performance of the proposed ﬁlter IMU (left)

and MARG (right) implementations

5.5 Gyroscope bias drift

The calibrated gyrscope data used by the propsoed ﬁlter did not contain any bias errors. To

investiagate the propsoed ﬁlter’s ability to compensate for bias drift, errors were artiﬁcally

introduced to the 8 experimental datasets. A constant drift of 0.2

◦

/s/s was introduced to

the gyroscope x axis measuremnts, ω

x

and a constant bias error of −0.2

◦

/s as added to the

gyroscope x axis measuremnts, ω

y

. The ﬁlter gain ζ was set to 0 for the ﬁrst 10 seconds of

each experiment whilst the ﬁlter states converge from intial conditions. After this a value of

0.015 was used corrisponding a maximum convergene rate of the esitmated gyroscope bias

of 1

◦

/s/s.

Figure 12 shows resutls typical of teh 8 experimetns, showing the gyroscope x and y axis

bias estimated by the ﬁlter, plotted against the actual. From these results, the ﬁtler can

be seen to scuseﬀuly esitmate teh gyroscope biases witht eh rate limtied rate of convergese.

The ﬁlter performance under these conditions was constent with that described in Table 1.

23

10 15 20 25 30 35 40 45 50 55 60

−4

−2

0

2

4

6

8

10

12

Gyroscope bias drift compensation

d

e

g

r

e

e

s

/

s

seconds

ω

x,b

(actual)

ω

x,b

(estimated)

ω

y,b

(actual)

ω

y,b

(estimated)

Figure 12: Filter tracking of gyroscope bias drift

6 Discussion

The derivation of the ﬁlter initially assumed that the accelerometer and magnetometer would

only measure gravity and the earth’s magnetic ﬁeld. In practise, accelerations due to motion

will result in an erroneous observed direction of gravity and so a potentially corrupt the

estimated attitude and local magnetic distortions may still corrupt the estimated heading.

In many applications it can be assumed that accelerations due to motion and local magnetic

distortions are present for only short periods of time. Therefore the magnitude of the ﬁlter

gain β may be chosen low enough that the divergence caused by the erroneous gravitational

and magnetic ﬁeld observations is reduced to an acceptable level over the period. The

minimum acceptable value of β is limited by the gyroscope measurement error. In many

applications it may be beneﬁcial to use dynamic values of gains β and ζ. This will the

inﬂuence accelerometers and magnetometers have on the estimated orientation to be reduced

during potential problematic periods; for example, when large accelerations are detected.

The use of large gains during the ﬁlter initialisation may also improve the ﬁlter’s convergence

from initial conditions. For example, it was found that a β and β gain of 10 enabled the

ﬁlter states to converge within 5 seconds when initiated with a gyroscope bias error of 1000

deg/s in each axis.

The structure of the ﬁlter implantation for a MARG sensor array is similar to that

proposed by Bachman et al [34]. Both ﬁlters estimate the gyroscope measurement error as

the gradient of a error surface created by the magnetometer and accelerometer measurements.

Bachman’s ﬁlter computes this using a Gauss-Newton approach which requires numerical

diﬀerentiation and a matrix inversion. The ﬁlter proposed in this report uses an analytical

derivation of the Jacobian and operates on a normalised gradient of the error surface. As

a result, the ﬁlter proposed in this paper provides a substantial reduction in computational

load and enables the derivation of an optimal ﬁlter gain based on system characteristics.

Appendix A and B describe the ﬁlter IMU and MARG implementations respectively,

each implemented in C where the code has been optimised to reduce the number of arith-

metic operations at the expense of data memory. The IMU implementation requires 108

arithmetic operations each ﬁlter update, and the MARG implementation requires 277 arith-

24

metic operations per ﬁlter update; which includes the magnetic distortion and gyroscope

drift compensation. These low computational requirements make the algorithm accessible

for low power embedded systems systems enabling the use of low cost, low power hardware.

The experimental procedure used to evaluated the ﬁlter performances has a number

of limitations; the ﬁlter performance was not evaluated for simultaneous rotations around

more than one rotational axis and rotational velocities were limited to short periods and

in magnitude. These limitations were necessary so that repeatable and quantiﬁable and

practical.

7 Conclusions

In this work, we have introduced a novel orientation ﬁlter, applicable to both IMUs and

MARG sensor arrays, that signiﬁcantly ameliorates the computational load and parameter

tuning burdens associated with conventional Kalman-based approaches. The ﬁlter is based

on a Newton optimization using an analytic formulation of the gradient that is derived from

a quaternion representation of motion. Novel aspects of the ﬁlter include:

• Analytic derivation of the Jacobian matrix, which eliminates signiﬁcant computational

load, allowing implementation at lower sampling frequencies and onto lower power,

smaller platforms.

• The need to tune only one or two ﬁlter gains (β and ζ)), deﬁned by the gyroscope

measurement error. Least squares curve ﬁtting and complex tuning processes are

therefore eliminated.

The ﬁlter derivation, magnetic distortion anf gyroscope bias drift compensation, and

experimental testing have been detailed. Empirical testing and benchmarking has shown

that the ﬁlter performs as well as a high quality commercial Kalman-based system, even

with a full order of magnitude in reduction of sampling rate. The ﬁlter is both simple to

implement and simple to tune. The implications of the low computational load and ability

to operate at low sampling rates open a very wide range of new opportunities for the use of

IMU and MARG sensor arrays in real-time applications. Applications where limited power

or processing resources may be available are particularly well suited for the new ﬁlter. The

ﬁlter also has great potential to alleviate computational load for applications that demand

extremely high sampling rates.

References

[1] Mark Euston, Paul Coote, Robert Mahony, Jonghyuk Kim, and Tarek Hamel. A com-

plementary ﬁlter for attitude estimation of a ﬁxed-wing uav with a low-cost imu. In 6th

International Conference on Field and Service Robotics, July 2007.

[2] Sung Kyung Hong. Fuzzy logic based closed-loop strapdown attitude system for un-

manned aerial vehicle (uav). Sensors and Actuators A: Physical, 107(2):109 – 118,

2003.

25

[3] A. Kallapur, I. Petersen, and S. Anavatti. A robust gyroless attitude estimation scheme

for a small ﬁxed-wing unmanned aerial vehicle. pages 666 –671, aug. 2009.

[4] B. Barshan and H. F. Durrant-Whyte. Inertial navigation systems for mobile robots.

11(3):328–342, June 1995.

[5] L. Ojeda and J. Borenstein. Flexnav: fuzzy logic expert rule-based position estimation

for mobile robots on rugged terrain. In Proc. IEEE International Conference on Robotics

and Automation ICRA ’02, volume 1, pages 317–322, May 11–15, 2002.

[6] David H. Titterton and John L. Weston. Strapdown inertial navigation technology. The

Institution of Electrical Engineers, 2004.

[7] S. Beauregard. Omnidirectional pedestrian navigation for ﬁrst responders. In Proc. 4th

Workshop on Positioning, Navigation and Communication WPNC ’07, pages 33–36,

March 22–22, 2007.

[8] H. J. Luinge and P. H. Veltink. Inclination measurement of human movement using a

3-d accelerometer with autocalibration. 12(1):112–121, March 2004.

[9] Huiyu Zhou and Huosheng Hu. Human motion tracking for rehabilitation–a survey.

volume 3, pages 1 – 18, 2008.

[10] E. A. Heinz, K. S. Kunze, M. Gruber, D. Bannach, and P. Lukowicz. Using wearable

sensors for real-time recognition tasks in games of martial arts - an initial experiment.

In Proc. IEEE Symposium on Computational Intelligence and Games, pages 98–102,

May 22–24, 2006.

[11] J. E. Bortz. A new mathematical formulation for strapdown inertial navigation. (1):61–

66, January 1971.

[12] M B Ignagni. Optimal strapdown attitude integration algorithms. In Guidance, Control,

and Dynamics, volume 13, pages 363–369, 1990.

[13] N. Yazdi, F. Ayazi, and K. Najaﬁ. Micromachined inertial sensors. 86(8):1640–1659,

August 1998.

[14] R. E. Kalman. A new approach to linear ﬁltering and prediction problems. Journal of

Basic Engineering, 82:35–45, 1960.

[15] E. Foxlin. Inertial head-tracker sensor fusion by a complementary separate-bias kalman

ﬁlter. In Proc. Virtual Reality Annual International Symposium the IEEE 1996, pages

185–194,267, March 30–April 3, 1996.

[16] H. J. Luinge, P. H. Veltink, and C. T. M. Baten. Estimation of orientation with gyro-

scopes and accelerometers. In Proc. First Joint [Engineering in Medicine and Biology

21st Annual Conf. and the 1999 Annual Fall Meeting of the Biomedical Engineering

Soc.] BMES/EMBS Conference, volume 2, page 844, October 13–16, 1999.

26

[17] J. L. Marins, Xiaoping Yun, E. R. Bachmann, R. B. McGhee, and M. J. Zyda. An ex-

tended kalman ﬁlter for quaternion-based orientation estimation using marg sensors. In

Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems, volume 4,

pages 2003–2011, October 29–November 3, 2001.

[18] Xsens Technologies B.V. MTi and MTx User Manual and Technical Documentation.

Pantheon 6a, 7521 PR Enschede, The Netherlands, May 2009.

[19] MicroStrain Inc. 3DM-GX3 -25 Miniature Attutude Heading Reference Sensor. 459

Hurricane Lane, Suite 102, Williston, VT 05495 USA, 1.04 edition, 2009.

[20] VectorNav Technologies, LLC. VN -100 User Manual. College Station, TX 77840 USA,

preliminary edition, 2009.

[21] InterSense, Inc. InertiaCube2+ Manual. 36 Crosby Drive, Suite 150, Bedford, MA

01730, USA, 1.0 edition, 2008.

[22] PNI sensor corporation. Spacepoint Fusion. 133 Aviation Blvd, Suite 101, Santa Rosa,

CA 95403-1084 USA.

[23] Crossbow Technology, Inc. AHRS400 Series Users Manual. 4145 N. First Street, San

Jose, CA 95134, rev. c edition, February 2007.

[24] A. M. Sabatini. Quaternion-based extended kalman ﬁlter for determining orientation

by inertial and magnetic sensing. 53(7):1346–1356, July 2006.

[25] H. J. Luinge and P. H. Veltink. Measuring orientation of human body segments us-

ing miniature gyroscopes and accelerometers. Medical and Biological Engineering and

Computing, 43(2):273–282, April 2006.

[26] David Jurman, Marko Jankovec, Roman Kamnik, and Marko Topic. Calibration and

data fusion solution for the miniature attitude and heading reference system. Sensors

and Actuators A: Physical, 138(2):411–420, August 2007.

[27] Markus Haid and Jan Breitenbach. Low cost inertial orientation tracking with kalman

ﬁlter. Applied Mathematics and Computation, 153(4):567–575, June 2004.

[28] D. Roetenberg, H. J. Luinge, C. T. M. Baten, and P. H. Veltink. Compensation of

magnetic disturbances improves inertial and magnetic sensing of human body segment

orientation. 13(3):395–405, September 2005.

[29] John L. Crassidis and Landis F. Markley. Unscented ﬁltering for spacecraft attitude

estimation. Journal of guidance, control, and dynamics, 26(4):536–542, 2003.

[30] D. Gebre-Egziabher, R.C. Hayward, and J.D. Powell. Design of multi-sensor atti-

tude determination systems. Aerospace and Electronic Systems, IEEE Transactions

on, 40(2):627 – 649, april 2004.

27

[31] N.H.Q. Phuong, H.-J. Kang, Y.-S. Suh, and Y.-S. Ro. A DCM based orientation esti-

mation algorithm with an inertial measurement unit and a magnetic compass. Journal

of Universal Computer Science, 15(4):859–876, 2009.

[32] Daniel Choukroun. Novel methods for attitude determination using vector observations.

PhD thesis, Israel Institute of Technology, May 2003.

[33] R. A. Hyde, L. P. Ketteringham, S. A. Neild, and R. J. S. Jones. Estimation of upper-

limb orientation based on accelerometer and gyroscope measurements. 55(2):746–754,

February 2008.

[34] E. R. Bachmann, I. Duman, U. Y. Usta, R. B. McGhee, X. P. Yun, and M. J. Zyda.

Orientation tracking for humans and robots using inertial sensors. In Proc. IEEE Inter-

national Symposium on Computational Intelligence in Robotics and Automation CIRA

’99, pages 187–194, November 8–9, 1999.

[35] R. Mahony, T. Hamel, and J.-M. Pﬂimlin. Nonlinear complementary ﬁlters on the

special orthogonal group. Automatic Control, IEEE Transactions on, 53(5):1203 –1218,

june 2008.

[36] J. B. Kuipers. Quaternions and Rotation Sequences: A Primer with Applications to

Orbits, Aerospace and Virtual Reality. Princeton University Press, 1999.

[37] John J. Craig. Introduction to Robotics Mechanics and Control. Pearson Education

International, 2005.

[38] David R. Pratt Robert B. McGhee Joseph M. Cooke, Michael J. Zyda. Npsnet: Flight

simulation dynamic modelling using quaternions. Presence, 1:404–420, 1994.

[39] John Arthur Jacobs. The earth’s core, volume 37 of International geophysics series.

Academic Press, 2 edition, 1987.

[40] E. R. Bachmann, Xiaoping Yun, and C. W. Peterson. An investigation of the eﬀects of

magnetic variations on inertial/magnetic orientation sensors. In Proc. IEEE Interna-

tional Conference on Robotics and Automation ICRA ’04, volume 2, pages 1115–1122,

April 2004.

[41] C.T.M. Baten F.C.T. van der Helm W.H.K. de Vries, H.E.J. Veeger. Magnetic distortion

in motion labs, implications for validating inertial magnetic sensors. Gait & Posture,

29(4):535–541, 2009.

[42] Speake & Co Limited. Autocalibration algorithms for FGM type sensors. Application

note.

[43] Michael J. Caruso. Applications of Magnetoresistive Sensors in Navigation Systems.

Honeywell Inc., Solid State Electronics Center, Honeywell Inc. 12001 State Highway 55,

Plymouth, MN 55441.

28

[44] J. F. Vasconcelos, G. Elkaim, C. Silvestre, P. Oliveira, and B. Cardeira. A geomet-

ric approach to strapdown magnetometer calibration in sensor frame. In Navigation,

Guidance and Control of Underwater Vehicles, volume 2, 2008.

[45] Demoz Gebre-Egziabher, Gabriel H. Elkaim, J. David Powell, and Bradford W. Parkin-

son. Calibration of strapdown magnetometers in magnetic ﬁeld domain. Journal of

Aerospace Engineering, 19(2):87–102, 2006.

[46] Vicon Motion Systems Limited. Vicon MX Hardware. 5419 McConnell Avenue, Los

Angeles, CA 90066, USA, 1.6 edition, 2004.

[47] Vicon Motion Systems Limited. Vicon Nexus Product Guide - Foundation Notes. 5419

McConnell Avenue, Los Angeles, CA 90066, USA, 1.2 edition, November 2007.

[48] Itzhack Y Bar-Itzhack. New method for extracting the quaternion from a rotation

matrix. AIAA Journal of Guidance, Control and Dynamics, 23(6):10851087, Nov.Dec

2000. (Engineering Note).

[49] M. A. Brodie, A. Walmsley, and W. Page. The static accuracy and calibration of

inertial measurement units for 3D orientation. Computer Methods in Biomechanics and

Biomedical Engineering, 11(6):641–648, December 2008.

A IMU ﬁlter implementation optimised in C

The following source code is an implementation of the orientation ﬁlter for an IMU, in C.

The code has been optimised minimise the required number of arithmetic operations at the

expense of data memory. Each ﬁlter update requires 109 scalar arithmetic operations (18

additions, 20 subtracts, 57 multiplications, 11 divisions and 3 square roots). The implemen-

tation requires 40 bytes of data memory for global variables and 100 bytes of data memory

for local variables during each ﬁlter update function call.

// Math library required for ‘sqrt’

#include <math.h>

// System constants

#define deltat 0.001f // sampling period in seconds (shown as 1 ms)

#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)

#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta

// Global system variables

float a_x, a_y, a_z; // accelerometer measurements

float w_x, w_y, w_z; // gyroscope measurements in rad/s

float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation quaternion elements with initial conditions

void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z)

{

// Local system variables

float norm; // vector norm

float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements

float f_1, f_2, f_3; // objective function elements

float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements

float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error

// Axulirary variables to avoid reapeated calcualtions

float halfSEq_1 = 0.5f * SEq_1;

float halfSEq_2 = 0.5f * SEq_2;

float halfSEq_3 = 0.5f * SEq_3;

float halfSEq_4 = 0.5f * SEq_4;

float twoSEq_1 = 2.0f * SEq_1;

float twoSEq_2 = 2.0f * SEq_2;

float twoSEq_3 = 2.0f * SEq_3;

29

// Normalise the accelerometer measurement

norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);

a_x /= norm;

a_y /= norm;

a_z /= norm;

// Compute the objective function and Jacobian

f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;

f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;

f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;

J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication

J_12or23 = 2.0f * SEq_4;

J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication

J_14or21 = twoSEq_2;

J_32 = 2.0f * J_14or21; // negated in matrix multiplication

J_33 = 2.0f * J_11or24; // negated in matrix multiplication

// Compute the gradient (matrix multiplication)

SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;

SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;

SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;

SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;

// Normalise the gradient

norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);

SEqHatDot_1 /= norm;

SEqHatDot_2 /= norm;

SEqHatDot_3 /= norm;

SEqHatDot_4 /= norm;

// Compute the quaternion derrivative measured by gyroscopes

SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;

SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;

SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;

SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;

// Compute then integrate the estimated quaternion derrivative

SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;

SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;

SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;

SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;

// Normalise quaternion

norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);

SEq_1 /= norm;

SEq_2 /= norm;

SEq_3 /= norm;

SEq_4 /= norm;

}

B MARG ﬁlter implementation optimised in C

The following source code is an implementation of the orientation ﬁlter for a MARG sensor

array including magnetic distortion and gyroscope drift compensation, in C. The code has

been optimised minimise the required number of arithmetic operations at the expense of

data memory. Each ﬁlter update requires 277 scalar arithmetic operations (51 additions,

57 subtracts, 155 multiplications, 14 divisions and 5 square roots). The implementation

requires 72 bytes of data memory for global variables and 260 bytes of data memory for local

variables during each ﬁlter update function call.

// Math library required for ‘sqrt’

#include <math.h>

// System constants

#define deltat 0.001f // sampling period in seconds (shown as 1 ms)

#define gyroMeasError 3.14159265358979 * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)

#define gyroMeasDrift 3.14159265358979 * (0.2f / 180.0f) // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s)

#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta

#define zeta sqrt(3.0f / 4.0f) * gyroMeasDrift // compute zeta

// Global system variables

float a_x, a_y, a_z; // accelerometer measurements

float w_x, w_y, w_z; // gyroscope measurements in rad/s

float m_x, m_y, m_z; // magnetometer measurements

float SEq_1 = 1, SEq_2 = 0, SEq_3 = 0, SEq_4 = 0; // estimated orientation quaternion elements with initial conditions

float b_x = 1, b_z = 0; // reference direction of flux in earth frame

30

float w_bx = 0, w_by = 0, w_bz = 0; // estimate gyroscope biases error

// Function to compute one filter iteration

void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z, float m_x, float m_y, float m_z)

{

// local system variables

float norm; // vector norm

float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements

float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements

float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements

J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //

float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error

float w_err_x, w_err_y, w_err_z; // estimated direction of the gyroscope error (angular)

float h_x, h_y, h_z; // computed flux in the earth frame

// axulirary variables to avoid reapeated calcualtions

float halfSEq_1 = 0.5f * SEq_1;

float halfSEq_2 = 0.5f * SEq_2;

float halfSEq_3 = 0.5f * SEq_3;

float halfSEq_4 = 0.5f * SEq_4;

float twoSEq_1 = 2.0f * SEq_1;

float twoSEq_2 = 2.0f * SEq_2;

float twoSEq_3 = 2.0f * SEq_3;

float twoSEq_4 = 2.0f * SEq_4;

float twob_x = 2.0f * b_x;

float twob_z = 2.0f * b_z;

float twob_xSEq_1 = 2.0f * b_x * SEq_1;

float twob_xSEq_2 = 2.0f * b_x * SEq_2;

float twob_xSEq_3 = 2.0f * b_x * SEq_3;

float twob_xSEq_4 = 2.0f * b_x * SEq_4;

float twob_zSEq_1 = 2.0f * b_z * SEq_1;

float twob_zSEq_2 = 2.0f * b_z * SEq_2;

float twob_zSEq_3 = 2.0f * b_z * SEq_3;

float twob_zSEq_4 = 2.0f * b_z * SEq_4;

float SEq_1SEq_2;

float SEq_1SEq_3 = SEq_1 * SEq_3;

float SEq_1SEq_4;

float SEq_2SEq_3;

float SEq_2SEq_4 = SEq_2 * SEq_4;

float SEq_3SEq_4;

float twom_x = 2.0f * m_x;

float twom_y = 2.0f * m_y;

float twom_z = 2.0f * m_z;

// normalise the accelerometer measurement

norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);

a_x /= norm;

a_y /= norm;

a_z /= norm;

// normalise the magnetometer measurement

norm = sqrt(m_x * m_x + m_y * m_y + m_z * m_z);

m_x /= norm;

m_y /= norm;

m_z /= norm;

// compute the objective function and Jacobian

f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;

f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;

f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;

f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - m_x;

f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - m_y;

f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - m_z;

J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication

J_12or23 = 2.0f * SEq_4;

J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication

J_14or21 = twoSEq_2;

J_32 = 2.0f * J_14or21; // negated in matrix multiplication

J_33 = 2.0f * J_11or24; // negated in matrix multiplication

J_41 = twob_zSEq_3; // negated in matrix multiplication

J_42 = twob_zSEq_4;

J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication

J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication

J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication

J_52 = twob_xSEq_3 + twob_zSEq_1;

J_53 = twob_xSEq_2 + twob_zSEq_4;

J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication

J_61 = twob_xSEq_3;

J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;

J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;

J_64 = twob_xSEq_2;

// compute the gradient (matrix multiplication)

SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;

SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;

SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;

SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;

// normalise the gradient to estimate direction of the gyroscope error

norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);

SEqHatDot_1 = SEqHatDot_1 / norm;

SEqHatDot_2 = SEqHatDot_2 / norm;

31

SEqHatDot_3 = SEqHatDot_3 / norm;

SEqHatDot_4 = SEqHatDot_4 / norm;

// compute angular estimated direction of the gyroscope error

w_err_x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;

w_err_y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;

w_err_z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;

// compute and remove the gyroscope baises

w_bx += w_err_x * deltat * zeta;

w_by += w_err_y * deltat * zeta;

w_bz += w_err_z * deltat * zeta;

w_x -= w_bx;

w_y -= w_by;

w_z -= w_bz;

// compute the quaternion rate measured by gyroscopes

SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;

SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;

SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;

SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;

// compute then integrate the estimated quaternion rate

SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;

SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;

SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;

SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;

// normalise quaternion

norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);

SEq_1 /= norm;

SEq_2 /= norm;

SEq_3 /= norm;

SEq_4 /= norm;

// compute flux in the earth frame

SEq_1SEq_2 = SEq_1 * SEq_2; // recompute axulirary variables

SEq_1SEq_3 = SEq_1 * SEq_3;

SEq_1SEq_4 = SEq_1 * SEq_4;

SEq_3SEq_4 = SEq_3 * SEq_4;

SEq_2SEq_3 = SEq_2 * SEq_3;

SEq_2SEq_4 = SEq_2 * SEq_4;

h_x = twom_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 - SEq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3);

h_y = twom_x * (SEq_2SEq_3 + SEq_1SEq_4) + twom_y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom_z * (SEq_3SEq_4 - SEq_1SEq_2);

h_z = twom_x * (SEq_2SEq_4 - SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);

// normalise the flux vector to have only components in the x and z

b_x = sqrt((h_x * h_x) + (h_y * h_y));

b_z = h_z;

}

32

Contents

1 Introduction 2 Quaternion representation 3 Filter derivation 3.1 Orientation from angular rate . . . . 3.2 Orientation from vector observations 3.3 Filter fusion algorithm . . . . . . . . 3.4 Magnetic distortion compensation . . 3.5 Gyroscope bias drift compensation . 3.6 Filter gains . . . . . . . . . . . . . . 3 4 6 6 6 10 11 12 13 14 14 14 16 18 19 19 21 22 23 23 24 25 29 30

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

4 Experimentation 4.1 Equipment . . . . . . . . . . . . . . . . 4.2 Orientation from optical measurements 4.3 Calibration of frame alignments . . . . 4.4 Experimental proceedure . . . . . . . . 5 Results 5.1 Typical results . . . . . . . . . 5.2 Static and dynamic performance 5.3 Filter gain vs. performance . . . 5.4 Sampling rate vs. performance . 5.5 Gyroscope bias drift . . . . . . 6 Discussion 7 Conclusions

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

A IMU ﬁlter implementation optimised in C B MARG ﬁlter implementation optimised in C

2

1

Introduction

The accurate measurement of orientation plays a critical role in a range of ﬁelds including: aerospace [1, 2, 3], robotics [4, 5], navigation [6, 7] and human motion analysis [8, 9] and machine interaction [10]. Whilst a variety of technologies enable the measurement of orientation, inertial based sensory systems have the advantage of being completely self contained such that the measurement entity is constrained neither in motion nor to any speciﬁc environment or location. An IMU (Inertial Measurement Unit) consists of gyroscopes and accelerometers enabling the tracking of rotational and translational movements. In order to measure in three dimensions, tri-axis sensors consisting of 3 mutually orthogonal sensitive axes are required. A MARG (Magnetic, Angular Rate, and Gravity) sensor is a hybrid IMU which incorporates a tri-axis magnetometer. An IMU alone can only measure an attitude relative to the direction of gravity which is suﬃcient for many applications [4, 2, 8, 1]. MARG systems, also known as AHRS (Attitude and Heading Reference Systems) are able to provide a complete measurement of orientation relative to the direction of gravity and the earth’s magnetic ﬁeld. A gyroscope measures angular velocity which, if initial conditions are known, may be integrated over time to compute the sensor’s orientation [11, 12]. Precision gyroscopes, ring laser for example, are too expensive and bulky for most applications and so less accurate MEMS (Micro Electrical Mechanical System) devices are used in a majority of applications [13]. The integration of gyroscope measurement errors will lead to an accumulating error in the calculated orientation. Therefore, gyroscopes alone cannot provide an absolute measurement of orientation. An accelerometer and magnetometer will measure the earth’s gravitational and magnetic ﬁelds respectively and so provide an absolute reference of orientation. However, they are likely to be subject to high levels of noise; for example, accelerations due to motion will corrupt measured direction of gravity. The task of an orientation ﬁlter is to compute a single estimate of orientation through the optimal fusion of gyroscope, accelerometer and magnetometer measurements. The Kalman ﬁlter [14] has become the accepted basis for the majority of orientation ﬁlter algorithms [4, 15, 16, 17] and commercial inertial orientation sensors; xsens [18], micro-strain [19], VectorNav [20], Intersense [21], PNI [22] and Crossbow [23] all produce systems founded on its use. The widespread use of Kalman-based solutions are a testament to their accuracy and eﬀectiveness, however, they have a number of disadvantages. They can be complicated to implement which is reﬂected by the numerous solutions seen in the subject literature [3, 4, 15, 16, 17, 24, 25, 26, 27, 28, 29, 30, 31, 32]. The linear regression iterations, fundamental to the Kalman process, demand sampling rates far exceeding the subject bandwidth; for example, a sampling rate between 512 Hz [18] and 30 kHz [19] may be used for a human motion caption application. The state relationships describing rotational kinematics in three-dimensions typically require large state vectors and an extended Kalman ﬁlter implementation [4, 17, 24] to linearise the problem. These challenges demand a large computational load for implementation of Kalmanbased solutions and provide a clear motivation for alternative approaches. Many previous approaches to address these issues have implemented either fuzzy processing [2, 5] or ﬁxed ﬁlters [33] to favour accelerometer measurements of orientation at low angular velocities and the integrated gyroscope measurements at high angular velocities. Such an approach is simple 3

A notation system of leading super-scripts and sub-scripts adopted from Craig [37] is used to denote the relative frames of orientations and vectors. B q is the conjugate of A q and describes the orientation of Aˆ Bˆ ˆ frame A relative to frame B. For example. ry and rz deﬁne the components of the unit vector A r in the x. However. and xB . Mahony et al [35] developed the complementary ﬁlter which is shown to be an eﬃcient and eﬀective solution. A q describes the orientation of Bˆ A ˆ frame B relative to frame A and r is a vector described in frame A. can be used to swap the relative frames described by an orientation. an on-line magnetic distortion compensation algorithm. 24. A complete derivation and empirical evaluation of the new ﬁlter is presented. The conjugate of A q is deﬁned by equation (2). Quaternion arithmetic often requires that a quaternion describing an orientation is ﬁrst normalised. and gyroscope bias drift compensation. The quaternion describing this orientation. performance is only validated for an IMU. 32]) to describe the coupled nature of orientations in three-dimensions and is not subject to the problematic singularities associated with an Euler angle representation1 . the process requires a least squares regression. yB and zB deﬁne the principle axis of coordinate frames A and B respectively. y and z axes of frame A respectively. Bˆ ˆ is deﬁned by equation (1) where rx . 17. This report introduces novel orientation ﬁlter that is applicable to both IMUs and MARG sensor arrays addressing issues of computational load and parameter tuning associated with Kalman-based approaches. Its performance is benchmarked against an existing commercial ﬁlter and veriﬁed with optical measurement system. however.but may only be eﬀective under limited operating conditions. denoted by ∗ . 4 . Innovative aspects of the proposed ﬁlter include: a single adjustable parameter deﬁned by observable systems characteristics. A ˆ Bq θ θ θ θ = q1 q2 q3 q4 = cos 2 −rx sin 2 −ry sin 2 −rz sin 2 (1) The quaternion conjugate. This is represented graphically in ﬁgure 1 where the ˆ ˆ ˆ ˆ ˆ ˆ mutually orthogonal unit vectors xA . For example. yA and zA . A q. 30. An arbitrary orientation of frame B relative to frame A can be achieved through a rotation of angle θ ˆ around an axis A r deﬁned in frame A. Bachman et al [34] proposed an alternative approach where the ﬁlter achieves an optimal fusion of measurements data at all angular velocities. which also brings in an associated computational load. It is therefore conventional for all quaternions describing an orientation to be of unit length. B A ∗ ˆ Bq 1 = B q = q1 −q2 −q3 −q4 Aˆ (2) Kuipers [36] oﬀers a comprehensive introduction to this use of quaternions. A leading sub-script denotes the frame being described and a leading super-script denotes the frame this is with reference to. 2 Quaternion representation A quaternion is a four-dimensional complex number that can be used to represent the orientation of a ridged body or coordinate frame in three-dimensional space. an analytically derived and optimised gradientdescent algorithm enabling performance at low sampling rates. The ﬁlter employs a quaternion representation of orientation (as in: [34.

ˆ zB ˆ zA A θ ˆ r ˆ yB ˆ yA ˆ xA xB ˆ Figure 1: The orientation of frame B is achieved by a rotation. A v and B v are the same vector described in frame A and frame B respectively where each vector contains a 0 inserted as the ﬁrst element to make them 4 element row vectors. For example. from alignment with frame A. a and b. that is. denoted by ⊗. B The orientation described by A q can Bˆ by equation (6) [36]. can be used to deﬁne compound orientations. a ⊗ b = b ⊗ a. of ψ around 5 . A ˆ Cq For two quaternions. the quaternion product can be determined using the Hamilton rule and deﬁned as equation (4). for two orientations described by A q and B q. θ and φ in the so called aerospace sequence [36] describe an orientation of frame B achieved by the sequential rotations. The quaternion product. of angle θ around the axis A r. from alignment with frame A. a ⊗ b = a1 a2 a3 a4 ⊗ b 1 b 2 b 3 b 4 T a1 b 1 − a2 b 2 − a3 b 3 − a4 b 4 a b + a2 b1 + a3 b4 − a4 b3 = 1 2 a1 b 3 − a2 b 4 + a3 b 1 + a4 b 2 a1 b 4 + a2 b 3 − a3 b 2 + a4 b 1 = Bq ⊗ Aq Cˆ Bˆ (3) (4) A three dimensional vector can be rotated by a quaternion using the relationship described in equation (5) [36]. the compounded orientation A q Bˆ Cˆ Cˆ can be deﬁned by equation (3). A quaternion product is not commutative. 2 2 2q1 − 1 + 2q2 A B R = 2(q2 q3 − q1 q4 ) 2(q2 q4 + q1 q3 ) v = A q ⊗ Av ⊗ A q∗ Bˆ Bˆ (5) be represented as the rotation matrix A R deﬁned B 2(q2 q3 + q1 q4 ) 2(q2 q4 − q1 q3 ) 2 2 2q1 − 1 + 2q3 2(q3 q4 + q1 q2 ) 2 2 2(q3 q4 − q1 q2 ) 2q1 − 1 + 2q4 (6) Euler angles ψ.

a magnetometer will measure only the earth’s magnetic ﬁeld. In these equations.t ∆t E ˙ Eˆ 3. it will initially be assumed that an accelerometer will measure only gravity.ˆ ˆ ˆ zB . 6 . 2 2 ψ = Atan2 2q2 q3 − 2q1 q4 . E qω. (8) and (9). ωy and ωz respectively. termed ωx . If these parameters (in rads−1 ) are arranged into the vector S ω deﬁned by equation (10). Similarly. the quaternion derivative describing the rate of change of orientation of the earth frame relative to the sensor frame S q can be calculated E ˙ [38] as equation (11). and φ around xB . can S ˙ be computed by numerically integrating the quaternion derivative S qω. a tri-axis magnetometer will measure the magnitude and direction of the earth’s magnetic ﬁeld in the sensor frame compounded with local magnetic ﬂux and distortions. ∆t is the sampling period and S qest.t S E qω.1 Filter derivation Orientation from angular rate A tri-axis gyroscope will measure the angular rate about the x.2 Orientation from vector observations A tri-axis accelerometer will measure the magnitude and direction of the ﬁeld of gravity in the sensor frame compounded with linear accelerations due to motion of the sensor. θ around yB . = S ˙ E q ω. y and z axes of the senor frame. This Euler angle representation of equations (7). In the context of an orientation ﬁlter. The sub-script ω indicates that the quaternion is calculated from angular rates. 2q1 + 2q4 − 1 3 3.t-1 ⊗ S ωt 2E (12) (13) = S qest.t-1 + S q ω.t as described by E equations (12) and (13) provided that initial conditions are known.t-1 is the previous Eˆ estimate of orientation. 2q1 + 2q2 − 1 A ˆ Bq is deﬁned by (7) (8) (9) θ = −sin−1 (2q2 q4 + 2q1 q3 ) 2 2 φ = Atan2 2q3 q4 − 2q1 q2 . S ωt is the angular rate measured at time t. S ω = 0 ωx ωy ωz S ˙ Eq (10) 1S ˆ q ⊗ Sω (11) 2E The orientation of the earth frame relative to the sensor frame at time t.t = 1S ˆ qest.t .

using the rotation operation S ˆ described by equation (5). However. ˆ with the measured direction of the ﬁeld in the sensor frame. S q. S s) Eˆ . Eˆ E ˆ is that which aligns a predeﬁned reference direction of the ﬁeld in the earth frame. S s. Equation (20) computes the gradient of the solution E surface deﬁned by the objective function and its Jacobian..If the direction of an earth’s ﬁeld is known in the earth frame. s) E k (19) (20) ˆ ˆ ˆ ˆ ˆ f (S q k . S s) = S q ∗ ⊗ E d ⊗ S q − S s Eˆ Eˆ Eˆ S ˆ Eq E = q1 q2 q3 q4 ˆ d = 0 dx dy dz ˆ s = 0 sx sy sz S Many optimisation algorithms exist but the gradient descent algorithm is one of the simplest to both implement and compute. S s) Eˆ Eˆ Eˆ 2 2 2dx ( 1 − q3 − q4 ) + 2dy (q1 q4 + q2 q3 )+ 2 1 2 2 = 2dx (q2 q3 − q1 q4 ) + 2dy ( 2 − q2 − q4 )+ 2dx (q1 q3 + q2 q4 ) + 2dy (q3 q4 − q1 q2 )+ 2dz (q2 q4 − q1 q3 ) − sx 2dz (q1 q2 + q3 q4 ) − sy 1 2 2 2dz ( 2 − q2 − q3 ) − sz ˆ ˆ f (S q k . instead there will inﬁnite solutions represented by all those orientations achieved by the rotation the true orientation around an axis parallel with the ﬁeld. S s) = J T (S q k . E d. S q∈ Eˆ ˆ ˆ min f (S q. E d. A quaternion representation requires a complete solution to be found. E d)f (S q k . S s) Eˆ 4 (14) (15) (16) (17) (18) ˆ ˆ ˆ ˆ f (S q. d. the unknown angle being the rotation around an axis parallel with the direction of the ﬁeld. This may be achieved through the formulation of an optimisation problem where an orientation of the sensor. simpliﬁed to the 3 row vectors deﬁned by equations (21) and (22) respectively. E d. S E qk+1 = S qk − µ Eˆ ˆ ˆ f (S q k . The components of each vector are deﬁned in equations (16) to (18). 1. E d. In some applications it may be acceptable to use an Euler angle representation allowing an incomplete solution to be found as two known Euler angles and one unknown [5]. for any given measurement there will not be a unique sensor orientation solution.. S s) Eˆ (21) 7 . a measurement of the ﬁeld’s direction within the sensor frame will allow an orientation of the sensor frame relative to the earth frame to be calculated. E d. k = 0. Equations (19) describes the gradient descent algorithm for n iterations resulting in an orientation estimation of S qn+1 based on an ‘initial Eˆ ˆ guess’ orientation S q0 and a step-size µ. d. E d.n S ˆ E ˆ Sˆ f ( q . 2. Therefore E q may be found as the solution to (14) where equation (15) deﬁnes the objective function.

This can be represented by equaˆ ˆ ˆ ˆ tion (27). Eˆ 2(q2 q4 − q1 q3 ) − ax fg (S q. if the direction of the ﬁeld can be assumed to only have components within 1 or 2 of the principle axis of the global coordinate frame then the equations simplify.ˆ J (S qk . An appropriate convention would be to assume that the direction of ˆ gravity deﬁnes the vertical. z axis as shown in equation (23). E −4dx q3 + 2dy q2 − 2dz q1 −4dx q4 + 2dy q1 + 2dz q2 2dx q2 + 2dz q4 −2dx q1 − 4dy q4 + 2dz q3 2dx q1 + 2dy q4 − 4dz q3 2dx q2 + 2dy q3 (22) ˆ g= 0 0 0 1 (23) (24) S ˆ a = 0 ax ay az The earth’s magnetic ﬁeld can be considered to have components in one horizontal axis and the vertical axis. S a) = 2(q1 q2 + q3 q4 ) − ay Eˆ ˆ 1 2 2 2( 2 − q2 − q3 ) − az −2q3 2q4 −2q1 2q2 2q1 2q4 2q3 Jg (S q) = 2q2 Eˆ 0 −4q2 −4q3 0 (25) (26) b = 0 bx 0 bz (27) (28) S ˆ m = 0 mx my mz 2 2 2bx (0. E d) Eˆ 2dy q4 − 2dz q3 2dy q3 + 2dz q4 −2dx q4 + 2dz q2 2dx q3 − 4dy q2 + 2dz q1 = 2dx q3 − 2dy q2 2dx q4 − 2dy q1 − 4dz q2 Equations (19) to (22) describe the general form of the algorithm applicable to a ﬁeld predeﬁned in any direction. in equations (21) and (22) yields equations (25) and (26). However. S m) = 2bx (q2 q3 − q1 q4 ) + 2bz (q1 q2 + q3 q4 ) − my Eˆ 2 2 2bx (q1 q3 + q2 q4 ) + 2bz (0.5 − q3 − q4 ) + 2bz (q2 q4 − q1 q3 ) − mx ˆ ˆ fb (S q. the vertical component due to the inclination of the ﬁeld which is between 65◦ and 70◦ to the horizontal in the UK [39]. E b.5 − q2 − q3 ) − mz (29) 8 . in equations (21) and (22) yields equations (29) and (30). Substituting E b and normalised magnetometer measurement S m for E d and S s respectively. Substituting E g and normalised S E ˆ S ˆ ˆ accelerometer measurement a for d and s respectively.

However. the measurement of gravity or the earth’s magnetic ﬁeld alone will not provide a unique orientation of the sensor. provided that bx = 0.b (E qest. a.t = S q est. the Hessian.b (E qest.t-1 )fg (S qest. E b.t-1 . E b. E b. Therefore µt can be calculated as equation (35) where ∆t is the sampling period and S qω. S at ) Eˆ Eˆ T S Eˆ S S ˆ ˆ ˆ ˆ ˆ Jg.b (S q. S E q . S a.t-1 and the E Eˆ S S ˆ ˆ objective function gradient f deﬁned by sensor measurements at and mt sampled at time t. Equation (33) calculates the estimated orientation S q . The form of f is chosen according to the sensors in use. S a) Eˆ ˆ S ˆ ˆ ˆ fb (E q.ˆ Jb (S q. S m) = Eˆ ˆ ˆ Jg. The sub-script indicates that the quaternion is calculated using the gradient descent algorithm. usually obtained based on the second derivative of the objective function.t-1 . b)fg. ˆ ˆ fg. 9 . E b) Eˆ As has already been discussed. these requirements considerably increase the computational load of the algorithm and are not necessary in this application. Efﬁcient algorithms would also require the step-size µ to be adjusted each iteration to an optimal value. as shown in equation (34). E b) = Eˆ fg (S q.t computed at time t based on a previous estimate of orientation S q est.t E is limited to the physical orientation rate as this avoids overshooting due an unnecessarily large step size. S m) An optimal value of µt can be deﬁned as that which ensures the convergence rate of S q . S m) (31) (32) −2bz q3 2bz q4 −4bx q3 − 2bz q1 −2bx q4 + 2bz q2 2bx q3 + 2bz q1 2bx q2 + 2bz q4 = 2bx q3 2bx q4 − 4bz q2 2bx q1 − 4bz q3 −4bx q4 + 2bz q2 −2bx q1 + 2bz q3 2bx q2 (30) T Jg (S q) Eˆ T S ˆ ˆ Jb (E q. the measurements and reference directions of both ﬁelds may be combined as described by equations (31) and (32). Whereas the solution surface created by the objective functions in equations (25) and (29) have a minimum deﬁned by a line.t-1 − µt Eˆ f f (33) (34) f= T ˆ Jg (S qest. To do so. It is acceptable to compute one iteration per time sample provided that the convergence rate governed by µt is equal or greater than the physical rate of change of orientation. the solution surface deﬁne by equation (31) has a minimum deﬁne by a single point. E b) A conventional approach to optimisation would require multiple iterations of equation (19) to be computed for each new orientation and corresponding senor measurements.b (S q.t is the physical orientation rate measured by gyroscopes and α is an augE ˙ mentation of µ to account for noise in accelerometer and magnetometer measurements.t-1 .

t = β∆t µt −µt f f + (1 − 0) S ˆ E qest.3 Filter fusion algorithm An estimated orientation of the sensor frame relative to the earth frame. γt ≈ β∆t µt (40) Substituting equations (13). It is important to note that in equation (41).t and S q .t-1 becomes negligible Eˆ and the equation can be re-written as equation (39).t ≈ −µt f f (39) The deﬁnition of γt in equation (38) also simpliﬁes as the β term in the denominator becomes negligible and the equation can be rewritten as equation (40). It is possible from equation (40) to also assume that γt ≈ 0.t = γt S q E . The fusion of S qω. This is represented by equation (37) E E µt where ∆t is the convergence rate of S q and β is the divergence rate of S qω expressed as E E the magnitude of a quaternion derivative corresponding to the gyroscope measurement error. 0 ≤ γt ≤ 1 E (36) An optimal value of γt can be deﬁned as that which ensures the weighted divergence of S qω is equal to the weighted convergence of S q .t-1 + S q ω. Equation (37) can be rearranged to deﬁne γt as equation (38).µt = α S ˙ E qω. calculated using equations E E (13) and (33) respectively. α > 1 (35) 3. γt has been substituted as both as equation (39) and 0. S E qest. (39) and (40) into equation (36) directly yields equation (41). A large value of µt used in equation (33) means that S qest.t . S E qest.t + (1 − γt )S qω.t . S qest. (1 − γt )β = γt γt = µt ∆t µt ∆t (37) (38) β +β Equations (36) and (38) ensure the optimal fusion of S qω.t . also becomes very large and the orientation ﬁlter equations simplify. If α is assumed to be very large then µt .t assuming that the E E convergence rate of S q governed by α is equal or greater than the physical rate of change E of orientation. Therefore α has no upper bound. S qω.t is described by equation (36) where Eˆ E γt and (1 − γt ) are weights applied to each orientation calculation.t ∆t E ˙ (41) 10 .t and S q .t ∆t.t and S q . S E q . is obtained E through the fusion of the orientation calculations. deﬁned by equation (35).

t E ˙ deﬁned by equation (44). S at ) Eˆ g Eˆ f f β Gyroscope S ω t 1S ˆ q ⊗ S ωt 2 E est.t ∆t Eˆ E ˙ ˙ = S q ω. termed soft iron.t−1)f g (S q est.t It can be seen from equations (42) to (44) that the ﬁlter calculates the orientation S qest E by numerically integrating the estimated orientation rate S q est . cause errors in the measured direction of 11 . 41].t−1 S ˙ E q est.Equation (41) can be simpliﬁed to equation (42) where S qest.t Figure 2: Block diagram representation of the complete orientation ﬁlter for an IMU implementation 3.4 Magnetic distortion compensation Measurements of the earth’s magnetic ﬁeld will be distorted by the presence of ferromagnetic elements in the vicinity of the magnetometer. S E qest.t is the estimated rate of E ˙ S ˙ ˆ change of orientation deﬁned by equation (43) and E q .t ˆ Accelerometer S at z−1 .t is the direction of the error of S qest. removed in the direction of the estimated error. Eˆ computed from accelerometer and magnetometer measurements. Investigations into the eﬀect of magnetic distortions on an orientation sensor’s performance have shown that substantial errors may be introduced by sources including electrical appliances. Sources of interference in the earth frame. β. can be removed through calibration [42.t E ˙ Eˆ = f f (42) (43) (44) S ˙ E qest. Sources of interference ﬁxed in the sensor frame.t S ˙ ˆ E q . 44. S q ω .t − β S q . 45].t-1 + S qest. ˆ J T (S q est. metal furniture and metal structures within a buildings construction [40. The ﬁlter computes S qest E ˙ E ˙ as the rate of change of orientation measured by the gyroscopes. 43. Figure 2 shows a block diagram representation of the complete orientation ﬁlter implementation for an IMU.t−1.t = S qest. with the magnitude E ˙ ˙ of the gyroscope measurement error.dt q q z−1 S ˆ E q est. S q . termed hard iron biases.

t ∆t S The compensated gyroscope measurements. E bt . Mahony et al [35] showed that gyroscope bias drift may also be compensated for by simpler orientation ﬁlters through the integral feedback of the error in the rate of change of orientation. Eˆ ∗ ˆ ht = 0 hx hy hz = S qest.t-1 Eˆ Eˆ Eˆ (45) (46) bt = 0 h2 + h2 0 hz x y Compensating for magnetic distortions in this way ensures that magnetic disturbances are limited to only aﬀect the estimated heading component of orientation. Declination errors.t = ζ t S ω . those in the horizontal plane relative to the earth’s surface. This would yield the compensated gyroscope measurements S ωc . a potential disadvantage of other orientation ﬁlter designs [17. can S ˆ be computed as the normalised magnetometer measurement. E ht . An advantage of Kalman-based approaches is that they are able to estimate the gyroscope bias as an additional state within the system model [26.5 Gyroscope bias drift compensation The gyroscope zero bias will drift over time. 15. is represented by the DC component of S ω and so may removed as the integral of S ω weighted by an appropriate gain. cannot be corrected without an additional reference of heading. However. ˙ The normalised direction of the estimated error in the rate of change of orientation.t Eˆ Eˆ S (47) (48) (49) ωb. S ω. in equation (11). Eˆ may be expressed as the angular error in each gyroscope axis using equation (47). as shown in equations (48) and (49). as described by equation (45).t = S ωt − S ωb. This is achieved by computing bt as ht normalised to have only components in the earth frame x and z axes. The approach also eliminates the need for the reference direction of the earth’s magnetic ﬁeld to be predeﬁned. The ﬁrst element of S ωc is always assumed to be 0. 3. ζ. may be compensated for as the accelerometer provides an additional measurement of the sensor’s attitude. 30.t .t-1 ⊗ S mt ⊗ S qest. The gyroscope bias. A similar approach will be used here. can ˆ be corrected if the ﬁlter’s reference direction of the earth’s magnetic ﬁeld.t-1 ⊗ S q . E ht . is of the same Eˆ Eˆ inclination. The E ˆ eﬀect of an erroneous inclination of the measured direction earth’s magnetic ﬁeld. mt . Any practical implementation of an IMU or MARG sensor array must account for this. as described by equation (46). S qest. derived as the inverse to the relationship deﬁned in equation (11). S q . The magnitude of the angular error in 12 ωc. 24]. S ωb . S ∗ ˙ ω .t = 2S qest. rotated by the estimated ˆ orientation of the sensor provided by the ﬁlter. those in the vertical plane relative to the earth’s surface. may then be used in place of the of the gyroscope measurements. with temperature and with motion. ˆ The measured direction of the earth’s magnetic ﬁeld in the earth frame at time t.t-1 . 24]. S ωc .the earth’s magnetic ﬁeld. Inclination errors.

dt q q z -1 S ˆ E q est. Similarly. S a. S ˆ E q est. signal aliasing. The ﬁlter gain ζ represents the rate of convergence to remove gyroscope measurement errors which are not mean zero. quantisation errors.t h2 + h2 x y 0 hz 0 Group 1 ˆ Magnetometer S mt ˆ Accelerometer S at .t Figure 3: Block diagram representation of the complete orientation ﬁlter for an MARG implementation including magnetic distortion (Group 1) and gyroscope drift (Group 2) compensation 3. Figure 3 shows a block diagram representation of the complete ﬁlter implementation for a MARG sensor array. sensor axis nonorthogonality and frequency response characteristics. ζ may be described by equation (51). E bt )f g. As this process requires the use of the ﬁlter estimate of a complete orientation. Therefore the integral gain ζ directly deﬁnes the rate of convergence of the estimated gyroscope bias.b E ˆ ˙ 2S q ∗ -1 ⊗ S q Eˆ E ˆ est.t-1.dt ζ ˆ ˆ ˆ ˆ J T (S q est. expressed as the magnitude of a quaternion derivative. S qest . Using the relation˙ ˆ ship described by equation (11).t-1 ˆ ⊗ S m t ⊗ S q ∗ -1 E ˆ est. The sources of error include: sensor noise. S ωb . ˙ where ωβ represents the estimated mean zero gyroscope measurement error of each axis and ˜ ωζ represents the estimated rate of gyroscope bias drift in each axis. sensor miss-alignment. These errors represent the gyroscope bias. also expressed as the magnitude of a quaternion derivative.each axis.b (S q est.t-1 S ˙ E q est. β may be deﬁned by equation (50) where q is any unit quaternion.t . E bt .t 2 E est.t f f β Group 2 . it is only applicable to a MARG implementation Eˆ of the ﬁlter. 13 . It is convenient to deﬁne β and ζ using the angular quantities ωβ and ωζ respectively. expressed as the magnitude of a quaternion derivative. including the magnetic distortion and gyroscope bias drift compensation.t-1. S ω is equal to a quaternion derivative of unit length.t z -1 Gyroscope S ω t 1S ˆ q ⊗ S ω c. calibration errors.6 Filter gains The ﬁlter gain β represents all mean zero gyroscope measurement errors. S m) Eˆ g.

The device and accompanying software oﬀer a mode of operations where raw sensor data may be logged at a rate of 512 Hz and then post-processed to provide calibrated sensor measurements. To measure the orientation of the sensor. Additional optical markers were placed at arbitrary but dissimilar positions along the lengths of the rods to break the rotational symmetry and aid the identiﬁcation of each rod within the measurement data. The calibrated sensor measurements could then be processed by the proposed ﬁlter to provide the estimated orientation of the sensor. The platform was constructed from an aluminium central hub. The Cartesian positions of IR reﬂective optical markers ﬁxed to the measurement subject may then be computed in the coordinate frame of the camera array. The cameras are ﬁxed at calibrated positions and orientations so that the measurement subject is within the ﬁeld of view of multiple cameras. The software also incorporates a propriety Kalman-based orientation ﬁlter to provide an additional estimate orientation. accelerometers and magnetometers. evenly distributed around the perimeter of a 4 m by 4 m enclosure. the performance of each algorithm could be evaluated relative to one-another. Each camera was orientated to face toward the centre of the room. Experiments were conducted with the measurement subject in the centre of the room at a height of approximately 1 m.β= 1 ˆ ˜ ˜ ˜ q ⊗ 0 ωβ ωβ ωβ 2 ζ= 3˜ ωζ ˙ 4 = 3 ωβ ˜ 4 (50) (51) 4 4. 4. mutually orthogonal rods. The system is an array of IR (Infrared) sensitive cameras with incorporated IR ﬂood lights. rigidly connected at the central position along each length. Optical markers were positioned at both ends of each rod and the orientation sensor ﬁxed to a platform at the point where the rods coincide. approximately 30◦ to 60◦ to horizontal. it was ﬁxed to an optical orientation measurement platform speciﬁcally designed for this application. The system was used to log the positions of optical markers at a rate of 120 Hz. carbon ﬁbre rods and assembled using adhesives to ensure that it had no magnetic properties that may interfere with the orientation sensor’s magnetometer. As both the Kalman-based algorithm and proposed ﬁlter’s outputs could be computed using identical sensor data.2 Orientation from optical measurements The orientation measurement platform is comprised of 3 500 mm. independent of sensor performance. consisting of 8 MX3+ cameras connected to an MXultranet server [46] and Nexus [47] software was used to provide reference measurement of the orientation sensor’s actual orientation. A Vicon system.1 Experimentation Equipment The ﬁlter was tested using the xsens MTx orientation sensor [18] containing 16 bit resolution tri-axis gyroscopes. Figure 4 14 . Cameras were ﬁxed at a height of approximately 2.5 m.

shows an annotated photograph of the orientation measurement platform where C istart . and C zM within the camera frame representing the direction the x. These positions may be used to deﬁne 3 mutually orthogonal unit vectors. C jstart . C jend . C yM . (53) and (54). The optimal M ˆ quaternion C q is found as the normalised Eigen vector corresponding to the maximum M Eigen value of K. C ˆ ˆ ˆ xM . the rotation matrix deﬁned by equation (55) cannot be considered orthogonal and so does not represent a pure rotation. as described by equations (52). K. as shown in equation (55). These vectors deﬁne the rotation matrix describing the orientation of the measurement platform in the camera frame. Bar-Itzhack provides a method [48] where by an optimal ‘best ﬁt’ quaternion may be extracted from an imprecise and non-orthogonal rotation matrix. C iend . y and z axes of the orientation measurement platform coordinate frame. where rmn corresponds to the element of the mth row and nth column of C R. The method requires the construction of the symmetric 4 by 4 matrix. Equation (57) deﬁnes the optimal quaternion accounting the alternative 15 . deﬁned by equation (56). C kstart and C kend deﬁne the measured position of each marker within the camera frame. C C z end j end Sensor C xstart C C xend j start C z start Figure 4: Photograph of the orientation measurement platform C C ˆ xM = ˆ yM = iend − C istart Ci C end − istart (52) (53) (54) (55) C C jend − C jstart Cj C end − jstart C C ˆ zM = = C kend − C kstart Ck C end − kstart C C MR ˆ xM ˆ yM C ˆ zM Due to measurement errors and tolerances in the marker frame construction.

41]. Once these quantities are found the optical measurement of the sensor frame orientation in the earth frame. Figure 60 shows an annotated photograph of the compass where C cstart and C cend deﬁne the position of the optical markers 16 . z1 C C ¯ ¯ pend − pstart C C z2 ˆ ˆ = zE = (59) p= C ¯ ¯ pend − C pstart z3 The direction of earth’s magnetic ﬁeld was measured using a magnetic compass constructed from a 1 m carbon-ﬁbre rod with neodymium magnets ﬁxed to each end. where v1 . The direction of gravity was measured using a pendulum constructed from a Eˆ 1 m length of cotton thread with a small weight ﬁxed to one end. The compass was hung from a 1 m length of cotton thread and left to come to rest.3 Calibration of frame alignments In order to compare the optical measurement of the platform orientation in the camera frame. v3 and v4 deﬁne elements of the normalised Eigen vector. v2 . it is necessary to know the alignment of the earth frame relative to the camera frame. but oﬀ-set form the rod’s axis. Figure 5 shows an annotated photograph of the pendulum where C pstart and C pend deﬁne the position of the optical markers in the camera frame. C q. E qest . Measurements of these ﬁelds in the camera frame can be used to deﬁne the alignment C q. as shown in equation (59). Optical markers were ﬁxed to either end of the rod as well as to an arbitrary position along the length. polarising each end to either magnetic north or south. little discussion is oﬀered on the calibration of these two quantities.quaternion element order convention assumed by the method. S qmeas . This directly deﬁnes C ˆ the earth frame z axis in the camera frame. 24. zE . The mean position of each marker over a period of ˆ time deﬁnes the direction of the pendulum in the camera frame. Additional optical markers were required at arbitrary ﬁxed positions relative to the static pendulum to break the rotational symmetry of the optical marker constellation. and the orientation ﬁlter’s estimated orientation of the earth in the sensor frame. C S ˆ ˆ M q. M q. may be deﬁned by equation (58). C p. to beak the rotational symmetry optical marker constellation. S ˆ E qmeas = Cq ⊗ Mq ⊗ S q Eˆ C ˆ Mˆ (58) The earth frame x and z axes are deﬁned by the earth’s magnetic and gravitational ﬁelds respectively. Optical markers where ﬁxed at either end of the length of cotton and the pendulum left to come to rest. Although the use of optical measurement Eˆ equipment in this application is documentaed [26. Eˆ S ˆ and the alignment of the measurement platform relative to the sensor frame. r11 − r22 − r33 r21 + r12 r31 + r13 r23 − r32 1 r21 + r12 r22 − r11 − r33 r32 + r23 r31 − r13 K= (56) r32 + r23 r33 − r11 − r22 r12 − r21 3 r31 + r13 r23 − r32 r31 − r13 r12 − r21 r11 + r22 + r33 C ˆ Mq = v4 v1 v2 v3 (57) 4.

This can then be removed from C c to deﬁne ˆ ˆ computed as the vector projection of c 17 ¯ ¯ cend − C cstart Cc Cc ¯end − ¯start (60) .C pend C pstart Figure 5: Photograph of the pendulum and optical markers used to measure the direction of gravity in the camera frame in the camera frame. C c ˆ cannot be assumed to be orthogonal to the direction of gravity deﬁned by C p and so cannot ˆ be used to directly deﬁne the earth x axis. as shown in equation (60). The mean position of each marker over a period of time deﬁne the ˆ direction of the compass in the camera frame. C cstart C cend Figure 6: Photograph of the magnetic compass and optical markers used to measure the direction of the earth’s magnetic ﬁeld in the camera frame C C ˆ c= ˆ Due to measurement errors and the imbalance of the suspended magnetic compass. The non-orthogonal component of C c can be C ˆ on C p. C c.

The alignment of the earth frame ˆ ˆ ˆ may be deﬁned as the rotation matrix C R. C C ˆ xE = C c − C C ˆ xE = xE E Cx ˆ The earth frame y axis in the camera frame. was E¯ computed for the measurement platform held stationary for a period of approximate 10 seconds. this deﬁnes the earth x axis. E q. The proposed ﬁlter’s gain β was set to 0. constructed from C xE . C yE . with the optical measurement data interpolated to match the higher sampling rate of the orientation sensor data. an initial value of 2. as equation (65).the the earth x axis direction in the camera frame. The raw orientation sensor data was then processed by the accompanying software to provide the calibrated sensor data and Kalman-based algorithm output. S qKalman . C yE . The E C ˆ quaternion representation.5 was used for the ﬁrst 10 seconds of any experiment to ensure the convergence of algorithm states from initial conditions. as shown in equation (61). C zE · C yE = 0 2 2 2 ± 1 − x3 − z3 C ER ˆ ˆ c · CpC ˆ p Cp 2 ˆ x1 x2 = x3 (61) (62) = C ˆ xE C ˆ yE C ˆ zE (64) In order to ﬁnd the alignment S q it is assumed that the static error of the orientation Mˆ ˆ ﬁlter’s Kalman-based algorithm is mean zero. Once ˆ normalised. 18 .4 Experimental proceedure The optical measurement data and raw orientation sensor data were logged simultaneously. C xE . This was used with the alignment E q and optical measurement C q to deﬁne the Mˆ Cˆ ˆ alignment of the measurement platform in the sensor frame. and C zE .3.033 for the IMU implementation and 0. The mean algorithm’s output. This data then synchronised the optical measurement data. can then be extracted form this using Bar-Itzhack’s method [48]. as shown in equation (62). M S ˆ Mq ˆ = C q ⊗ E q ⊗ S qKalman Mˆ Cˆ E¯ (65) 4. Trials summarised in section 5. can be calculated as the vector that is ˆ ˆ orthogonal to both C xE and C zE and so be deﬁned by equation (63) where signs are chosen to satisfy the relative axis direction direction convention.041 for the MARG implementation. 2 ± 1 − x2 − z1 1 C 2 ˆ ˆ ˆ ˆ ˆ (63) yE = ± 1 − x2 − z2 . The calibrated sensor data was then processed through both the IMU and MARG implementations of the proposed orientation ﬁlter and calibrated orientation measurements were extracted from the optical measurement data using the methods described in sections 4. S q. found these values to provide optimal performance. was set to 0 as the calibrated orientation sensor data was not subject to gyroscope bias drift. C xE . applicable to the MARG implementation of the proposed ﬁler. The gain ζ. C xE · C yE = 0.2 and 4.3. However.

The measurement platform was initially held stationary for 20 to 30 seconds to allow time for algorithm states to converge to steady-state values. Pitch. An Euler angle representation has the advantage that the decoupled angles may be more easily interpreted or visualised. This sequence was then repeated around the y and then z axes. In each ﬁgure. then 180◦ in the opposite direction. were computed as the diﬀerence between the Euler parameters of the calibrated optical measurements and those of each of the corresponding estimated values. 21] to quantify orientation sensor performance as the static and dynamic RMS (Root-Mean-Square) errors in the decoupled Euler parameters describing the pitch.Data was obtained for a sequence of rotations preformed by hand. and the proposed ﬁlter estimated angle. 5. The peak angular rate measured during each rotation was between 110◦ /s and 190◦ /s. (8) and (9). 19 . roll. The errors of estimated Euler parameters. corresponding to the calibrated optical measurements of orientation. 5 Results It is common [24. The platform was held stationary for 3 to 5 seconds between each rotation. 19. φ.1 Typical results Figures 7. roll. The experiment was repeated 8 times to compile a dataset representative of system performance. the 3 traces of the upper plot represent the optically measured angle. The platform was then rotated 90◦ around its x axis. and z axis respectively. 8 and 9 show results typical of the 8 experiments for both the Kalman-based algorithm and the proposed ﬁlter MARG implementation. φ . and then 90◦ to bring the platform back to the starting position. The 2 traces of the lower plot represent the calculated error in each of the estimated angles. 20. 26. A total of 4 sets of of Euler parameters were computed. the Kalman-based algorithm estimated orientation and the proposed ﬁlter estimates orientation for both the MARG and IMU implementations. The disadvantage of an Euler representation is that it fails to described the coupling between each of the parameters and will subject to large and erratic errors if the Euler angle sequence reaches a singularity. ψ correspond to rotations around the sensor frame x. θ and heading. the Kalman-based algorithm estimated angle. 18. and heading components of an orientation. θ and ψ . Euler parameters were computed directly from quaternion data using equations (7). y.

Measured and estimated angle φ 100 50 0 −50 −100 22 Measured Kalman Proposed filter φ (degrees) 24 26 28 30 32 Error 34 36 38 40 42 44 3 2 φε (degrees) 1 0 −1 −2 −3 22 24 26 28 30 32 34 time (seconds) 36 38 40 42 44 Kalman Proposed filter Figure 7: Typical results for measured and estimated angle φ (top) and error (bottom) Measured and estimated angle θ 100 50 0 −50 −100 Measured Kalman Proposed filter θ (degrees) 42 44 46 48 50 52 Error 54 56 58 60 62 5 θε (degrees) Kalman Proposed filter 0 −5 42 44 46 48 50 52 time (seconds) 54 56 58 60 62 Figure 8: Typical results for measured and estimated angle θ (top) and error (bottom) 20 .

668◦ 1.847 0. Each value.625◦ 0.073◦ N/A ◦ 1. This was to prevent errors due to initial convergence or singularities in the Euler angle representation from corrupting results. when θ = ±90.497◦ ◦ ◦ 0. The manufacturer of orientation sensor specify the typical performance of the Kalman-based algorithm as having a static RMS error of < 0.769◦ 0.502◦ 0. that is.789◦ 0.623◦ 0.110◦ N/A Table 1: Static and dynamic RMS error of Kalman-based algorithm and proposed ﬁlter IMU and MARG implementations Results indicate that the proposed ﬁlter achieves higher levels of accuracy than the Kalman-based algorithm. represents the mean of all 8 experiments.Measured and estimated angle ψ 100 50 0 −50 −100 Measured Kalman Proposed filter ψ (degrees) 60 65 70 Error 75 80 5 ψε (degrees) Kalman Proposed filter 0 −5 60 65 70 time (seconds) 75 80 Figure 9: Typical results for measured and estimated angle ψ (top) and error (bottom) 5.594◦ 0. as indicated in ﬁgures 7.819◦ 0. compensate for an accumulating error in this parameter.5◦ in φ and 21 . θ .581◦ 0.344 1. Euler parameter RMS[φ ] static RMS[φ ] dynamic RMS[θ ] static RMS[θ ] dynamic RMS[ψ ] static RMS[ψ ] dynamic Kalman-based Proposed ﬁlter Proposed ﬁlter algorithm (MARG) (IMU) 0.668 0. Values of RMS[ψ ] were not computed for the IMU implementation of the proposed ﬁlter as the ﬁlter cannot. and ψ were calculated where a static state was assumed when the measured corresponding angular rate was < 5◦ /s. This threshold was chosen to be suitably high enough above the noise ﬂoor of the data. Each RMS value was calculated for the period of time framing only the rotation sequence of the corresponding Euler parameter. and a dynamic state when ≥ 5◦ /s. The results are summarised in table 1. nor is intended to.150◦ 1.2 Static and dynamic performance The static and dynamic RMS values of φ . 8 and 9.

2 1 0. suggests an increased heading error.3 0. relative to the 30 Hz and 40 Hz bandwidth of the accelerometer and gyroscope respectively.033 mean[ RMS[φ ]. on the performance of the proposed ﬁlter IMU (left) and MARG (right) implementations 22 . and roll. φ . φ. The inclination of the earth’s magnetic ﬁeld during testing was between 65◦ and 70◦ to the horizontal [39]. Other studies [49] have shown that accuracy may far less than that quoted by the manufacture and that quoted levels of accuracy are only achieved once the devices is recalibrated. As a consequence the component of the magnetic ﬂux vector available as a reference of heading is relatively small.4 0.6 1.4 1. on the proposed ﬁlter performance are summarised in Figure 10. hence errors in pitch.4 β = 0.2 1 0. Effect of β on proposed filter performance (IMU) 2 1. β. θ . RMS[θ ] ] (degrees) 1. The static and dynamic performance is quantiﬁed as the mean of the corresponding RMS values of φ and θ . RMS[ψ ] ] (degrees) Static performance Dynamic performance 2 1. and roll.5. alongside the reference measurement of gravity. and ψ [18]. These values do not conform with those listed in table 1.4 1. in dynamic conditions. 5.6 0. There is a clear optimal value of β for each ﬁlter implementation. high enough to minimises errors due to integral drift but suﬃciently low enough that unnecessary noise is not introduced by large steps of gradient descent iterations.4 0. θ. The lower levels of accuracy in the heading. The larger component of the vector serves as an additional reference for pitch.6 1. for the IMU implementation and φ .2 0 0 0.θ. θ. The experimental data was processed though the separate proposed ﬁlter IMU and MARG implantations.1 0. RMS[θ ]. ψ . and a dynamic RMS error of < 2◦ in φ. performance The results of an investigation into the eﬀect of the ﬁlter adjustable parameter. β.3 Filter gain vs.8 0.2 0.2 0 0 0. and < 1◦ in ψ. ψ .8 mean[ RMS[φ ]. using ﬁxed values of β between 0 to 0.3 0.8 0.2 0.4 β = 0. The magnetometer is speciﬁed [18] as having a bandwidth of 10 Hz which.6 0.1 0.8 1. ψ . θ and φ for the MARG implementation. are to due characteristics of the sensor’s measurements of the earth’s magnetic ﬁeld.041 Static performance Dynamic performance Effect of β on proposed filter performance (MARG) ε ε β ε ε ε β Figure 10: The eﬀect of the adjustable parameter. may be expected to be less than those in heading.

To investiagate the propsoed ﬁlter’s ability to compensate for bias drift. performance The results of an investigation into the eﬀect of sampling rate on ﬁlter performance is summarised in Figure11. The ﬁlter gain ζ was set to 0 for the ﬁrst 10 seconds of each experiment whilst the ﬁlter states converge from intial conditions. ωx and a constant bias error of −0.2◦ /s/s was introduced to the gyroscope x axis measuremnts.4 Sampling rate vs. plotted against the actual. the ﬁtler can be seen to scuseﬀuly esitmate teh gyroscope biases witht eh rate limtied rate of convergese. The experimental data was processed though the separate proposed ﬁlter IMU and MARG implantations. using the previously deﬁned. After this a value of 0. A constant drift of 0. From these results. RMS[θε].015 was used corrisponding a maximum convergene rate of the esitmated gyroscope bias of 1◦ /s/s. Effect of sampling rate on proposed filter performance (IMU) 30 Static performance Dynamic performance mean[ RMS[φε].5 Gyroscope bias drift The calibrated gyrscope data used by the propsoed ﬁlter did not contain any bias errors.2◦ /s as added to the gyroscope x axis measuremnts. Figure 12 shows resutls typical of teh 8 experimetns. optimal values β where the experimental data was decimated to simulate sampling rates between 1Hz and 512 Hz. ωy . RMS[ψε] ] (degrees) 20 15 15 10 10 5 5 0 0 10 10 Sampling rate (Hz) 1 10 2 0 0 10 10 Sampling rate (Hz) 1 10 2 Figure 11: The eﬀect of sampling rate on the performance of the proposed ﬁlter IMU (left) and MARG (right) implementations 5.5. The ﬁlter performance under these conditions was constent with that described in Table 1. This level of accracuy may be suﬃcent be eﬀective for applications such as human motion capture. 23 . It can be seen from Figure11 that the propsoed ﬁlter acheives similar levels of performance at 50 Hz as at 512 Hz. errors were artiﬁcally introduced to the 8 experimental datasets. Both ﬁlter implemntation are able to acheive a static error < 2◦ and dynamic error < 7◦ while sampling at 10 Hz. showing the gyroscope x and y axis bias estimated by the ﬁlter. RMS[θε] ] (degrees) 25 Effect of sampling rate on proposed filter performance (MARG) 30 Static performance Dynamic performance 25 20 mean[ RMS[φε].

The IMU implementation requires 108 arithmetic operations each ﬁlter update. Both ﬁlters estimate the gyroscope measurement error as the gradient of a error surface created by the magnetometer and accelerometer measurements. The minimum acceptable value of β is limited by the gyroscope measurement error. Therefore the magnitude of the ﬁlter gain β may be chosen low enough that the divergence caused by the erroneous gravitational and magnetic ﬁeld observations is reduced to an acceptable level over the period. The use of large gains during the ﬁlter initialisation may also improve the ﬁlter’s convergence from initial conditions.b (estimated) degrees/s 15 20 25 30 35 40 seconds 45 50 55 60 Figure 12: Filter tracking of gyroscope bias drift 6 Discussion The derivation of the ﬁlter initially assumed that the accelerometer and magnetometer would only measure gravity and the earth’s magnetic ﬁeld. Bachman’s ﬁlter computes this using a Gauss-Newton approach which requires numerical diﬀerentiation and a matrix inversion. Appendix A and B describe the ﬁlter IMU and MARG implementations respectively. each implemented in C where the code has been optimised to reduce the number of arithmetic operations at the expense of data memory. when large accelerations are detected.b (estimated) ωy. This will the inﬂuence accelerometers and magnetometers have on the estimated orientation to be reduced during potential problematic periods. for example. accelerations due to motion will result in an erroneous observed direction of gravity and so a potentially corrupt the estimated attitude and local magnetic distortions may still corrupt the estimated heading.b (actual) ωx. In many applications it may be beneﬁcial to use dynamic values of gains β and ζ. In practise. The structure of the ﬁlter implantation for a MARG sensor array is similar to that proposed by Bachman et al [34].Gyroscope bias drift compensation 12 10 8 6 4 2 0 −2 −4 10 ωx.b (actual) ωy. As a result. and the MARG implementation requires 277 arith24 . In many applications it can be assumed that accelerations due to motion and local magnetic distortions are present for only short periods of time. the ﬁlter proposed in this paper provides a substantial reduction in computational load and enables the derivation of an optimal ﬁlter gain based on system characteristics. it was found that a β and β gain of 10 enabled the ﬁlter states to converge within 5 seconds when initiated with a gyroscope bias error of 1000 deg/s in each axis. For example. The ﬁlter proposed in this report uses an analytical derivation of the Jacobian and operates on a normalised gradient of the error surface.

low power hardware. These low computational requirements make the algorithm accessible for low power embedded systems systems enabling the use of low cost. Sensors and Actuators A: Physical. the ﬁlter performance was not evaluated for simultaneous rotations around more than one rotational axis and rotational velocities were limited to short periods and in magnitude. Novel aspects of the ﬁlter include: • Analytic derivation of the Jacobian matrix. Paul Coote. Jonghyuk Kim. The ﬁlter is based on a Newton optimization using an analytic formulation of the gradient that is derived from a quaternion representation of motion. which includes the magnetic distortion and gyroscope drift compensation. The implications of the low computational load and ability to operate at low sampling rates open a very wide range of new opportunities for the use of IMU and MARG sensor arrays in real-time applications. The ﬁlter also has great potential to alleviate computational load for applications that demand extremely high sampling rates. smaller platforms.metic operations per ﬁlter update. deﬁned by the gyroscope measurement error. Robert Mahony. and experimental testing have been detailed. magnetic distortion anf gyroscope bias drift compensation. [2] Sung Kyung Hong. July 2007. and Tarek Hamel. The experimental procedure used to evaluated the ﬁlter performances has a number of limitations. we have introduced a novel orientation ﬁlter. that signiﬁcantly ameliorates the computational load and parameter tuning burdens associated with conventional Kalman-based approaches. Applications where limited power or processing resources may be available are particularly well suited for the new ﬁlter. • The need to tune only one or two ﬁlter gains (β and ζ)). 25 . References [1] Mark Euston. 2003. Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (uav). The ﬁlter is both simple to implement and simple to tune. These limitations were necessary so that repeatable and quantiﬁable and practical. which eliminates signiﬁcant computational load. 107(2):109 – 118. Least squares curve ﬁtting and complex tuning processes are therefore eliminated. applicable to both IMUs and MARG sensor arrays. even with a full order of magnitude in reduction of sampling rate. The ﬁlter derivation. A complementary ﬁlter for attitude estimation of a ﬁxed-wing uav with a low-cost imu. Empirical testing and benchmarking has shown that the ﬁlter performs as well as a high quality commercial Kalman-based system. In 6th International Conference on Field and Service Robotics. 7 Conclusions In this work. allowing implementation at lower sampling frequencies and onto lower power.

2007. P. Optimal strapdown attitude integration algorithms. October 13–16. and the 1999 Annual Fall Meeting of the Biomedical Engineering Soc. (1):61– 66. May 22–24. and S. In Proc. and K. First Joint [Engineering in Medicine and Biology 21st Annual Conf. J. K. [16] H. M. IEEE Symposium on Computational Intelligence and Games. Lukowicz. [5] L. [7] S. and C. [9] Huiyu Zhou and Huosheng Hu. The Institution of Electrical Engineers. Control. Bannach. F. Using wearable sensors for real-time recognition tasks in games of martial arts . pages 33–36. T. 1999.267. [6] David H. Luinge and P. In Proc. Omnidirectional pedestrian navigation for ﬁrst responders. H. [13] N. Anavatti. E. 2009. Yazdi. 2002. [10] E. Heinz. I. 86(8):1640–1659. volume 13. Durrant-Whyte. March 30–April 3. Inertial navigation systems for mobile robots. Weston. [11] J. Flexnav: fuzzy logic expert rule-based position estimation for mobile robots on rugged terrain. January 1971. pages 185–194. March 22–22. volume 3. A. and Dynamics. Kunze. [14] R. A new mathematical formulation for strapdown inertial navigation. D. F. Navigation and Communication WPNC ’07. Foxlin. Kalman.an initial experiment. pages 98–102. March 2004. Baten. 1990. [8] H. IEEE International Conference on Robotics and Automation ICRA ’02. [15] E. pages 317–322. Estimation of orientation with gyroscopes and accelerometers. August 1998. volume 1. and P. Inclination measurement of human movement using a 3-d accelerometer with autocalibration. In Proc. [4] B. Kallapur. Bortz.] BMES/EMBS Conference. 26 . Inertial head-tracker sensor fusion by a complementary separate-bias kalman ﬁlter. pages 666 –671. Luinge. J. Ayazi. In Guidance. Petersen. Borenstein. Virtual Reality Annual International Symposium the IEEE 1996. 1960. Beauregard. S. Najaﬁ. In Proc. 12(1):112–121. 2004. Barshan and H. [12] M B Ignagni. E. Veltink. aug. 82:35–45. In Proc. volume 2. 11(3):328–342. page 844. 1996. M. pages 363–369. Veltink. Human motion tracking for rehabilitation–a survey. 4th Workshop on Positioning. A new approach to linear ﬁltering and prediction problems. Ojeda and J. Titterton and John L. June 1995. pages 1 – 18. Gruber. May 11–15. H. A robust gyroless attitude estimation scheme for a small ﬁxed-wing unmanned aerial vehicle. 2006. Strapdown inertial navigation technology.[3] A. 2008. Journal of Basic Engineering. Micromachined inertial sensors.

[23] Crossbow Technology. and J. CA 95134. Luinge. First Street. 13(3):395–405. 2009. Roman Kamnik.0 edition.D. L. E. H. The Netherlands.04 edition. Applied Mathematics and Computation. 2001. [22] PNI sensor corporation. [18] Xsens Technologies B. Unscented ﬁltering for spacecraft attitude estimation. april 2004. 7521 PR Enschede. volume 4. 2003. Medical and Biological Engineering and Computing. R. Bedford. R. [24] A. H. Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation. MA 01730. Spacepoint Fusion. T. IEEE/RSJ International Conference on Intelligent Robots and Systems. Aerospace and Electronic Systems. 26(4):536–542. Inc. Santa Rosa. M. August 2007. 153(4):567–575. Hayward. San Jose. Measuring orientation of human body segments using miniature gyroscopes and accelerometers. J. June 2004. AHRS400 Series Users Manual. 1. [19] MicroStrain Inc. 459 Hurricane Lane. 3DM-GX3 -25 Miniature Attutude Heading Reference Sensor. and dynamics. Markley. Suite 102. InertiaCube2+ Manual. and M. McGhee. preliminary edition. College Station. C. Powell. [25] H. [21] InterSense. 40(2):627 – 649. [20] VectorNav Technologies. R. February 2007. 36 Crosby Drive. May 2009. [27] Markus Haid and Jan Breitenbach. LLC.C. 2008. H. and Marko Topic. rev. July 2006. Inc. Sensors and Actuators A: Physical. Design of multi-sensor attitude determination systems. Baten. In Proc. M. VN -100 User Manual. Veltink. [30] D. [29] John L. [28] D. B. 138(2):411–420. 53(7):1346–1356. VT 05495 USA. USA. An extended kalman ﬁlter for quaternion-based orientation estimation using marg sensors. 2009. September 2005. Low cost inertial orientation tracking with kalman ﬁlter. Marko Jankovec. TX 77840 USA. Gebre-Egziabher. 1. Suite 101. Veltink. Xiaoping Yun. 27 . Quaternion-based extended kalman ﬁlter for determining orientation by inertial and magnetic sensing.[17] J. J. IEEE Transactions on. 43(2):273–282. J. Suite 150. Bachmann. Zyda. Calibration and data fusion solution for the miniature attitude and heading reference system. April 2006. control.V. 4145 N. Sabatini. Marins. pages 2003–2011. MTi and MTx User Manual and Technical Documentation. Luinge and P. Pantheon 6a. Williston. CA 95403-1084 USA. Journal of guidance. Crassidis and Landis F. [26] David Jurman. and P. Roetenberg. October 29–November 3. c edition. 133 Aviation Blvd.

[34] E. [41] C. [43] Michael J. H. 15(4):859–876.H. Solid State Electronics Center. In Proc. 1:404–420. [38] David R. 29(4):535–541. Introduction to Robotics Mechanics and Control.Q. pages 187–194. November 8–9. I. Nonlinear complementary ﬁlters on the special orthogonal group. Honeywell Inc. IEEE International Conference on Robotics and Automation ICRA ’04. W. and Y. Zyda. van der Helm W. [42] Speake & Co Limited. Yun. 12001 State Highway 55. 53(5):1203 –1218. L. Application note. H. Honeywell Inc. May 2003. MN 55441. February 2008. P. S. Academic Press. and M. Xiaoping Yun. Hamel. Neild.[31] N. T.C. Zyda. Estimation of upperlimb orientation based on accelerometer and gyroscope measurements. Plymouth. Applications of Magnetoresistive Sensors in Navigation Systems. Y. Pﬂimlin. Princeton University Press. J. 1999.-S.J. and C. Journal of Universal Computer Science. PhD thesis. McGhee Joseph M. Craig. Kang. Jones. Israel Institute of Technology. Magnetic distortion in motion labs.-S. A. 1999. 2005. 1994. Y.K. and J. Suh.T. J. R. de Vries. Peterson. [40] E. Autocalibration algorithms for FGM type sensors.E. Pratt Robert B. Orientation tracking for humans and robots using inertial sensors. 28 . Ketteringham. volume 2. 55(2):746–754. Veeger.T. [33] R. pages 1115–1122. Duman. implications for validating inertial magnetic sensors. volume 37 of International geophysics series. Gait & Posture. Caruso. and R. B. IEEE Transactions on. R. Mahony. Npsnet: Flight simulation dynamic modelling using quaternions.-J. Presence. Automatic Control. Bachmann. 2009. B. [36] J. Quaternions and Rotation Sequences: A Primer with Applications to Orbits. 2009. [37] John J.M. Pearson Education International. Phuong. [39] John Arthur Jacobs. S. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass. A. Novel methods for attitude determination using vector observations. Michael J. McGhee. [35] R. june 2008. R.H. 2 edition. In Proc. Kuipers. An investigation of the eﬀects of magnetic variations on inertial/magnetic orientation sensors. [32] Daniel Choukroun..-M. 1987. Cooke. U. Aerospace and Virtual Reality. The earth’s core. Ro. Bachmann. April 2004. X. IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA ’99. Usta. Hyde. P. Baten F.

SEq_3 = 0.0f.5f * SEq_2. Los Angeles. SEqDot_omega_2. November 2007. A IMU ﬁlter implementation optimised in C The following source code is an implementation of the orientation ﬁlter for an IMU.0f. // gyroscope measurements in rad/s float SEq_1 = 1. SEqHatDot_3. J_32. The implementation requires 40 bytes of data memory for global variables and 100 bytes of data memory for local variables during each ﬁlter update function call. New method for extracting the quaternion from a rotation matrix. [46] Vicon Motion Systems Limited. 2008. 11 divisions and 3 square roots).[44] J. float halfSEq_2 = 0. and Bradford W.0f. 19(2):87–102. SEq_4 = 0.14159265358979f * (5. 5419 McConnell Avenue. float a_y. 29 . Vicon Nexus Product Guide . Cardeira. Walmsley.0f / 4. SEqHatDot_4. // Math library required for ‘sqrt’ #include <math. Vasconcelos. (Engineering Note). Brodie.0f * SEq_1. SEqDot_omega_4. P. Oliveira. f_3. [49] M. 2006.Dec 2000.0f) * gyroMeasError // sampling period in seconds (shown as 1 ms) // gyroscope measurement error in rad/s (shown as 5 deg/s) // compute beta // Global system variables float a_x.0f * SEq_2. Calibration of strapdown magnetometers in magnetic ﬁeld domain. float twoSEq_2 = 2. // estimated direction of the gyroscope error // Axulirary variables to avoid reapeated calcualtions float halfSEq_1 = 0. Each ﬁlter update requires 109 scalar arithmetic operations (18 additions.h> // System constants #define deltat 0. a_y. USA. float a_z) { // Local system variables float norm. J_33.5f * SEq_4. 2004. December 2008.0f / 180. SEq_2 = 0. f_2.2 edition. AIAA Journal of Guidance. CA 90066. C. [45] Demoz Gebre-Egziabher. Gabriel H. David Powell. CA 90066. Computer Methods in Biomechanics and Biomedical Engineering.0f * SEq_3. 11(6):641–648. 1. a_z. 57 multiplications. [48] Itzhack Y Bar-Itzhack. // quaternion derrivative from gyroscopes elements float f_1. Vicon MX Hardware.5f * SEq_1. and W. G. J_14or21. J_13or22. float twoSEq_1 = 2. Parkinson. float twoSEq_3 = 2. 5419 McConnell Avenue. Control and Dynamics. Nov.5f * SEq_3. float a_x. float halfSEq_4 = 0.6 edition. [47] Vicon Motion Systems Limited. The static accuracy and calibration of inertial measurement units for 3D orientation. in C. // objective function elements float J_11or24. SEqDot_omega_3. In Navigation.Foundation Notes. // objective function Jacobian elements float SEqHatDot_1. and B.0f. Elkaim. volume 2. A. J. w_z. Journal of Aerospace Engineering. 20 subtracts. A geometric approach to strapdown magnetometer calibration in sensor frame. Elkaim. J_12or23. 1. F.0f) #define beta sqrt(3. USA. A. // estimated orientation quaternion elements with initial conditions void filterUpdate(float w_x. Guidance and Control of Underwater Vehicles. The code has been optimised minimise the required number of arithmetic operations at the expense of data memory. SEqHatDot_2. // vector norm float SEqDot_omega_1. w_y. Page. Los Angeles. float halfSEq_3 = 0. float w_z. // accelerometer measurements float w_x. Silvestre. 23(6):10851087. float w_y.001f #define gyroMeasError 3.

J_13or22 * f_1.a_x.// Normalise the accelerometer measurement norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z). SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2.twoSEq_2 * SEq_2 .twoSEq_3 * SEq_3 .0f .2f / 180. = halfSEq_1 * w_y . 14 divisions and 5 square roots). J_12or23 = 2. SEqHatDot_3 /= norm. The code has been optimised minimise the required number of arithmetic operations at the expense of data memory. f_3 = 1.0f) * gyroMeasDrift // Global system variables float a_x. // J_11 negated in matrix multiplication // J_12 negated in matrix multiplication // negated in matrix multiplication // negated in matrix multiplication // Compute then integrate the estimated quaternion SEq_1 += (SEqDot_omega_1 . J_14or21 = twoSEq_2.0f) * gyroMeasError #define zeta sqrt(3. f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 . SEq_2 /= norm. J_33 = 2. // Compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 .J_33 * f_3 . m_y.001f #define gyroMeasError 3. 57 subtracts. Each ﬁlter update requires 277 scalar arithmetic operations (51 additions. 155 multiplications. SEq_4 = 0. SEq_3 = 0. The implementation requires 72 bytes of data memory for global variables and 260 bytes of data memory for local variables during each ﬁlter update function call. SEq_4 /= norm. float m_x. derrivative deltat. in C.0f / 4.J_11or24 * f_1.(beta * SEqHatDot_4)) * // Normalise quaternion norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4). w_z.14159265358979 * (0. a_y.halfSEq_4 * w_z. w_y. J_32 = 2.0f) #define beta sqrt(3. SEq_3 /= norm. a_z. = halfSEq_1 * w_z + halfSEq_2 * w_y .(beta * SEqHatDot_1)) * SEq_2 += (SEqDot_omega_2 .0f * J_11or24. float SEq_1 = 1. deltat. SEqHatDot_3 = J_12or23 * f_2 . float b_x = 1. // Normalise the gradient norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4). J_11or24 = twoSEq_3. // Compute the SEqDot_omega_1 SEqDot_omega_2 SEqDot_omega_3 SEqDot_omega_4 quaternion derrivative measured by gyroscopes = -halfSEq_2 * w_x .0f / 4. } B MARG ﬁlter implementation optimised in C The following source code is an implementation of the orientation ﬁlter for a MARG sensor array including magnetic distortion and gyroscope drift compensation.a_y. SEqHatDot_2 /= norm. = halfSEq_1 * w_x + halfSEq_3 * w_z . // Compute the gradient (matrix multiplication) SEqHatDot_1 = J_14or21 * f_2 .halfSEq_4 * w_y.14159265358979 * (5. deltat.halfSEq_2 * w_z + halfSEq_4 * w_x.0f * J_14or21. J_13or22 = twoSEq_1. a_x /= norm. SEq_1 /= norm. SEq_2 = 0.(beta * SEqHatDot_3)) * SEq_4 += (SEqDot_omega_4 . // // // // // sampling period in seconds (shown as 1 ms) gyroscope measurement error in rad/s (shown as 5 deg/s) gyroscope measurement error in rad/s/s (shown as 0. float w_x. b_z = 0.twoSEq_1 * SEq_3 .halfSEq_3 * w_x.a_z.2f deg/s/s) compute beta compute zeta // // // // // accelerometer measurements gyroscope measurements in rad/s magnetometer measurements estimated orientation quaternion elements with initial conditions reference direction of flux in earth frame 30 .J_32 * f_3. SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 .0f / 180.0f) #define gyroMeasDrift 3. a_z /= norm. deltat. m_z.0f * SEq_4.(beta * SEqHatDot_2)) * SEq_3 += (SEqDot_omega_3 . SEqHatDot_4 /= norm.h> // System constants #define deltat 0. SEqHatDot_1 /= norm. a_y /= norm.halfSEq_3 * w_y . // Math library required for ‘sqrt’ #include <math.

float twom_z = 2. to estimate direction of the gyroscope error SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4).twob_zSEq_2.5f * SEq_4. J_52.0f * twob_zSEq_3. float SEq_2SEq_4 = SEq_2 * SEq_4. J_11or24 = twoSEq_3.0f * b_z * SEq_3. a_z. w_err_z. J_62. // estimate gyroscope biases error // Function to compute one filter iteration void filterUpdate(float w_x. J_42.0f * b_z * SEq_1.SEq_3 * SEq_3 . float SEq_3SEq_4.a_x. SEqHatDot_4. // float w_err_x. float m_y. w_bz = 0. float twob_z = 2. h_y.2. 31 . J_14or21. f_6. // float SEqDot_omega_1.0f * b_x * SEq_1. float twom_y = 2. J_61. // negated in matrix multiplication J_51 = twob_xSEq_4 . J_32. float twoSEq_4 = 2.5f .J_32 SEqHatDot_3 = J_12or23 * f_2 . // J_41. float SEq_1SEq_4. J_43. float twob_xSEq_1 = 2. J_51.J_11or24 * f_1 . SEqDot_omega_4. f_5 + J_63 * f_6. f_5 + J_62 * f_6.0f * SEq_3. float twom_x = 2. J_12or23.0f . // negated in matrix multiplication J_42 = twob_zSEq_4. // // axulirary variables to avoid reapeated calcualtions float halfSEq_1 = 0. // negated in matrix multiplication J_33 = 2.m_y.J_44 // normalise the gradient norm = sqrt(SEqHatDot_1 * SEqHatDot_1 = SEqHatDot_1 SEqHatDot_2 = SEqHatDot_2 * * * * f_4 f_3 f_1 f_4 + - J_51 J_42 J_43 J_54 * * * * f_5 f_4 f_4 f_5 + + + + J_61 J_52 J_53 J_64 * * * * f_6. J_54 = twob_xSEq_1 .0f * SEq_1. float halfSEq_4 = 0. float m_z) vector norm quaternion rate from gyroscopes elements objective function elements objective function Jacobian elements estimated direction of the gyroscope error estimated direction of the gyroscope error (angular) computed flux in the earth frame // compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 . J_62 = twob_xSEq_4 .0f * J_14or21.SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 .5f * SEq_1. SEqDot_omega_3. J_54. f_3. float twob_xSEq_2 = 2. // float J_11or24.twoSEq_2 * SEq_2 . float SEq_1SEq_2.0f * SEq_4.SEq_3 * SEq_3) . h_z.0f * twob_xSEq_4 . float m_x. // negated in matrix multiplication J_61 = twob_xSEq_3. f_5. float twob_zSEq_2 = 2. a_x /= norm.0f * m_x.0f * b_x * SEq_4. J_64. / norm. J_64 = twob_xSEq_2.float w_bx = 0. float twob_zSEq_4 = 2.0f * b_x * SEq_2.J_13or22 SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 . float twob_zSEq_1 = 2.0f * SEq_4. a_z /= norm.twoSEq_3 * SEq_3 .twob_zSEq_3. J_63 = twob_xSEq_1 . // float h_x. SEqDot_omega_2.0f * m_z. f_6.m_x. float w_z.0f * b_x. // J_11 negated in matrix multiplication J_12or23 = 2. SEqHatDot_2. float SEq_1SEq_3 = SEq_1 * SEq_3. f_4.SEq_1SEq_3) . float twoSEq_3 = 2.J_33 * f_3 . J_53 = twob_xSEq_2 + twob_zSEq_4.twoSEq_1 * SEq_3 .a_z.m_z. f_5 = twob_x * (SEq_2 * SEq_3 . w_err_y.0f * m_y. float { // local system variables float norm. J_44. float halfSEq_3 = 0.0f * twob_zSEq_2. float twoSEq_2 = 2. a_y /= norm. float SEq_2SEq_3. J_33.0f * twob_xSEq_3 + twob_zSEq_1. float w_y. J_32 = 2.twob_zSEq_2. J_63.5f .2. // normalise the magnetometer measurement norm = sqrt(m_x * m_x + m_y * m_y + m_z * m_z). // normalise the accelerometer measurement norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z). float twob_zSEq_3 = 2. // compute the gradient (matrix multiplication) SEqHatDot_1 = J_14or21 * f_2 . f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 .0f * SEq_2. float twob_xSEq_4 = 2. w_by = 0. J_43 = 2. f_3 = 1. J_13or22 = twoSEq_1. m_y /= norm. // negated in matrix multiplication J_52 = twob_xSEq_3 + twob_zSEq_1. f_4 = twob_x * (0. // float f_1.0f * J_11or24. float twoSEq_1 = 2. J_13or22.a_y.0f * b_x * SEq_3. float a_x. m_x /= norm.SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) . float twob_xSEq_3 = 2.0f * b_z. float twob_x = 2.J_41 SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 . // J_12 negated in matrix multiplication J_14or21 = twoSEq_2. // negated in matrix multiplication J_44 = 2. J_53. float halfSEq_2 = 0.5f * SEq_2. f_2.0f * b_z * SEq_2.5f * SEq_3. float a_y. // float SEqHatDot_1.0f * b_z * SEq_4. SEqHatDot_3. / norm. m_z /= norm.SEq_2 * SEq_2 . f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0. // negated in matrix multiplication J_41 = twob_zSEq_3.

(beta * SEqHatDot_2)) * SEq_3 += (SEqDot_omega_3 . h_x = twom_x * (0.5f .5f . // compute then integrate the estimated quaternion SEq_1 += (SEqDot_omega_1 . // normalise the flux vector to have only components in the x and z b_x = sqrt((h_x * h_x) + (h_y * h_y)). SEq_3 /= norm. SEq_3SEq_4 = SEq_3 * SEq_4. // compute the SEqDot_omega_1 SEqDot_omega_2 SEqDot_omega_3 SEqDot_omega_4 quaternion rate measured by gyroscopes = -halfSEq_2 * w_x .twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3. SEq_2SEq_3 = SEq_2 * SEq_3. deltat. SEq_1SEq_4) + twom_y * (0. deltat. w_by += w_err_y * deltat * zeta. // compute flux in the earth SEq_1SEq_2 = SEq_1 * SEq_2.SEq_4 * SEq_4) + twom_z * (SEq_3SEq_4 . // compute and remove the gyroscope baises w_bx += w_err_x * deltat * zeta. SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 .5f . rate deltat. SEqHatDot_4 = SEqHatDot_4 / norm.halfSEq_3 * w_y .twoSEq_3 * SEqHatDot_1 .SEqHatDot_3 = SEqHatDot_3 / norm. w_x -= w_bx. b_z = h_z.twoSEq_4 * SEqHatDot_2.halfSEq_3 * w_x. SEq_1SEq_4 = SEq_1 * SEq_4.(beta * SEqHatDot_4)) * // normalise quaternion norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4).halfSEq_2 * w_z + halfSEq_4 * w_x. SEq_2 /= norm. SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0. } 32 . SEq_1SEq_3 = SEq_1 * SEq_3.(beta * SEqHatDot_3)) * SEq_4 += (SEqDot_omega_4 .(beta * SEqHatDot_1)) * SEq_2 += (SEqDot_omega_2 . SEq_1 /= norm.twoSEq_2 * * SEqHatDot_3 + twoSEq_2 * * SEqHatDot_4 . = halfSEq_1 * w_x + halfSEq_3 * w_z . = halfSEq_1 * w_y . w_y -= w_by. deltat. SEq_4 /= norm.SEq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3).twoSEq_2 * gyroscope error SEqHatDot_1 . // compute angular w_err_x = twoSEq_1 w_err_y = twoSEq_1 w_err_z = twoSEq_1 estimated direction of the * SEqHatDot_2 . SEq_2SEq_4 = SEq_2 * SEq_4.twoSEq_4 * SEqHatDot_1.halfSEq_4 * w_y.SEq_3 h_y = twom_x * (SEq_2SEq_3 + h_z = twom_x * (SEq_2SEq_4 frame // recompute axulirary variables * SEq_3 .halfSEq_4 * w_z. = halfSEq_1 * w_z + halfSEq_2 * w_y . SEqHatDot_4 .SEq_2 * SEq_2 .SEq_1SEq_2).SEq_3 * SEq_3). w_z -= w_bz.SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 .SEq_2 * SEq_2 . w_bz += w_err_z * deltat * zeta.

- Aviation Legal TermsUploaded bysadanand
- Visnav Lecture NotesUploaded byNitin Deshpande
- 10.1.1.119Uploaded bySri Sai
- Full Text 01Uploaded byMuhamad Saipudin
- Module 12EUploaded byTy When
- Error Reduction Techniques for a MEMS Accelerometer-based Digital Input DeviceUploaded byMarkusMayer
- syllabi5th6Sep2016Uploaded byAyush Bihani
- Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic SensingUploaded bynguyen_mecha
- JEC2 paper 5-1Uploaded byphamvantang1
- APM Autopilot setup using simulinkUploaded byKiran Raghuram Iyengar
- Am or Us Op a Per 1Uploaded bynmf77
- UAV_Sim.pdfUploaded byJamal Alshawesh
- Quadcopter Dynamics, Simulation, And ControlUploaded byFajar Prima
- EN-SET-116(2011)-03Uploaded bySmita Pednekar
- A Efficient Orientation Filter for IMUs and MARG Sensor ArraysUploaded bywbblair3
- Lifting Analysis for Heavy Ship Hull Blocks Using 4 CranesUploaded byFerry H. Sitohang
- USLI Presentation - Task 7Uploaded byericccao
- 01338525Uploaded byLakshmi Kumari
- Orientation HistogramUploaded byhighhopehn
- Single Shooting and Multiple ShootingUploaded byDavid Yule
- Reflex Gyro BrochureUploaded byAyleen nuñez
- Aula 04 Din & Cont Vei Espaciais_EnglishUploaded byDiogo Martins
- II CM _SY_110112115544Uploaded bymayank13333
- IRI IntroductionUploaded byBridger Wang
- U Frame Support User DocumentationUploaded byHELIX2009
- 333Uploaded byأحمد دعبس
- caplibUploaded byibkhan80
- tmpA038Uploaded byFrontiers
- Design Reference Manual - Creating the ModelUploaded byManny Mendoza
- Movie MonitorUploaded byNaeem Gul