Proceedings of the 19th World Congress

The International Federation of Automatic Control

Cape Town, South Africa. August 24-29, 2014

Experimental Comparison of Sensor Fusion

Algorithms for Attitude Estimation
A. Cavallo, A. Cirillo, P. Cirillo, G. De Maria, P. Falco,
C. Natale, S. Pirozzi

Dipartimento di Ingegneria Industriale e dell’Informazione, Seconda

Università degli Studi di Napoli, Via Roma 29, 81031 Aversa, Italy
(e-mail: andrea.cirillo@unina2.it).

Abstract: Inertial Measurement Unit is commonly used in various applications especially as a

low-cost system for localization and attitude estimation. Some applications are: real-time motion
capture system, gait analysis for rehabilitation purposes, biomedical applications, advanced
robotic applications such as mobile robot localization and Unmanned Aerial Vehicles (UAV)
attitude estimation. In all the mentioned applications the accuracy and the fast response are the
most important requirements, thus the research is focused on the design and the implementation
of highly accurate hardware systems and fast sensor data fusion algorithms, named Attitude
and Heading Reference System (AHRS), aimed at estimating the orientation of a rigid body
with respect to a reference frame. A large number of different solutions can be found in the
literature, and an experimental comparison of the most popular is presented in this work. In
particular, the algorithm based on the gradient descent method and the algorithm based on
a nonlinear complementary filter are compared to a standard Extended Kalman Filter (EKF)
with the aim to show that a general method can easily compete with ad-hoc solutions and even
outperform them in particular conditions. In order to validate the estimation accuracy a Kuka
robot is used to compute the ground truth. Moreover, in order to estimate the computational
burden, the algorithms are implemented on an ARM-Cortex M4-based evaluation board.

Keywords: Sensor fusion, Extended Kalman Filter, Advanced Robotics, Attitude estimation

1. INTRODUCTION tri-axial gyroscope and a tri-axial accelerometer, which can

be employed in orientation estimation. The signal output
Inertial Measurement Unit (IMU) sensors are a technol- of low-cost IMU systems, however, is characterized by low-
ogy capable of estimating orientation of a rigid body so resolution signals subject to high noise levels as well as
they are largely used as an implementation of real-time general time-varying bias terms. Therefore, raw signals
motion capture systems to track the location and the must be processed to reconstruct smoothed attitude es-
body posture of people (see Ziegler et al. (2011), Prayudi timates and bias-corrected angular velocity measurements
and Doik (2012)) in contrast to optical solutions such through suitable sensor fusion algorithms. In fact, suitable
as (Falco et al., 2012), or to measure the joint angles for exploitation of acceleration measurements can avoid drift
gait analysis for rehabilitation purposes and biomedical caused by numerical integration of gyroscopic measure-
applications as well as for performance assessment of the ments. However, it is well-known that use of only these
aging population (see Zecca et al. (2013) and Bennett et al. two source of information cannot correct the drift of the
(2013). Also, because of the small size and low weight estimated heading, thus an additional sensor is needed,
that make it better suited to the purpose, the interest i.e., a tri-axial compass, which allows to obtain a correct
for IMU-based systems is growing in advanced robotic heading estimation. Several fusion methods have been
applications, i.e., localization and wheel slip estimation of proposed in the literature. Mahony et al. (2008) formulate
a skid steered mobile robot (Jingang et al. (2007)), po- the filtering problem as a deterministic observer posed
sition and attitude determination for unmanned airborne on the special orthogonal group SO(3) termed ’explicit
vehicles (UAVs) (Joong-hee et al. (2011)) and unmanned complementary filter’. Madgwick et al. (2011) present a
underwater vehicles (UUVs) (Kim et al. (2011)). In all computationally efficient orientation algorithm based on
these applications, in order to provide an added value optimized gradient descent algorithm designed to support
and to provide a valid alternative to the typical expensive a wearable inertial human motion tracking system for
tools such as optical camera track systems, high accuracy rehabilitation applications.
and high precision estimation of the device orientation are The contribution of this paper is to experimentally com-
required. So, the research is focusing on the design and the pare the most popular AHRS algorithms and to validate
implementation of sensor data fusion algorithms, named them using a robotic manipulator in order to define a
Attitude and Heading Reference System (AHRS), able to reliable ground truth. In particular, the standard EKF
estimate the orientation of a rigid body with respect to a framework and the methods proposed by Mahony et al.
reference frame. The IMU provides real-time readings of a

978-3-902823-62-5/2014 © IFAC 7585

19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

I2C/SPI digital output interface, 16 bit value data output

and three selectable ranges (±250, ±500, ±2000 dps) while
the ST LSM303DLHC offers a 3-axis magnetometer with
a full-scale from ±1.3 to ±8.1 Gauss and a 3-axis ac-
celerometer with ±2 g/±4 g/±8 g/±16 g selectable range,
16 bit data output and a I2C serial interface. The exper-
iments have been carried out by setting an acquisition
rate of 760 Hz, 1344 Hz, 220 Hz and a full-scale range of
±2000 dps, ±2 g, ±1.3 Gauss for the gyroscope, the ac-
celerometer and the magnetometer, respectively.
IMU sensor
2.2 Robot

The robot used to compute the ground truth for per-

Fig. 1. Experimental setup. forming the comparison among the attitude estimation
(2008) and by Madgwick et al. (2011) have been considered algorithms is a KUKA Youbot (Bischoff et al., 2011)
worthy of particular interest. Although most of the papers constituted by a 5-dof serial manipulator mounted on an
omnidirectional platform. In order to obtain a reference
in literature propose modified versions of the EKF, e.g.
(Kim et al., 2011), (Chen et al., 2012) and (Xia et al., attitude with a clear geometrical interpretation, the first
2008), the authors consider the standard EKF framework phase of the planned trajectory consists of rotating the
three joints with orthogonal axes of the robot individually,
still a valid option. The main reasons are its generality and
flexibility. In fact, this framework is particularly suitable while, in the second phase the joints are moved in a
to add and remove sensors without significantly changing coordinated fashion so as generate rotations about the roll,
pitch and yaw axes contemporarily. The joint angle values
the estimation algorithm, to take into account the different
reliability and accuracy of sensors on the basis of their are measured using the robot encoders with a sampling
statistical characteristics, and to easily exploit all the a frequency of 40 Hz.
priori knowledge on the involved signals.
3. SENSOR CALIBRATION
In the first phase, the AHRS algorithms are imple-
mented in Matlab/Simulink environment. The IMU is then Low-cost sensors have much lower performance character-
mounted on a KUKA robot by mechanically aligning the istics than high-end sensors for sophisticated applications.
sensor frame to the end-effector frame. The gyroscope, Therefore, an accurate calibration of such sensors is very
accelerometer, magnetometer and joint angles of the robot important for the compensation of their systematic errors,
are simultaneously acquired. By computing the direct i.e., bias and scale factor. Usually, accurate values of such
kinematics of the robot, the orientation of the end-effector parameters are not available from the manufacturer or
frame can be obtained. Thus, the estimated orientation they depend on the actual mounting of the MEMS compo-
and the accuracy of the AHRS algorithms can be validated nents, which limits the use of these sensors for those appli-
using the measured orientation. In the second phase, in cations that require high accuracy, such as human-machine
order to compare the computational burden of the AHRS interfaces, biomedical research and aerial robotics. To ob-
algorithms, they are implemented on an embedded sys- tain a satisfactory performance, it is necessary to use a
tem constituted by an ARM-Cortex M4. In this case, the proper calibration method that could be performed in the
required time to compute a single cycle of data sensor background (self-calibration) or off-line by the system.
acquisition and filter computation is measured.
Three-axis accelerometers and three-axis magnetometers
supplied for the consumer market are typically calibrated
2. EXPERIMENTAL SETUP by the sensor manufacturer using a six-element linear
model comprising a gain and offset in each of the three
The experimental setup is constituted by a KUKA robot axes. This factory calibration can change as a result of
and by a STM32F3Discovery board. The evaluation board the thermal stresses during soldering of the accelerome-
is based on the STM32F303VCT6 ARM-Cortex M4 micro- ter/magnetometer to the circuit board. Additional small
controller, a ST L3GD20 3-axis digital output gyroscope, errors, including rotation of the IC package relative to the
a ST LSM303DLHC MEMS system-in-package featuring circuit board and misalignment of the circuit board to
a 3D digital linear acceleration sensor and a 3D digital the final product, can be introduced during the soldering
magnetic sensor. In order to mechanically align the sensor and final assembly process. The original factory calibration
frame to the robot end-effector frame and in order to avoid will still be adequate for the vast majority of applications,
undesired rotations, the evaluation board is fixed to the however for professional applications this is not the case. In
robot gripper using a calibrated mechanical part. In Fig 1 addition, the magnetometer behaviour can be influenced
the experimental setup is reported. by the presence of hard-iron and soft-iron distortions that
cannot be foreseen by the manufactures but are strictly
2.1 IMU related to the application. Hard-iron interference is nor-
mally generated by ferromagnetic materials with perma-
The considered IMU is constituted by ST MEMS motion nent magnetic fields that are part of the hand-held device
sensors directly mounted on the STM32F3Discovery eval- structure. These materials could be permanent magnets or
uation board. The ST L3GD20 3-axis gyroscope offers an magnetized iron or steel. They are time invariant and their

7586
19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

effect is to bias the magnetic sensor outputs. A soft-iron accelerometer calibration

1.5
magnetometer calibration

without re−calibration without re−calibration

interference magnetic field is generated by the items inside 1.2
with re−calibration 1.4 with re−calibration

the hand-held device. They could be the electric traces on 1.3

k
1.2

f
kŷs k, kỹ s
f
the PCB or other magnetically soft materials.

kŷs k, kỹs
1.1
1.1

1 1

0.9
0.9

3.1 Accelerometer/Magnetometer Calibration 0.8

0.7
0.8
0.6

Calibration of accelerometers and magnetometers can be 0.7

1 2 3
samples
4 5
x 10
4
0.5
1 2 3
samples
4 5
x 10
4

reduced to 3D-ellipsoid fitting problems (Gietzelt et al.,

2013), (Camps et al., 2009). In the proposed calibration Fig. 2. Calibration error of the accelerometer (left) and
algorithm a six-parameter error model has been considered magnetometer (right) before and after re-calibration.
(Pedley, 2013). Denoting with y f = [yf 1 yf 2 yf 3 ]T a
generic sensor output, a re-calibration procedure can be 4. ATTITUDE ESTIMATION ALGORITHMS
applied to compute the same six calibration parameters as
the original factory calibration (a scale factor and an offset As explained in the introduction, in this work three atti-
for each channel) but then applied on top of the factory tude estimation algorithms have been compared, one based
calibrated output y f . The 3D fitting problem requires a on a standard EKF formulation and two specifically de-
set of measurements that should cover as much as possible signed to solve the problem. As opposed to what one would
the 3D space and, for the accelerometer calibration, it is expect from a specifically designed solution compared to a
necessary to carry out the measurements in a quasi-static more general approach, the performance of the EKF will
condition to avoid accelerations other than the gravity. be demonstrated better than the other two methods, at
The re-calibrated sensor output ŷ s , expressed in the sensor the price of a higher computational burden. Nevertheless,
frame Σs , become the generality of the approach keeps the “door open” to
ŷ s = Λ y sf − bs = Λy sf − Λbs


(1) further improvements that could come from the adoption
of additional sensor data without the need to completely
where Λ = diag{λ1 , λ2 , λ3 } > 0 and bs = [b1 b2 b3 ]T are re-design the estimation algorithm and especially re-tune
the scale factors and offsets, respectively. the algorithm parameters, which are based on the statistic
characteristics of each signal.
Objective of the calibration is to compute Λ and b such
that the ellipsoid becomes a unit sphere centered in the
origin, i.e., 4.1 Extended Kalman Filter
T T T
1 = ŷ s ŷ s = y sf Λ2 y sf + bsT Λ2 bs − 2y sf Λ2 bs (2) The proposed AHRS algorithm is based on a stan-
By introducing the following intermediate variables 1 dard EKF method differently from the Kalman-based
approaches proposed in Marins et al. (2001) and Saba-
d = 1 − bsT Λ2 bs (3) tini (2006), where modifications to the standard Bayesian
2 framework were introduced with different motivations.
Λ̄ = (1/d)Λ = diag{λ̄1 , λ̄2 , λ̄3 } (4) However, the modification proposed by Marins et al.
2 s s (2001) lead to a solution where handling of noise statistics
c = (1/d)Λ b = Λ̄b , (5)
is less trivial than in the standard EKF formulation, in fact
Eq. (2) can be written as noise rejection is delegated to a Gauss-Newton iterative
λ̄1
  algorithm, which is claimed less computational demanding
λ̄  than the standard EKF, but without a convincing evi-
1 = yf2 1 yf2 2 yf2 3 − 2yf 1 − 2yf 2 − 2yf 3  2  ,
 
(6)

λ̄3 dence. The modification proposed by Sabatini (2006) con-
c sists in introducing the gyroscopic measurement directly
in the state update equation rather than in the measure-
which can be easily solved by writing it for the entire ment equation, which implies the assumption of low noise
measurement set as a linear system to be solved via a least affecting the sensor so as to allow a linearization of the
square algorithm. The bias term bs can be immediately update function. Finally, the recent survey on nonlinear
computed from (5), while Λ2 can be computed by solving attitude estimation techniques (Crassidis et al. (2007))
the linear system obtained substituting Eq. (3) into (4). recognizes that EKF-based approaches are the most used
The algorithm has been applied to both the magnetometer for two main reasons, their proven reliability and the
and the accelerometer of the IMU used in the experimental ease of incorporation of further measurement sources that
setup and the results are reported in Fig. 2, where it can improve the quality of the estimate or even provide
is evident how the re-calibration allows to obtain lower estimate of further quantities, e.g. altitude and vertical
errors. In fact, after the re-calibration the standard devi- velocity using GPS, like in Xia et al. (2008). Moreover, the
ation of kŷ s k is 2.8% for the magnetometer and 3.1% for statistical characterization of the sensors required in the
the accelerometer, compared to the standard deviations of EKF framework allows tuning the algorithm parameters
kỹ sf k = ky sf k/ȳsf that are 17.9% and 5.2%, respectively, in a more straightforward fashion.
being ȳ sf the mean value of y sf . Let Σb be the base frame to which the orientation to
be estimated is referred and Σv be the body-fixed frame.
1 Note that d = 0 can always be avoided by artificially translating Without loss of generality, Σv is assumed aligned with the
the data. sensor frame Σs .

7587
19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

Given two frames Σd and Σc , the quaternion expressing r 1 = 1/ (I − r 3 r T3 )e1 (I − r 3 rT3 )e1 (14)
the orientation of Σd with respect to Σc is denoted by
T h iT r 2 = S(r1 )r 3 . (15)
T
Qd,c = q1d,c q2d,c q3d,c q4d,c = η d,c ǫd,c

, (7) Hence, the constant magnetic field in the base frame
b b
is computed as h̄ = R̄v hv (0) and the measurement
where η d,c = q1d,c is the scalar part and ǫd,c =
equation of the magnetometer is
[q2d,c q3d,c q4d,c ]T is the vector part. Note that a quaternion
0
     
is here represented as a 4 × 1 vector and two operators can 0 −1 0
= Q ∗ ∗ Q + , (16)
be applied to two quaternions, i.e. the standard matrix hv v,b
h̄
b v,b nm
product and the quaternion product defined as where nm is the measurement noise affecting magnetic
 T
η d,c η e,f − ǫd,c ǫd,c
 field measurements, assumed normally distributed.
Qd,c ∗ Qe,f = d,c e,f , (8)
η ǫ + η e,f ǫd,c + S(ǫd,c )ǫe,f Gyroscope. The gyroscope provides measurements of the
where S(·) is the 3 × 3 skew-symmetric operator of the angular velocity, which is related to the time derivative of
vector product. the quaternion through the equation
1 0 −ω vT
 
In order to simplify the notation, the orientation of Σv Q̇ = Q , E(ωv )Q. (17)
with respect to Σb will be expressed by omitting the frame 2 ωv −S(ω v )
superscripts, i.e., In order to take into account that the angular velocity is
T
Qv,b = [q1 q2 q3 q4 ] , Q, (9) affected by an additive measurement noise nω (assumed
normally distributed), it will be included both among the
which is the quantity to estimate. In order to setup the
measured variables and the variables to be estimated. For
EKF, the measurement equations are considered first.
implementation purposes, Eq. (17) can be written as a
Accelerometer. Under the following assumptions function of the discrete time k as
Qk+1 = Qk + Ts E(ω vk )Qk , (18)
• Σb has the z-axis perfectly vertical
where Ts is the sampling time.
• The vehicle is subject to low translational accelera-
tions and thus the accelerometer measures only the Now, the equations of the estimation filter are presented.
gravity acceleration vector
• The accelerometer is calibrated as in Section 3.1 such The estimation filter. Let us define the augmented state
T T
that av av = 1 and thus ab = [0 0 −1] as    
x Qk
where av is the measured acceleration expressed in the xk = 1k = . (19)
x2k ω vk
body-fixed frame and ab is the same acceleration expressed
Assuming the classical constant velocity model, the state
in the base frame, the measurement equation of the
update equation is
accelerometer can be obtained by using the standard  
quaternion formula to change the reference frame of a x + Ts E(x2k )x1k
xk+1 = f (xk ) + wk , 1k + wk (20)
vector as   x2k
   
0 −1 0 0 where wk is the process noise which is assumed unbiased,
= Qv,b ∗ b ∗ Qv,b + (10)
av a na uncorrelated with the state and with a covariance matrix
where na is the measurement noise affecting acceleration W with finite norm. In order to setup the EKF, the
measurements, assumed normally distributed. measurement equation is defined as
" v#
ωk x2k
" #
Magnetometer. Under the following assumptions y k = avk + v k = g(xk ) + v k , av (x1k ) + v k (21)
hvk hv (x1k )
• The magnetometer is calibrated as in Section 3.1 such
T where av (x1k ) is expressed as in (10), hv (x1k ) is expressed
that hv hv = 1 as in (16) and v k = [nTω nTa nTm ]T is the measurement
b
• h is constant and is computed as the average value noise, which is assumed normally distributed and with
b
h̄ measured at the beginning of any experiment positive definite covariance matrix V .
Since the initial orientation (at the time instant t = 0) The EKF equations are set in the classical 3-steps form as
of Σv with respect to Σb is unknown, but the base frame
has been chosen with a vertical z-axis, a rotation matrix • prediction step
b b
R̄v = Rbv (0) has to be computed to obtain h̄ from the x̂k+1|k = f (x̂k|k ), x̂0|0 = xi (22)
v
measurement h (0) expressed in frame Σv , i.e.,
b
P̂ k+1|k = F k P̂ k|k F Tk + W, P̂ 0|0 = W (23)
T
R̄v av (0) = [0 0 −1] (11) ∂f
v
where a (0) is the measured (averaged) acceleration in being Fk = (24)
∂x x̂k|k
t = 0. Denoting with
v
R̄b = [r1 r2 r3 ] , (12) • Kalman gain computation step
 −1
from (11) it results K k+1 = P̂ k+1|k GT
k+1 G k+1 P̂ k+1|k G T
k+1 + V (25)
r 3 = −av (0), (13)
∂g
while r1 and r2 can be easily computed to complete a being Gk = (26)
∂x x̂k+1|k
right-handed frame as

7588
19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

• update step 15
EKF error
15
EKF error
Roll Roll

10 Pitch 10 Pitch
x̂k+1|k+1 = x̂k+1|k + K k+1 y k − g(x̂k+1|k ) (27) Yaw Yaw

Error [degrees]

Error [degrees]
5 5

P̂ k+1|k+1 = (I − K k+1 Gk+1 ) P̂ k+1|k (28) 0 0

−5 −5
where xi is the initial state which is chosen with zero
−10 −10
angular velocity and orientation aligned with the base
−15 −15
frame, I is the 7×7 identity matrix, x̂k+1|k and P̂ k+1|k de- 0 10 20
Time [s]
30 40 0 5 10
Time [s]
15 20

note the expected value and covariance matrix estimates, Madgwick error Madgwick error
respectively, at time step k + 1 given the observations up 15
Roll
15
Pitch
to time step k. 10
Yaw
10

Error [degrees]

Error [degrees]
5 5

0 0
4.2 Madgwick Algorithm
−5 −5
Roll
−10 −10 Pitch
It is applicable to IMU’s consisting of tri-axis gyroscopes −15 −15
Yaw

and accelerometers, and magnetic angular rate and gravity 0 10 20

Time [s]
30 40 0 5 10
Time [s]
15 20

(MARG) sensor arrays that also include tri-axis magne- Mahony error Mahony error
tometers. The algorithm incorporates magnetic distortion 15 15
Roll
Pitch
compensation and it uses a quaternion representation, 10 10
Yaw

Error [degrees]

Error [degrees]
allowing accelerometer and magnetometer data to be used 5 5

in an analytically derived and optimised gradient descent 0 0

algorithm to compute the direction of the gyroscope mea- −5

Roll
−5

surement error as a quaternion derivative. The equations −10 Pitch

Yaw
−10

of the algorithms can be found in Madgwick et al. (2011). −15

0 10 20
Time [s]
30 40
−15
0 5 10
Time [s]
15 20

4.3 Non-Linear Complementary Filter Fig. 3. Orientation error: slow trajectory (left) and fast
trajectory (right).
Mahony et al. (2008) propose the orientation estimation
problem as a deterministic observation problem posed di- time derivative of the quaternion, which is exact except
rectly on the special orthogonal group SO(3). Thought the for the numerical integration error.
definition of a Direct Complementary Filter and a Passive The sole adjustable parameter of the algorithm recalled in
Complementary Filter they arrive to a reformulation of Section 4.2 has been chosen as the optimal value proposed
the complementary filter, named Explicit Complementary by Madgwick et al. (2011) for the MARG implementation,
Filter, in terms of direct vectorial measurements, such as i.e., β = 0.041. The gain parameters of the algorithm re-
gravitational or magnetic field directions obtained from called in Section 4.3 and implemented using the quaternion
an IMU. This observer does not require online algebraic representation, have been chosen as proposed by Mahony
reconstruction of attitude and is ideally suited for imple- et al. (2008), i.e., KP = 2 and KI = 0.6, which have been
mentation on embedded hardware platforms owing to its verified to be optimal also in this case.
low complexity. However, it suffers from possible disconti-
nuities in the bias correction signal when the equivalent The three algorithms have been implemented in Mat-
rotation angle of the estimated quaternion approaches lab/Simulink with a sampling time Ts = 2 ms, since the
±π rad that could result in systematic errors in the re- sensor data have been acquired from IMU at sampling
constructed attitude. frequency of 500 Hz, which is the frequency experimen-
tally found to guarantee the most reliable communica-
5. EXPERIMENTAL RESULTS tion. To compare the three AHRS algorithms, two robot
trajectories are considered. In the first (slow) trajectory,
an average speed of 18 deg/s is applied to robot joints,
The parameters of each of the three algorithms to be
while, in the second (fast) trajectory, the average speed
compared have been set as follows. For the EKF, the
is raised to 45 deg/s. The ground truth is computed using
measurement covariance matrix V has been estimated by
the direct kinematics of the robot and, thus, computing
a simple static acquisition of the sensor signals and the
the end-effector frame orientation (aligned to the body-
resulting values are
fixed frame) in terms of the unit quaternion Qv,r , being Σr
V = diag{2.9 · 10−5 , 2.3 · 10−5 , 3.2 · 10−5 , 3.1 · 10−5 , the robot base frame. Thus, the estimated orientation of
, the body-fixed frame resulting from the AHRS algorithms
4.5 · 10−5 , 5.5 · 10−5 , 2.0 · 10−3 , 2.1 · 10−3 , 2.0 · 10−3 }
while the entries of the process covariance matrix have Q̂v,b , is compared with the ground truth Qv,b (computed
been tuned so as to obtain a satisfactory response time as Qv,b (0) ∗ Qr,v (0) ∗ Qv,r (t)) and the orientation error
−1
and a good noise rejection, as well as to guarantee filter is calculated as the quaternion Q̃(t) = Q̂v,b (t) ∗ Qv,b (t).
convergence according to the result by Natale (2011), i.e., This orientation error is then expressed in terms of Euler
W = diag{10−10 , 10−10 , 10−10 , 10−10 , 10−3 , 10−3 , 10−3 }. angles in Roll-Pitch-Yaw representation and it is reported
in Fig. 3.
Note how the first four entries are significantly lower than
the others since the first four state update equations in (20) To better appreciate the response time and noise rejec-
are the kinematic relation between angular velocity and tion of the three algorithms, Fig. 4 shows the quaternion

7589
19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

1.5 1.5 1 x x
1 y
y
z
Quaternion components

Quaternion components
z

normalized magnetic field

normalized magnetic field

1 1 0.5
0.5

0.5 0.5 0
q1−Ground truth q −Ground truth 0
1
q −EKF q −EKF
1 1
0 q1−Madgwick 0 q −Madgwick −0.5 −0.5
1
q −Mahony q −Mahony
1 1
−0.5 −0.5 −1 −1
0 10 20 30 40 0 5 10 15 20
Time [s] Time [s] 0 5 10 15 20 25 30 35 40 0 5 10 15 20 25 30 35 40
Time [s] Time [s]

0.2 0.2

Fig. 5. Measured magnetic field without (left) and with

Quaternion components

Quaternion components
0 0
disturbance (right).
−0.2 −0.2

q −Ground truth q −Ground truth EKF error

−0.4 2 −0.4 2
q −EKF q −EKF 15
2 2 Roll
−0.6 q −Madgwick −0.6 q −Madgwick Pitch
2 2 10
q −Mahony q −Mahony Yaw
2 2

Error [degrees]
−0.8 −0.8 5
0 10 20 30 40 0 5 10 15 20
Time [s] Time [s]
0
0.2 0.2
−5
Quaternion components

Quaternion components

0 0 −10

−0.2 −0.2 −15

0 10 20 30 40
q −Ground truth q −Ground truth Time [s]
3 3
−0.4 −0.4
q −EKF q −EKF
3 3
Madgwick error
q −Madgwick q −Madgwick
−0.6 3 −0.6 3 15
q −Mahony q −Mahony Roll
3 3
10 Pitch
−0.8 −0.8
0 10 20 30 40 0 5 10 15 20 Yaw
Time [s] Time [s]

Error [degrees]
5

0.8 0.8 0
q −Ground truth q4−Ground truth
4
q −EKF
Quaternion components

Quaternion components

0.6 4 0.6 q −EKF −5

4
q −Madgwick q4−Madgwick
4
0.4 q −Mahony 0.4 q −Mahony −10
4 4

0.2 0.2
−15
0 10 20 30 40
Time [s]
0 0
Mahony error
−0.2 −0.2
15
0 10 20 30 40 0 5 10 15 20
Time [s] Time [s] 10
Error [degrees]

5
Fig. 4. Estimated and true attitude: slow trajectory (left) 0
and fast trajectory (right).
−5
Roll
Euler angles [◦ ] EKF Madgwick Mahony −10 Pitch
Yaw
Roll (static) 0.04 0.03 0.02 −15
0 10 20 30 40
Pitch (static) 0.01 0.05 0.05 Time [s]
Yaw (static) 0.30 1.92 1.85
Roll (slow) 4.71 4.85 5.07 Fig. 6. Orientation error: slow trajectory in the noisy case.
Pitch (slow) 1.91 2.65 2.89
Yaw (slow) 5.19 5.13 5.67 Euler angles [◦ ] EKF Madgwick Mahony
Roll (fast) 6.55 6.51 6.69 Roll 5.05 5.54 5.87
Pitch (fast) 2.83 3.34 2.85 Pitch 3.24 3.93 4.53
Yaw (fast) 6.71 7.07 6.92 Yaw 5.93 6.27 6.66
Table 1. Static and dynamic RMSE Table 2. Dynamic RMSE with noisy measure-
ments.
components of the estimated orientation for both trajec-
tories compared to the ground truth. To quantify the Fig. 5 reports the measured magnetic field in absence
algorithms performance, the static and dynamic RMSE and presence of the disturbance showing the high noise
(root-mean-square-error) still in terms of Euler angles have level. Executing the slow trajectory in such condition, the
been computed and reported in Tab. 1. The results of algorithms exhibit a degradation of the performance as it
the experiments show that in the slow trajectory, the can be appreciated in Fig. 6 and from the RMSE figures
three algorithms provide comparable results in terms of reported in Tab. 2.
accuracy. In terms of RMSE, the proposed EKF algorithm
As a further analysis, a comparison on the computational
provides a more accurate estimation in both static and
burden of the considered algorithms has been carried out.
dynamic conditions.
In particular, the algorithms have been implemented in
To test the capability of the algorithms to work under se- Matlab/Simulink environment on an Intel I7 quad-core
vere disturbance conditions a specific experiment has been processor at 1.6 GHz. The tic-toc Matlab functions have
carried out by intentionally actuating the robot gripper been used to estimate the execution time of a single cycle
with a periodic signal so as to generate an electromag- that includes the gyroscope, accelerometer and magne-
netic disturbance which mainly affects the magnetometer. tometer measurement and the attitude estimation. The

7590
19th IFAC World Congress
Cape Town, South Africa. August 24-29, 2014

Algorithm Matlab/Simulink [ms] Embedded System [ms] Gietzelt, M., Wolf, K.H., Marschollek, M., and Haux, R.
EKF 0.1 2.7 (2013). Performance comparison of accelerometer cal-
Madgwick 0.017 0.15 ibration algorithms based on 3d-ellipsoid fitting meth-
Mahony 0.014 0.11 ods. Computer Methods and Programs in Biomedicine,
Table 3. Computational burden estimation 3, 62–71.
Jingang, Y., Junjie, Z., Dezhen, S., and Jayasuriya, S.
superior performance of the EKF can be attributed to (2007). Imu-based localization and slip estimation for
the availability of a tunable parameter for each sensor skid-steered mobile robots. In IEEE/RSJ Int. Conf. on
measurement, which is paid in terms of a higher execu- Intelligent Robots and Systems (IROS), 2845–2850.
tion time. Finally, to validate the proposed results, the Joong-hee, H., Kwon, J., Impyeong, L., and Kyoungah,
algorithms have been coded for implementation on the C. (2011). Position and attitude determination for uav-
STM32F3Discovery evaluation board using Chibi/OS as based gps, imu and at without gcps. In Int. Workshop
real-time embedded operating system. To this aim, the on Multi-Platform/Multi-Sensor Remote Sensing and
ARM Cortex Microcontroller Software Interface Standard Mapping (M2RSM), 1–5.
(CMSIS) DSP library is used to implement the mathe- Kim, H.S., Choi, H.S., su Yoon, J., and Ro, P. (2011).
matical operations, i.e., matrix product and inverse. Ta- Study on ahrs sensor for unmanned underwater vehicle.
ble 3 reports the average time required to compute one Int. J. of Ocean System Engineering, 1, 165–170.
estimation cycle in both Matlab/Simulink environment Madgwick, S., Harrison, A., and Vaidyanathan, R. (2011).
and embedded system implementation. Even though the Estimation of imu and marg orientation using a gradient
execution times of the two fastest algorithms would allow descent algorithm. In IEEE Int. Conf. on Rehabilitation
to use a lower sampling time, this would lead to a negli- Robotics (ICORR), 1–7.
gible improvement due to the limited update rate of the Mahony, R., Hamel, T., and Pflimlin, J.M. (2008). Non-
magnetometer and gyroscope sensors. linear complementary filters on the special orthogonal
group. IEEE Trans. on Automatic Control, 53, 1203–
6. CONCLUSION 1218.
Marins, J., Yun, X., Bachmann, E., McGhee, R., and Zyda,
An experimental comparison of popular IMU-based al- M. (2001). Extended kalman filter for quaternion-based
gorithms for orientation estimation of a rigid body with orientation estimation using marg sensors. In Proc. of
respect to a reference frame is presented in this work. the 2001 IEEE/RSJ Int. Conf. on Intelligent Robots and
In particular, two specifically designed solutions are com- Systems, 2003–2011.
pared to a standard EKF algorithm, which outperforms Natale, C. (2011). Kinematic control of robots with noisy
the others, as experimentally demonstrated, at the expense guidance system. In Proc. of the 18th IFAC World
of a higher computational burden. However, its generality Congress, 6937–6944.
allows the user to add further sensory information without Pedley, M. (2013). High precision calibration of a three-
the need to completely re-design the estimation filter or to axis accelerometer - an4399. Technical report, Freescale
re-tune the parameters of the algorithm, which are simply Semiconductor.
based on the statistics of the noise affecting each measured Prayudi, I. and Doik, K. (2012). Design and implemen-
signal. tation of imu-based human arm motion capture system.
In Int. Conf. on Mechatronics and Automation (ICMA),
REFERENCES 670–675.
Sabatini, A. (2006). Quaternion-based extended kalman
Bennett, C., Odom, C., and Ben-Asher, M. (2013). Knee filter for determining orientation by inertial and mag-
angle estimation based on imu data and artificial neural netic sensing. IEEE Trans. on Biomedical Engineering,
networks. In Biomedical Engineering Conf. (SBEC), 53, 1346–1356.
111–112. Xia, L., Wang, J., and Liu, Y. (2008). Quadratic ekf
Bischoff, R., Huggenberger, U., and Prassler, E. (2011). algorithm enhancements for low cost tightly-coupled
Kuka youbot - a mobile manipulator for research and ahrs/gps. In 2nd Int. Symposium on Systems and
education. In IEEE Int. Conf. on Robotics and Au- Control in Aerospace and Astronautics, 1–6.
tomation (ICRA), 1–4. Zecca, M., Saito, K., Sessa, S., Bartolomeo, L., Lin, Z.,
Camps, F., Harasse, S., and Monin, A. (2009). Numerical Cosentino, S., Ishii, H., Ikai, T., and Takanishi, A.
calibration for 3-axis accelerometers and magnetome- (2013). Use of an ultra-miniaturized imu-based motion
ters. In IEEE Int. Conf. on Electro/Information Tech- capture system for objective evaluation and assessment
nology, 217–221. of walking skills. In 35th Annual Int. Conf. of the IEEE
Chen, S., Ding, C., Han, Y., Fang, Y., and Chen, Y. (2012). Engineering in Medicine and Biology Society (EMBC),
Study on information fusion algorithm for the miniature 4883–4886.
ahrs. In 4th Int. Conf. on Intelligent Human-Machine Ziegler, J., Kretzschmar, H., Stachniss, C., Grisetti, G.,
Systems and Cybernetics, 114–117. and Burgard, W. (2011). Accurate human motion
Crassidis, J.L., Markley, F.L., and Cheng, Y. (2007). A capture in large areas by combining imu- and laser-based
survey of nonlinear attitude estimation methods. J. of people tracking. In IEEE/RSJ Int. Conf. on Intelligent
Guidance, Control, and Dynamics, 30, 12–28. Robots and Systems (IROS), 86–91.
Falco, P., Maria, G.D., Natale, C., and Pirozzi, S. (2012).
Data fusion based on optical technology for observation
of human manipulation. Int. J. of Optomechatronics, 6,
37–70.

7591