See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/285392548

A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs

Article  in  International Journal of Navigation and Observation · December 2015

DOI: 10.1155/2015/503814

CITATIONS READS
18 2,084

2 authors:

Heikki Hyyti Arto Visala

Finnish Geospatial Research Institute FGI, National Land Survey of Finland Aalto University
16 PUBLICATIONS   45 CITATIONS    87 PUBLICATIONS   1,014 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Tubridi View project

COMBAT View project

All content following this page was uploaded by Heikki Hyyti on 01 December 2015.

The user has requested enhancement of the downloaded file.

Hindawi Publishing Corporation
International Journal of Navigation and Observation
Volume 2015, Article ID 503814, 18 pages
http://dx.doi.org/10.1155/2015/503814

Research Article
A DCM Based Attitude Estimation Algorithm for Low-Cost
MEMS IMUs

Heikki Hyyti and Arto Visala

Autonomous Systems Research Group, Department of Electrical Engineering and Automation, School of Electrical Engineering,
Aalto University, P.O. Box 15500, 00076 Aalto, Finland

Correspondence should be addressed to Heikki Hyyti; heikki.hyyti@aalto.fi

Received 16 July 2015; Revised 29 October 2015; Accepted 4 November 2015

Academic Editor: Aleksandar Dogandzic

Copyright © 2015 H. Hyyti and A. Visala. This is an open access article distributed under the Creative Commons Attribution
License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly
cited.

An attitude estimation algorithm is developed using an adaptive extended Kalman filter for low-cost microelectromechanical-
system (MEMS) triaxial accelerometers and gyroscopes, that is, inertial measurement units (IMUs). Although these MEMS sensors
are relatively cheap, they give more inaccurate measurements than conventional high-quality gyroscopes and accelerometers. To
be able to use these low-cost MEMS sensors with precision in all situations, a novel attitude estimation algorithm is proposed
for fusing triaxial gyroscope and accelerometer measurements. An extended Kalman filter is implemented to estimate attitude in
direction cosine matrix (DCM) formation and to calibrate gyroscope biases online. We use a variable measurement covariance for
acceleration measurements to ensure robustness against temporary nongravitational accelerations, which usually induce errors
when estimating attitude with ordinary algorithms. The proposed algorithm enables accurate gyroscope online calibration by
using only a triaxial gyroscope and accelerometer. It outperforms comparable state-of-the-art algorithms in those cases when
there are either biases in the gyroscope measurements or large temporary nongravitational accelerations present. A low-cost,
temperature-based calibration method is also discussed for initially calibrating gyroscope and acceleration sensors. An open source
implementation of the algorithm is also available.

1. Introduction Unfortunately, the use of low-cost MEMS IMUs intro-

duces several challenges compared to high-precision mea-
Inertial measurement units (IMUs) are widely used in atti- surement devices. Low-cost MEMSs are noisy and their mea-
tude estimation in mobile robotics, aeronautics, and navi- surements usually include various errors. These errors consist
gation. An IMU consists of a triaxial accelerometer and a of an unknown zero level, that is, bias error, and unknown
triaxial gyroscope and it is used for measuring accelerations scale factor, that is, gain error [5]. Moreover, the gain and the
and angular velocities in three orthogonal directions. The bias tend to drift over time and are affected by temperature
attitude, which is a 3D orientation of the IMU with respect to change. Therefore, sensor calibration has become one of the
the Earth coordinate system, can be estimated by combining most challenging issues in inertial navigation [6].
integrated angular velocities and acceleration measurements. Other problems with IMUs are related to the standard
Microelectromechanical-system (MEMS) IMUs are small, design of the sensor fusion algorithms. Usually, the roll and
light, and low-cost solutions for attitude estimation. They pitch angles are estimated using the measured angle of the
are widely used in mobile robotics, such as unmanned Earth’s gravitation force as reference to the integrated angle
aerial vehicles (UAVs) [1]. MEMS IMUs are also used in obtained from angular velocity measurements. This works
combination with other sensors, such as global navigation perfectly if no other forces than gravity exist in the system.
satellite systems (GNSS) [2, 3], light detection and ranging Unfortunately, this is rarely the case. In practical use, when
(LIDAR) sensors, or cameras in various applications. In an IMU is attached to a moving platform or object, these
addition, MEMS IMUs are commonly included in modern accelerations are unavoidable. In various cases, as in the
mobile phones [4]. estimation of either a human body [7] or a mobile phone
2 International Journal of Navigation and Observation

[4] position, or in a legged robot [8], these accelerations can In addition to the calibration and robust estimation of
become significantly large. Therefore, their presence should gyroscope biases, we adapt our measurement covariances to
be taken into account. increase the quality of the filter against changing dynamic
Another problem with standard IMU implementations conditions. IMU algorithms usually use the direction of
concerns heading angle estimation. The Earth’s gravitation gravity (through measured accelerations) to reduce accumu-
field gives no information about this. Therefore, the integra- lating errors in integrated angular velocities during attitude
tion of triaxial accelerometers and gyroscopes cannot provide estimation. This causes unwanted errors in the attitude esti-
an absolute heading angle. This is commonly overcome using mate when nongravitational accelerations or contact forces
an extra sensor, usually a triaxial magnetometer [2, 9–16] or are present. We also use a variable-measurement-covariance
satellite navigation [3, 5, 17–20]. Triaxial magnetometers offer method to reduce errors in the attitude estimation caused
a good solution if the magnetic field measurement can be by rapid and temporary nongravitational accelerations. It is
trusted. In practice, this is usually not the case at least in affordable to do, as in the DCM representation of rotation;
robotics, since robots are usually made of magnetic metal, the bottom row of rotation matrix represents the direction
have high current electronics and motor drives, and may of gravitational force. As a result, our implementation of
travel within locations that are surrounded by power lines, the extended Kalman filter is able to use only six states for
magnets, and magnetic metals. Moreover, because the mag- estimating the attitude, gyroscope biases, and the gravity
netic sensor observes the sum field caused by all magnets and vector.
electrically induced magnetic fields, the Earth’s magnetic field In Experiments and Results, we show that our solution is
cannot be easily separated. Furthermore, satellite navigation significantly more accurate than the compared algorithms in
can be of little help, as it does not work well inside buildings those situations in which either temporary accelerations are
or caves or under dense forest foliage. present or significant gyroscope biases exist. With fully cali-
The ability to reliably estimate attitude with a minimal brated bias-free data without nongravitational accelerations,
number of sensors would also increase the robustness of our algorithm performs as well as the compared algorithms,
the system in two ways. Firstly, as fewer sensors would be since our gyroscope bias estimates do not disturb attitude
needed, there would be less risk of sensor failures. Secondly, estimation.
it would be beneficial to run an IMU algorithm on the Our work provides a complete solution that integrates
background as a backup method, even when other mea- low-cost MEMS IMU, temperature calibration, online bias
surements are available. In addition, these results could be estimator, and a robust extended Kalman filter which is able
compared between the different algorithms to detect failures to handle large temporary accelerations and changes in sam-
and possibly compensate for those faults. pling rate. We verify our algorithm using multiple different
Our proposed method solves presented challenges by tests and we also obtain accurate reference measurements
firstly formulating a calibration method as a function of using the KUKA LWR 4+ robot arm and comparisons to
measured temperature for gains and biases and secondly other freely available state-of-the-art algorithms. The used
using an extended Kalman filter to estimate the bias in calibration and measurement data for our tests and the
gyroscope measurements online. We have purposely omitted proposed algorithm (in MATLAB and C++) are published as
magnetometer and other possible measurements from our open source at https://github.com/hhyyti/dcm-imu.
filter, and we estimate only absolute pitch and roll angles,
thus keeping the main focus on the gyroscope bias estimation.
Although the heading estimate (yaw angle) can be computed 2. Related Work
from the results of the proposed filter, it is not an absolute
heading. Instead, it is an integrated bias-corrected angular Numerous attitude estimation algorithms have become avail-
velocity around 𝑧-axis. The absolute yaw could be estimated able. Most of these consist of various Kalman filter solutions
in a separate filter using any extra measurements. By doing [2, 3, 5, 12, 13, 16, 25], usually extended Kalman filters (EKF)
this, we can increase the robustness of the proposed sensor [7, 9, 10, 15, 17, 18, 20, 26], and some unscented Kalman filters
fusion algorithm and enhance gyroscope bias estimation. If (UKF) [14, 19, 27], though some non-Kalman filter solutions
the magnetometer or other sensors would fail catastrophi- also exist [1, 4, 11, 21, 28–30] as well as some geometric
cally, only the heading estimate would fail, leaving pitch and methods [31–34]. In addition, Chao et al. [35] have carried
roll estimates unaffected. This can offer significant advantages out a comparative study of low-cost IMU filters. Existing
for robotic applications such as flying UAVs or other robots. algorithms often rely on data obtained from military-grade
In contrast to other proposed solutions [21–24], intro- IMUs, which are usually subject to export restrictions and
duced in more detail in the next chapter, we implement an high cost that limit commercial applications [29]. Several
extended Kalman filter to tune the bias estimates when such authors have reported that cheaper commercial grade IMUs
information is available. Furthermore, we do not rely on commonly include non-Gaussian noise in their gyroscope
constant gains for updating bias. Instead, we use an extended and accelerometer measurements, often leading to instability
Kalman filter to employ available information in system and in connecting classical Kalman and extended Kalman filter
measurement covariances in order to tune the gains for algorithms [29, 30]. The same has been noted in an extensive
updating bias and attitude estimates. Later, in Experiments survey of nonlinear attitude estimation methods by Crassidis
and Results, we also show in practice that even in the worst et al. [36]. They also state that EKF is not as good a solution
case scenario, the filter remains stable. as other filtering schemes. The survey was published a few
International Journal of Navigation and Observation 3

years prior to most of the papers presenting DCM based Currently, the most commonly used (at least among hob-
methods [3, 9, 12, 21], and their possibilities were therefore byists with low-cost MEMS) and freely available IMU algo-
not considered in their review. rithms are Madgwick’s [11] and Mahony’s [21] non-Kalman
Low-cost MEMSs are usually subject to time-dependent filter methods. Both of these methods are computation-
errors, such as drifting gyroscope biases. Therefore, all IMU ally simpler than any Kalman filter implementation. Open-
algorithms for low-cost sensors should have an online bias source implementations by Madgwick are used to compare
estimator. Unfortunately, few of the previously published these two state-of-the-art implementations to our work.
algorithms developed for only triaxial accelerometers and These implementations are freely available for C and MAT-
gyroscopes have included online bias estimates for gyro- LAB at http://www.x-io.co.uk/open-source-imu-and-ahrs-
scopes. In many algorithms, other measurements are needed algorithms/. The explicit complementary filter by Mahony
in addition to gyroscopes and accelerometers in order to et al. is used as a primary attitude estimation system on
estimate gyroscope biases. The most commonly used sensors several MAV vehicles worldwide [21]. However, neither of
are triaxial magnetometers [2, 9–16, 25] and satellite naviga- these Madgwick’s implementations was able to estimate
tion [3, 5, 17–20]. In addition to our work, few filters [21– biases using only accelerometer and gyroscope measure-
24, 30] have been able to estimate gyroscope biases without ments. Nevertheless, we used these, as other implementations
extra sensors in addition to the triaxial accelerometer and were not available at the time of writing.
gyroscope. Mahony’s and Baldwin’s IMU algorithm [21, 29] is based
The development of a filter that uses only accelerometer on an idea roughly similar to our solution. In contrast to our
and gyroscope measurements is difficult and can lead to an EKF solution, they derive their direct and complementary
observability problem. This problem arises when a single filters using tools from differential geometry on the Lie group
vector measurement, such as the gravity through acceleration SO(3). This solution is based on the Special Orthogonal
measurements, gives only information to correct estimates group SO(3), which is the underlying Lie group structure
of attitude angles, as well as the related biases, which for space of rotation matrices. Our solution is in principle
could rotate that vector. The single vector measurement defined similarly to their explicit complementary filter with
provides no information about the rotation around that bias correction; however, instead of their constant gains for
vector. Hamel and Mahony [30] have discussed the problem measurement update and bias estimator, we use EKF to tune
of orientation and gyroscope bias estimation using passive these gains.
complementary filter. They have also proposed a solution to Madgwick’s implementation [11, 28] is a quaternion
estimate biases even in the single vector case. Subsequently, implementation of Mahony’s observer [21], and it uses a
Mahony et al. [21] derived a nonlinear observer, termed the gradient descent algorithm in the orientation estimation.
explicit complementary filter, which similarly requires only Madgwick’s implementation [11] also uses simple algebraic
accelerometer and gyro measurements. In another theoretical modifications [21] to ensure separation of the roll and pitch
study [37], they discussed observability and stability issues estimation error from the yaw estimation. This addition
that arise especially while using single vector measurements. helps to deal with unreliable magnetic field measurements.
Finally, they proved that, in these single vector cases, the Madgwick’s solution is computationally efficient because only
derivation leads to asymptotically stable observers if they one iteration of the gradient descent algorithm is performed
assume persistent excitation of rigid-body motion. for each measurement. Therefore, the filter is better suited for
Similar ideas have been later used in the work by Khosra- high measurement frequencies. If used sampling rate is larger
vian and Namvar [38], who proposed a nonlinear observer than 50 Hz, the remaining error is negligible [11]. In our tests,
using a magnetometer as a single vector measurement. In we used a significantly larger rate of 150 Hz (the maximum we
addition to their work, Hua et al. [23] have implemented a could record using Microstrain Inertia-Link).
nonlinear attitude estimator that allows the compensation The effects of dynamic motion and nongravitational
of gyroscope biases of a low-cost IMU using an antiwindup accelerations have previously been taken into account in [14–
integration technique. They also show valuable aspects of a 16, 18, 27]. Most of this previous work compares the magni-
practical implementation of a filter. Finally, the observability tude of accelerometer measurements to the expected magni-
problem for systems, in which the measurement of system tude of Earth’s gravitation field [15, 18, 27]. It is not a perfect
input is corrupted by an unknown constant bias, is tackled approach, as exactly the same magnitude of accelerometer
in [39]. measurements can occur in multiple configurations (e.g., free
The observability problem can also be avoided. Ruizenaar fall and an acceleration of 1 g in any direction). A more exact
et al. [22] solve the problem by adding a second IMU to the approach to do this would be using a separate estimate for
system. They propose a filter that uses two sets of triaxial nongravitational accelerations as in [14]. Although they use
accelerometers and gyroscopes attached in a predefined three separate filter states for estimating linear acceleration
orientation with respect to each other in order to over- components, this unnecessarily increases the computational
come the observability problem. After their work, Wu et al. load of the filter. One solution would be making an adaptation
[24] overcome the same problem by actively rotating their in the covariance for acceleration measurements using the
IMU device. Rather than having two separate measurement magnitude of the difference between an estimated gravity
devices or an instrumented rotating mechanism, a simpler, vector and a measured acceleration [16]. We implemented
cheaper solution to this problem could be obtained, if we this method for our DCM-type representation of orientation.
could overcome the problem algorithmically. In our representation of rotation, it is efficient to use an
4 International Journal of Navigation and Observation

estimated gravity vector, as it is included in our state estimates which is derived from (1), (2), and (3) [12]. In (4), 𝑏 𝜔𝑖 , 𝑖 ∈
representing the rotation. {𝑥, 𝑦, 𝑧} are measured angular velocities and 𝑛𝑏 𝐶3𝑖 , 𝑖 ∈ {1, 2, 3}
If there were extra measurements for the velocity, for are bottom-row elements of the DCM, which are used as
example, from satellite navigation sensors, then the nongravi- an estimate of the partial attitude. Later in this paper, these
tational acceleration could be added to the filter as a state to be three bottom-row elements are called DCM states, which
estimated. This would make it possible to avoid our proposed form a DCM vector 𝑛𝑏 C3 . [C3 ×] is defined as a rotation
method to tune the measurement covariance of accelerom- operator which rotates the current DCM vector according to
eters and to improve the accuracy of the proposed filter, as the measured angular velocities.
it would no longer be vulnerable to nontemporary or con- The observation model is constructed using accelerom-
stant accelerations. Many velocity aided attitude estimation eter measurements, which are compared to the current
methods are dependent on measurement of linear velocity in estimate of the direction of gravity by [12]
addition to gyro and accelerometer measurements in order 𝑏 𝑛
to reliably compensate for nongravitational accelerations [31– 𝑓𝑥 0 𝑏 𝐶31
[ ] 𝑛 𝑇[ ] [ ]
f=[ ]
34]. Instead of using velocity measurements, we use only a 𝑏 𝑏 𝑛
[ 𝑓𝑦 ] = 𝑏 C [ 0 ] = [𝑏 𝐶32 ] 𝑔, (5)
low-cost triaxial set of an accelerometer and a gyroscope. 𝑛
𝑏
[ 𝑓𝑧 ] [𝑔] [𝑏 𝐶33 ]
3. Methods where 𝑏 𝑓𝑖 , 𝑖 ∈ {𝑥, 𝑦, 𝑧} are accelerometer measurements
3.1. DCM Based Partial Attitude Estimation. Our proposed (forming a measurement vector 𝑏 f) in the body-fixed frame
partial attitude estimation builds upon the work by Phuong (𝑏) and 𝑔 is the magnitude of the Earth’s gravitation field.
et al. [12]. It is based on the relation between the estimated This simple model assumes that the gravity is aligned parallel
direction of gravity and measured accelerations. The direc- to the 𝑧-axis and that there is no other acceleration than
tion of gravity is estimated by integrating measured angular gravity. Later in this paper, this assumption is relaxed with
velocities using a partial direction cosine matrix (DCM). The the application of a variable measurement covariance in the
results are translated into Euler angles in the 𝑍𝑌𝑋 convention extended Kalman filter algorithm.
[40], which have the following relation to the DCM:
3.2. Adaptive Extended Kalman Filter with Gyroscope Bias
𝜃𝑐 𝜓𝑐 −𝜙𝑐 𝜓𝑠 + 𝜙𝑠 𝜃𝑠 𝜓𝑐 𝜙𝑠 𝜓𝑠 + 𝜙𝑐 𝜃𝑠 𝜓𝑐 Estimation and Variable Covariances. An extended Kalman
𝑛 [ ]
𝑏C = [𝜃𝑐 𝜓𝑠 𝜙𝑐 𝜓𝑐 + 𝜙𝑠 𝜃𝑠 𝜓𝑠 −𝜙𝑠 𝜓𝑐 + 𝜙𝑐 𝜃𝑠 𝜓𝑠 ] . (1) filter (EKF) is a linearized approximation of an optimal
nonlinear filter, similar to the original Kalman filter [42].
[ −𝜃𝑠 𝜙𝑠 𝜃𝑐 𝜙𝑐 𝜃𝑐 ] Usually, the state and measurements are predicted with
The notation “𝑠” in (1) refers to sine and “𝑐” to cosine, 𝜑 the original nonlinear functions, and the covariances are
to roll, 𝜃 to pitch, and 𝜓 to yaw in Euler angles. The direction predicted and updated with a linearized mapping. In this
cosine matrix 𝑛𝑏 C, that is, the rotation matrix, defines rotation case, the measurement model is linear, and the state update
from the body-fixed frame (𝑏) to the navigation frame (𝑛). It is is nonlinear. Some attitude estimation algorithms based
integrated from initial DCM using an angular velocity tensor on previously presented Kalman filters [3, 5, 12, 13, 25]
[𝑏 𝜔×] formed from triaxial gyroscope measurements in the use a simpler linear model; however, in our work, we use
body-fixed frame according to [41] a nonlinear state-transition model for a purely nonlinear
problem.
𝑛
̇ = 𝑛𝑏 C [𝑏 𝜔×] ,
𝑏C (2) We enhance the commonly used standard EKF algorithm
through a few additions. First, the filter is adapted to
where changing measurements by using a variable time-dependent
0 −𝑏 𝜔𝑧 𝑏 𝜔𝑦 state-prediction and acceleration-dependent measurement
[ ] covariances. Second, the filter is simplified and made compu-
[𝑏 𝜔×] = [ 𝑏
[ 𝜔𝑧 0 −𝑏 𝜔𝑥 ]]. (3) tationally more feasible by using gyroscope measurements as
𝑏 𝑏 control inputs in the EKF. Third, the magnitude of the DCM
[− 𝜔𝑦 𝜔𝑥 0 ]
vector is constrained to be always exactly one. Finally, the
Normally, in DCM-type filters, the whole DCM matrix filter is formulated for a variable sampling interval to tolerate
would be updated; however, in this partial-attitude- jitter or changes in sampling rate.
estimation case, only the bottom row of the matrix in (1) is The filter principle is shown as a simplified block diagram
estimated for the proposed filter. These bottom-row elements in Figure 1. The accelerometer measurements are used as a
are collected into a row vector 𝑛𝑏 C3 which can be updated measurement for the EKF, and gyroscope measurements are
using used as control inputs in the prediction subsection. Later
𝑛 on, the EKF is used to fuse these measurements in the
̇ − 𝑛𝑏 𝐶33 𝑛 𝑏
𝑏 𝐶31 0 𝑏 𝐶32
𝜔𝑥 update subsection. In Figure 1, x refers to the EKF state vector
𝑛 [𝑛 ] [ ][ ]
̇ [ 𝐶̇ ] = [ 𝑛𝑏 𝐶33 0 − 𝑛𝑏 𝐶31 ][ 𝑏 ] defined in (6), x̂ is an unnormalized predicted state, z is a
𝑏 C3 = [𝑏 32 ] [ 𝜔𝑦 ], (4) measurement, u is a control input, and ỹ𝑘 is a measurement
𝑛
[𝑏 𝐶̇33 ] [ − 𝑛𝑏 𝐶32 𝑛𝑏 𝐶31 0 ][
⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟
𝑏
⏟⏟⏟⏟⏟⏟𝜔
⏟⏟𝑧⏟⏟⏟]
⏟⏟ residual. The colored blocks in the figure are updated or
[C3 ×] u estimated online. The accelerometer measurement is drawn
International Journal of Navigation and Observation 5

Measurement Update of extended Kalman filter. In (9), 𝑔 is the magnitude of the

gravity
Accelerometer
z + ỹ I3 −Δ𝑡 [C3 ×]𝑘
g + + 𝑓𝑘 (x𝑘 , u𝑘 ) = [ ] x𝑘
− 03×3 I3
a (8)
Noise Δ𝑡 [C3 ×]𝑘
H K
+[ ] u𝑘 ,
03×3
u x̂
Gyroscope B + + H = [𝑔I3 03×3 ] . (9)

Process noise
w
Γ A
In (7), v𝑘 and w𝑘 represent a zero mean Gaussian
Normalization
white noise, and Γ𝑘 = Δ𝑡 is a simplified time-dependent
x model for the state-prediction noise. This simplified model
Prediction
is derived assuming a Wiener white noise process in angular
Figure 1: A simplified block diagram of the DCM IMU filter. The velocity measurements (used as control inputs) which are
covariance computation has been hidden to simplify the work flow integrated into the DCM state and bias estimates over time
of the EKF filter. The colored blocks are adjusted online. in each prediction step [43]. Therefore, we can formulate a
state-prediction model to have a linear relationship to time
step size. This time-dependent modification of the process
using different subblocks for gravitational, 𝑔, and nongravita- noise covariance allows the filter to behave more robustly
tional accelerations, 𝑎, because nongravitational acceleration to changing sampling interval. When this is applied to a
is estimated using the predicted state in the EKF, thus covariance model, we simplify and assume that there is no
allowing these two to be separated. cross-correlation between states, thus yielding the following
The state-transition model to update angular velocities to equation for process noise covariance Q𝑘−1 :
DCM states (4) and the measurement model to incorporate
accelerations (5) are formulated into an extended Kalman 𝜎𝐶2 3 I3 03×3
filter that has six states: three for orientation (the DCM vector, Q𝑘 = Γ [ 2
] Γ𝑇
𝑛 𝑏 𝜔 03×3 (𝜎𝑏𝜔 ) I3
𝑏 C3 ) and three for gyroscope biases (the bias vector, b ): (10)
𝑛 𝜎𝐶2 3 I3 03×3
2
𝑏 C3 𝑏 𝜔 𝑏 𝜔 𝑏 𝜔 𝑇 = Δ𝑡 [ ].
x= [𝑏 𝜔 ] = [𝑛𝑏 𝐶31 𝑛 𝑛
𝑏 𝐶32 𝑏 𝐶33 𝑏𝑥 𝑏𝑦 𝑏𝑧 ] . (6) 2
03×3 (𝜎𝑏𝜔 ) I3
b
In (10), there are two parameters. First, 𝜎𝐶2 3 is the DCM-
The state-space model for the proposed system is the
following: state-prediction variance which is mainly driven by the
control input u (angular velocities), and thus the value of
x𝑘+1 = 𝑓𝑘 (x𝑘 , u𝑘 ) + Γ𝑘 w𝑘 , the parameter can be approximated as the variance of noise
of gyroscope measurements. Second, (𝜎𝑏𝜔 )2 is the bias-state-
y𝑘 = Ηx𝑘 + k𝑘 , prediction variance. In this work, it is assumed that since
gyroscope biases drift very slowly, setting a tiny value for the
𝑐 (x𝑘 ) = 1 constraint, prediction variance of corresponding bias states is reasonable
(see experimental parameters in Table 1). This forces the filter
E {w𝑘 } = E {k𝑘 } = 0, to trust its own bias estimate much more than its attitude
(7) estimate, and bias estimates change only slightly during each
measurement update. Our work omits the optimal estimation
E {w𝑘 w𝑘 𝑇 } = Q𝑘 ,
of these experimental parameters as acceptable parameters
can be found manually.
E {k𝑘 k𝑘 𝑇 } = R𝑘 , The measurement covariance R𝑘 in (7) is adjusted accord-
ing to the acceleration measurement as follows:
𝑇
x0 = [0 0 1 0 0 0] . 󵄩 󵄩
R𝑘 = (󵄩󵄩󵄩󵄩𝑏 a𝑘 󵄩󵄩󵄩󵄩 𝜎𝑎2 + 𝜎𝑓2 ) I3 , (11)
In (7), 𝑓𝑘 (x𝑘 , u𝑘 ) is the discrete nonlinear state-transition
function at time index 𝑘. The state-transition function is pre- which is a robust covariance for acceleration measurements
sented in (8) (see Appendix for the derivation). The function based on the work by Li and Wang [16]. The proposed
uses the state vector x𝑘 in (6) and gyroscope measurements measurement covariance R𝑘 is built upon two parts: first, a
as control input u𝑘 . The measurement y𝑘 in (7) is derived constant part which represents a variance of a measurement
with a linear and static observation model H presented in noise for a triaxial accelerometer 𝜎𝑓2 and second, a variable
(9). In (8), [C3 ×] is a rotation operator defined in (4), and part which represents a constant variance of estimated
Δ𝑡 is a sampling interval which can vary in this formulation acceleration 𝜎𝑎2 scaled using the magnitude of the estimated
6 International Journal of Navigation and Observation

Table 1: Experimental parameters for IMU algorithms. The constraint, 𝑐, in (7) keeps the DCM vector 𝑛𝑏 C3,𝑘 used
in the first three states as a unit vector. The constraint is
Symbol Quantity Value defined according to
𝑔 The acceleration of gravity 9.8189 m/s2 󵄩 󵄩
(around Helsinki, Finland) 𝑐 (x𝑘 ) = 󵄩󵄩󵄩󵄩𝑛𝑏 C3,𝑘 󵄩󵄩󵄩󵄩 = 1. (13)
Δ𝑡 Sampling interval ∼1/150 s

𝜎𝐶2 3 ,0 Initial variance of the DCM 12 (rad/s)2

Many different nonlinear filters could be used to solve the
state proposed estimation problem, for example, Gaussian filters
𝜔 2
(𝜎𝑏,0 ) Initial variance of bias states 0.12 (rad/s)2 like extended and unscented Kalman filters [44]. We selected
DCM-state-prediction an extended Kalman filter [43] as we wanted to minimize
𝜎𝐶2 3 0.12 (rad/s)2 computational load of the filter. We used the projection
variance
(𝜎𝑏𝜔 )2 Bias-state-prediction variance 0.00012 (rad/s)2 method by Julier and LaViola Jr. [45] to handle the constraint
in (7). The derivation and practical implementation of the
𝜎𝑓2 Variance of accelerometer 0.52 (m/s2 )2
measurement
proposed EKF and the constraint projection are explained in
Appendix.
𝜎𝑎2 Variance of estimated 102 (m/s2 )2
acceleration
A tuning parameter of 3.3. Computation of Euler Angles from Filter States. To com-
𝛽 0.1 pare our results to other filters and to use the estimate, the
Madgwick’s filter
A tuning parameter of estimated attitude should be able to be translated into an
𝐾𝑝 0.5 Euler angle representation. As the proposed attitude filter
Mahony’s filter
only estimates the partial attitude, corresponding to two out
of the three Euler angles present in the bottom row of the
rotation matrix in (1), the transformation of filter states to
nongravitational acceleration ‖𝑏 a𝑘 ‖, which is the difference Euler angle representation is not trivial. While the yaw angle
between the measured acceleration and the estimated gravity. should be integrated separately, pitch 𝜃 and roll 𝜑 angles
These nongravitational accelerations are estimated using a can be estimated using the following equations that can be
𝑛
̂3,𝑘 (the first part derived from (1):
relation between a predicted DCM vector 𝑏 C
of the predicted state vector in the EKF) and the current 𝜃𝑘 = arcsin (−𝐶31,𝑘 ) ,
accelerometer measurement 𝑏 f𝑘 deriving from (5): (14)
𝜙𝑘 = atan2 (𝐶32,𝑘 , 𝐶33,𝑘 ) ,
𝑛
𝑏 ̂3,𝑘 .
a𝑘 = 𝑏 f𝑘 − 𝑔 𝑏 C (12) where atan2 is an inverse tangent function with two argu-
ments to distinguish angles in all four quadrants [46] and
The proposed measurement covariance adaptation is 𝐶3𝑖,𝑘 is the 𝑖th element of the DCM vector at time index 𝑘.
more effective than the standard approach, since it mod- Yaw angle, 𝜓, can be integrated from bias-corrected
ifies the algorithm to become more robust against rapid angular velocities with (1) and (2) using previously computed
accelerations. However, if measurement errors are correlated roll, pitch, and yaw angles as a starting point. The yaw can
(i.e., there exists a long-term or constant nongravitational be resolved from the full DCM matrix C using the following
acceleration), the assumption underlying the proposed model equation:
would no longer hold. Therefore, this solution is only limited
𝜓𝑘 = atan2 (𝐶21,𝑘 , 𝐶11,𝑘 ) , (15)
to cases where nongravitational acceleration 𝑏 a𝑘 in (12) can
be assumed to be temporary. where 𝐶𝑖𝑗,𝑘 is the 𝑖, 𝑗th index of the DCM matrix at time
For simplicity, since the sampling time of accelerometer index 𝑘. Note that indices 2, 1 and 1, 1 are not estimated in
is assumed to be constant, these parameters (measurement the proposed filter. In (15), since the upper rows of the matrix
variances 𝜎𝑓2 and 𝜎𝑎2 ) should be tuned to include the effect are needed, the whole rotation matrix needs to be computed
of the used sampling time. Time-dependent modifications outside the filter if the yaw angle estimate is required.
could be added similar to that for Q in (10). However, it
is not necessarily needed, as measurement updates can be 3.4. Temperature Calibration Method. MEMS gyroscopes and
avoided if a measurement is lost, and the physical sampling accelerometers usually show at least some bias error, though
time in practice usually remains constant, although the some gain error might be present as well. To be able to reduce
sampling interval might change or measurements might be the effect of these errors and to achieve the best performance
lost. For a practical implementation, parameter 𝜎𝑎2 (the gain of the IMU, it is important to calibrate the system before
for estimated gravitational acceleration) should be set to a feeding the data into any attitude estimation algorithm. The
much larger value than 𝜎𝑓2 (measurement noise). This makes gains and biases of the MEMS gyroscope and accelerometer
the adaptation to changing acceleration values significant vary over time, a large part of which can be explained
compared to measurement noise. For the values used, see by temperature changes in the physical instrument. Our
experimental parameters in Table 1. observations (Figures 11 and 12) indicate that while working
International Journal of Navigation and Observation 7

near room temperatures, a linear model for temperature is

sufficiently accurate to model most of the changes in bias
and gain terms using low-cost MEMS accelerometers and
gyroscopes.
As our proposed IMU uses accelerometers to calibrate
gyroscope biases online, accelerometer calibration is essential
for reaching accurate measurements. Therefore, temperature-
dependent calibration is needed at least for the accelerom-
eters in order to estimate temperature-dependent bias and
gain terms for all the sensor axes if there is any possibility
of temperature changes in the environment. We used a linear
model for the gain and bias parameters to scale the magnitude
and reduce the bias from all gyroscope and accelerometer Figure 2: KUKA robot arm and the two independent IMU devices
measurements. The measurement model is derived from [6] fixed to the tool. MicroStrain Inertia-Link is installed coaxially
by adding the linear model of temperature for each parameter. below SparkFun 6DOF Digital IMU which is inside the topmost
aluminum enclosure.
The used measurement model for each sensor axis 𝑖 ∈
{𝑥, 𝑦, 𝑧} is
The first strategy is having two different constant temper-
𝑏 𝑓𝑖 𝑏 𝑓𝑖
𝑓meas,𝑖 = 𝑝gain (𝑇) 𝑓𝑖 + 𝑝bias (𝑇) , atures and performing the calibration [6] at these two differ-
(16) ent temperatures. From the resulting two sets of calibration
𝑏 𝜔 𝜔
𝜔meas,𝑖 = 𝑝gain
𝑖
(𝑇) 𝑏 𝜔𝑖 + 𝑝bias
𝑖
(𝑇) , parameters, the temperature-dependent linear model (16) can
be calculated by fitting a parameterized line (17) into two
𝑓 /𝜔 𝑓 /𝜔 𝑓 /𝜔 points in a 12-dimensional space. The other strategy, which
𝑝gain/bias
𝑖 𝑖
(𝑇) = 𝑎gain/bias
𝑖 𝑖
𝑇 + 𝑏gain/bias
𝑖 𝑖
. (17)
could be used if constant temperatures cannot be arranged,
is separately cooling the device for one orientation out of
𝑓 six needed in the presented methods and to perform the
In (16) and (17), 𝑝gain/bias
𝑖
(𝑇) is a function of accelerometer
𝜔𝑖 calibration measurements as a function of temperature while
gain/bias as a function of temperature 𝑇, and 𝑝gain/bias (𝑇) the device is gradually heated. Next, a linear regression line
is a function of gyroscope gain/bias as a function of tem- can be fitted into the data for each sensor axis as a function of
perature, both separately for each axis 𝑖. In the equation, temperature (similarly as in Figures 11 and 12). This regression
𝑏
𝑓meas,𝑖 is the accelerometer measurement, and 𝑏 𝜔meas,𝑖 is model can then be used to estimate constant temperature
the gyroscope measurement for each axis 𝑖. All accelerations averages for two different temperatures. These estimates
and angular velocities are in body-fixed frame (𝑏). In (17), could be used similarly to the average measurements in the
the temperature-dependent linear model is expressed for all first strategy.
different sensors and sensor axes for bias and gain. In the
equation, 𝑓𝑖 and 𝜔𝑖 are interchangeable similar to subscripts 4. Experiments and Results
gain and bias. As we used the linear model for all biases
and gains as a function of temperature, there are a total of Experiments were conducted using two independent IMU
four parameters for each sensor axis in the calibration model devices, MicroStrain Inertia-Link [48] and a low-cost Spark-
for the accelerometer and the gyroscope, thus yielding 24 Fun 6DOF Digital IMU breakout board (combination of an
unknown calibration parameters to tune. ADXL345 accelerometer [49] and an ITG-3200 gyroscope
The calibration of accelerometers can be performed [50]). The reference measurement was acquired using a
without accurate reference positions using the iterative math- KUKA Lightweight Robot 4+ [51] and a Fast Research
ematical calibration method by Won and Golnaraghi [47]. interface [52] for measuring the pose of the IMUs fixed to the
According to the method, the three-axis accelerometer is tool of the robot arm. Comparetti et al. [53] measured KUKA
placed in six different positions and held stationary during LWR 4+ accuracy to be on average 1.18 mm and 0.95∘ for
each calibration measurement. Measurements from these the translation and rotation components, respectively. Both
six positions are then used in the algorithm iteratively to of the IMU devices were installed coaxially to the robot arm
optimize gains and biases for each axis of the accelerometer (Figure 2) and aligned to have an axis orientation similar to
sensor. Gyroscope biases and gains are estimated by com- that for the end effector of the robot. The built-in calibration
paring rotations to a reference measurement and forming a procedure for MicroStrain Inertia-Link was performed prior
linear model of gains and biases for all axes of the sensor. to data collection [48].
Similar to Sahawneh and Jarrah [6] who use an instrumented Since the developed algorithm should be robust for dif-
rotation plate to perform gyroscope calibration, we use one ferent parameters between different accelerometers and gyro-
rotation axis of the robot arm to accomplish the same task. scopes, only one common choice of experimental parameters
Whereas their method has 12 parameters, we use 24 unknown was used for both measurement devices. The comparison
parameters. Our parameters can be acquired by using two between proposed algorithm and comparison algorithms is
different strategies and single-temperature methods [6, 47]. thus more general, as parameter tuning plays a less important
8 International Journal of Navigation and Observation

role in the paper. The experimental parameters for the fair; however, as there were no other freely available imple-
proposed filter and comparison algorithms are shown in mentations to use, we performed our comparison for only
Table 1. These same parameters (measurement and state- these algorithms (see details in Section 2). The gyroscope bias
prediction covariances and initial values) were used in both tolerance test is presented only with Microstrain Inertia-Link
IMUs for simplicity. These parameters were tuned using a data, which is less noisy and has better accuracies with all
separate set of measurement data and a robot trajectory as algorithms in the other tests. The lower-cost, lower-quality
the reference measurement prior to the tests presented later SparkFun 6DOF Digital IMU would have yielded very similar
in this work. results between the compared algorithms.
The variance of the accelerometer was estimated using For the first three tests, the orientation measurement of
measured accelerometer noise in a static situation, and DCM- the KUKA robot arm was used as a reference measurement
state-prediction variance was similarly estimated using mea- which was compared to yaw, pitch, and roll angles computed
sured gyroscope noise. Bias-state-prediction variance is man- using the proposed DCM-IMU algorithm as well as Madg-
ually selected as an arbitrary value that is significantly wick’s [11] and Mahony’s [21] algorithms as a comparison. The
smaller than DCM-state-prediction variance. Similarly, ini- built-in estimate of Microstrain Inertia-Link was also used
tial values and the variance of estimated acceleration are as a comparison. Errors to the reference measurement are
initially selected as arbitrary values that are large enough plotted separately for yaw, pitch, and roll angles in Figure 3.
and then tuned manually. The reference measurement was The reference measurement is plotted to the topmost subplot
compared to the estimate of the proposed algorithm, and in the figure. The figure also shows the acceleration test and
the arbitrarily selected parameters were manually changed the rotation test sequences. As can be noted, exact differences
until the accuracy became satisfactory. The experimental between the compared algorithms are difficult to discern
parameters for the comparison methods by Madgwick and from this plot. Therefore, differences between the algorithms
Mahony were selected as their default values (Table 1). are later studied using a box-and-whiskers plot and root mean
The used data sequence consists of two similar calibration squared errors.
sequences to calibrate accelerometers and gyroscopes before Finally, at the end of results section, we present the
and after the actual test data. These calibration sequences effects of temperature change on the used low-cost IMU
are used to calibrate measurements and remove any bias and device, SparkFun 6DOF Digital IMU [49, 50]. This section
gain errors from the measurements using the temperature is important, as it demonstrates the need for our linearly
based calibration method described in Section 3.4, except temperature-dependent sensor calibration model and the
for Inertia-Link measurements which were calibrated using proposed extension to the previous calibration methods. We
only a simpler temperature-non-dependent method [6]. also present our temperature-dependent calibration values
Temperature changes could not be taken into account in the for our low-cost device.
Inertia-Link data, as the device does not give temperature
measurement together with its own orientation estimate.
Luckily, the temperature change during the test was small, 4.1. Rotation Test. In the rotation test, IMUs were rotated
thus limiting the possibility of any remaining bias and gain in 6D using a preprogrammed path. The same path was
errors in the Inertia-Link test data after calibration. driven four times, first at full speed and then by halving the
After calibration of the test data, we separated the test target velocity at each iteration. To use some well-known
data into two separate test sequences. The first sequence, the standard rotation formalism, the quality of algorithms is
acceleration test, is designed to test the magnitude of errors compared in Euler angles, which is easily computed for all
caused by induced linear accelerations. Our hypothesis is that compared algorithms. To highlight the differences, the errors
the larger linear acceleration is induced, the more likely it is are visualized using a box-and-whiskers plot in Figure 4. The
that there will be larger errors in the attitude estimate. The statistics in the figure are computed from the errors to the
second test sequence, the rotation test, is designed to test reference measurement during the rotation test sequence.
dynamic performance of gyroscopes at different velocities The upper subplot shows yaw errors, and lower subplot
moving according to a preprogrammed path in 6D. The shows combined roll and pitch errors. As revealed in the
rotation test is designed to be long enough to truly measure figure, roll and pitch errors behave quite similarly, as the
drifts and give reliable error statistics. In addition, tests are measured gravity helps similarly in their estimation (in the
designed to have the same path in four times at different 𝑍𝑌𝑋 convention [40]); therefore, their errors are combined
velocities to cover different frequencies. into the same statistics plot in Figure 4. The initial yaw error
In addition to comparing different algorithms with fully (caused by errors before the test sequence) is reduced from
calibrated data, the sensitivity of the algorithms to gyroscope the yaw estimates of all the compared methods.
biases was tested by adding artificial bias to fully calibrated All algorithms are computed for two separate devices,
test data. In the third test, we added different magnitudes of Microstrain Inertia-Link (“A” in Figure 4) and SparkFun
artificial bias to the test data (the same for all sensor axes 6DOF Digital IMU (“B” in Figure 4). The label “Inertia-
for simplicity). The root mean squared errors (RMSE) to the Link” refers to the built-in orientation estimate of Microstrain
reference measurement for yaw, pitch, and roll angles were Inertia-Link, recorded in addition to raw triaxial accelera-
compared at different added biases. As Madgwick’s imple- tions and angular velocities. As our paper focuses on partial
mentations of his and Mahony’s algorithms do not include attitude estimation (roll and pitch), the main focus of the test
an online bias estimator, this bias test is not completely is given to the lower subplot in Figure 4. The yaw-error plot
International Journal of Navigation and Observation 9

The reference measurement and measurement errors Error statistics in the rotation test
Reference angles (deg.)

200 10
100
5

Yaw errors (deg.)

0
−100 0
−200 −5
500 550 600 650 700 750
Time (s) −10

Yaw Roll −15

DCMA

MadgwickA

Mahony A

DCMB

MadgwickB

Mahony B

Inertia-Link
Pitch
Yaw errors (deg.)

10

0 10

Combined roll and pitch errors

−10 8
6
500 550 600 650 700 750 4
2

(deg.)
Time (s)
0
Pitch errors (deg.)

5 −2
−4
0 −6
−5 −8

DCMA

MadgwickA

Mahony A

DCMB

MadgwickB

Mahony B

Inertia-Link
500 550 600 650 700 750
Time (s)
10
Roll errors (deg.)

5 Figure 4: A box-and-whiskers plot showing error statistics in the

0 rotation test. The red line shows the median, the blue box is drawn
−5 between the first and the last quadrants, and whiskers are drawn to
Acceleration test Rotation test
the most distant measurements.
−10
500 550 600 650 700 750
Time (s)
Table 2: Root mean squared errors of the rotation test.
DCM Mahony
Madgwick Inertia-Link Method Yaw RMSE (deg) Pitch RMSE (deg) Roll RMSE (deg)
DCMA 1.57∗ 0.56∗ 0.61∗
Figure 3: The three compared IMU algorithms, a built-in estimate MadgwickA 2.40 0.98 1.52
of Microstrain Inertia-Link, and the reference trajectory which is
MahonyA 2.49 0.68 0.82
performed using the KUKA LWR 4+ robot arm. An error to the
reference measurement is shown for each algorithm for yaw, pitch, DCMB 3.91 1.29 1.46
and roll angles. The visible time range covers only the performed MadgwickB 4.28 1.46 1.71
tests. The calibration sequences before and after the tests are omitted MahonyB 4.20 1.22 1.05
from the figure. Inertia-Link 5.68 1.17 2.09
Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B).
∗
The most accurate value in the test.

(the upper subplot in Figure 4) indicates that yaw drift is not

significantly larger than other methods, although its value
𝑥, 𝑦, and 𝑧 directions separately. The test sequence, shown
is integrated outside the proposed partial attitude estimator.
in Figure 5, consists of back and forth movement, first using
The surprisingly good result observed in the yaw angle can
maximal velocity and then halving the target velocity at each
be explained by the accurate gyroscope bias estimates in
iteration. The reference velocity, measured using KUKA robot
the proposed filter. The root mean squared errors (RMSE)
hand, is drawn on top of the figure. The purpose of the
are also counted for all methods in Table 2, where the most
test is to show unintended rotations in attitude estimates
accurate results are highlighted for each angle. In this test,
during temporary accelerations. These errors caused by rapid
the DCM method produced the most accurate algorithm
accelerations have not usually been considered in other
with Inertia-Link data and performed slightly worse than
papers, but these accelerations are present in practical cases,
Mahony’s method with the SparkFun data.
such as the estimation of a human body [7], a mobile phone
[4], or a legged robot [8] orientation.
4.2. Acceleration Test. In the acceleration test, the IMUs were The statistics for the acceleration test using fully cali-
rotated minimally, moving the KUKA robot linearly in the brated data are presented using a box-and-whiskers plot in
10 International Journal of Navigation and Observation

Reference measurement and accelerations during the acceleration test Error statistics in the acceleration test
Reference velocity (m/s)

1
4
0.5
3

Yaw errors (deg.)

0 2
−0.5 1
0
−1
470 480 490 500 510 520 530 540 550 −1
Time (s) −2
−3
x z −4
y

DCMA

MadgwickA

Mahony A

DCMB

MadgwickB

Mahony B

Inertia-Link
5
Accx (m/s2 )

Combined roll and pitch errors

−5 4
470 480 490 500 510 520 530 540 550 2
Time (s)

(deg.)
0
5
−2
Accy (m/s2 )

0 −4
−6

DCMA

MadgwickA

Mahony A

DCMB

MadgwickB

Mahony B
−5

Inertia-Link
470 480 490 500 510 520 530 540 550
Time (s)

11
Accz (m/s2 )

10 Figure 6: A box-and-whiskers plot showing error statistics in the

9 acceleration test. The red line indicates the median, the blue box
8 is drawn between the first and the last quadrants, and whiskers are
7 x-axis movement y-axis movement z-axis movement drawn to the most distant measurements which are not considered
470 480 490 500 510 520 530 540 550 as outliers (red + signs).
Time (s)

A (Inertia-Link) Table 3: Root mean squared errors of the acceleration test.

B (SparkFun)
Method Yaw RMSE (deg) Pitch RMSE (deg) Roll RMSE (deg)
Figure 5: Reference velocities and accelerations during the accel- DCMA 1.19 0.42 0.17∗
eration test, at first 𝑥-axis, then 𝑦-axis, and last 𝑧-axis movement
separately. MadgwickA 0.31 0.93 0.87
MahonyA 0.31 0.40 0.32
DCMB 2.29 0.26∗ 0.70
MadgwickB 0.14∗ 0.78 0.79
Figure 6. In addition, root mean squared errors are computed
MahonyB 0.16 0.30 0.40
for all methods in Table 3, where the most accurate results are
Inertia-Link 2.61 0.50 3.45
highlighted. As can be seen from the results, the proposed
DCM method is much more robust against rapid accelera- Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B).
∗
The most accurate value in the test.
tions than any of the compared methods for pitch and roll
angles. The yaw angle estimate is less accurate in the DCM
method than in Madgwick’s and Mahony’s methods. This is cold-started at the beginning of the test sequence, and bias
caused by the bias estimate around the yaw axis in the DCM estimates are computed during the test sequence; that is, the
method that is not working perfectly in this test, as there are filter is reset to the default values as the test begins. This cold
hardly any rotations present in the test data. In addition, this start reduces the measured quality of our DCM method, as
test reveals that Mahony’s method is much more robust than the biases are assumed to be zero at the start of each test. This
Madgwick’s method, which is highly prone to errors caused first part of the test, when bias estimates are converging, adds
by rapid accelerations in pitch and roll angles. most of the measured errors to the DCM method with larger
values of induced bias.
4.3. Gyroscope Bias Tolerance Test. The bias test was per- For an example of biased behavior of all the compared
formed in the same manner for the rotation test (results in algorithms, a rotation test where the same bias of 1 deg./s is
Figure 8) and for the acceleration test (results in Figure 9) added to all gyroscope measurements is shown in Figure 7.
sequences. To save computation time, all algorithms were As can be seen from the figure, for pitch and roll, Madgwick’s
International Journal of Navigation and Observation 11

Errors to the reference measurement with a bias of 1 deg./s RMSEs as a function of gyroscope bias (rotation test)
10 103

Yaw RMSE (deg.)

Yaw error (deg.)

0
−10 102
−20
101
−30
−40 100
580 600 620 640 660 680 700 720 740 760 0 1 2 3 4 5 6 7
Time (s) Constant gyroscope bias (deg./s)
10 102

Pitch RMSE (deg.)

Pitch error (deg.)

5
101
0
−5 100

−10 10−1
580 600 620 640 660 680 700 720 740 760 0 1 2 3 4 5 6 7
Time (s) Constant gyroscope bias (deg./s)
15 102

Roll RMSE (deg.)

Roll error (deg.)

10
5 101
0
100
−5
−10 10−1
580 600 620 640 660 680 700 720 740 760 0 1 2 3 4 5 6 7
Time (s) Constant gyroscope bias (deg./s)

DCM DCM
Madgwick Madgwick
Mahony Mahony

Figure 7: Errors in yaw, pitch, and roll as an artificial 1 deg./s bias is Figure 8: Root mean squared errors of yaw pitch and roll angles in
added to all gyroscope measurements in the rotation test. the rotation test as a function of added constant bias in gyroscope
measurements.

method performs almost as well as the DCM method, while cases where there is at least some unknown gyroscope bias
Mahony’s method shows the largest error. For the yaw present.
angle, our DCM is the only method that is able to cope Convergence of the bias estimate in the acceleration test
with biased gyroscope measurements and to obtain reliable is interesting, as the test is a special pathological case for bias
measurements. The reason for this is the fact that since our estimate around the 𝑧-axis (corresponds to the yaw angle
method can reliably estimate gyroscope biases for each sensor seen in the upper subplot in Figures 6 and 9, as there are
axis, the end result contains less integrated bias error. The no rotations). In this case, the yaw angle and gyroscope
start transient caused by the cold start is also visible in the bias estimates around the 𝑧-axis are not observable. The
figure. behavior of the proposed filter, while estimating biases and
To test bias tolerance, test sequences were computed using their corresponding variances, is shown in Figure 10 when
different added biases changing from zero to seven degrees there is an induced bias of 1 deg./s in each gyroscope axis. In
per second separately for rotation and acceleration tests. For the figure, biases for pitch and roll converge rapidly towards
both of the test sequences (the rotation test in Figure 8 and the true value of 1 deg./s, whereas the bias estimate around
the acceleration test in Figure 9), the proposed DCM method yaw converges very slowly. This happens as the filter receives
is able to successfully find the induced bias and estimate it for marginal information about the tiny rotations present in
pitch and roll angles. The bias around the yaw angle for the the data. Even in this pathological special case with the
acceleration test in Figure 9 is not correctly estimated in the observability problem, the presented filter behaves correctly,
EKF, since the test data includes minimally rotations (only as demonstrated by the absence of any increase in the variance
linear accelerations). The filter cannot estimate the induced of the bias estimate; that is, the filter remains stable.
bias around the 𝑧-axis due to the lack of information about
the bias present in this test data (see the discussion about 4.4. Effects of Temperature to Bias. To test the sensitivity of
the observability problem in Section 2). In the rotation test low-cost MEMS IMUs to temperature changes, a long calibra-
(results in Figure 8), bias estimates work for all angles, and tion sequence over different temperatures was performed for
the effect of induced bias is minimal compared to all other the SparkFun 6DOF Digital IMU. First, the device was cooled
presented algorithms. As can be noted from the figures, the down and then held stationary for over 40 minutes. During
proposed DCM method has the smallest error in nearly all that time, the excess heat from the electronics warmed the
12 International Journal of Navigation and Observation

RMSEs as a function of gyroscope bias (acceleration test) Bias estimates and 1-sigma distances of standard deviations
104
Yaw RMSE (deg.)

1.5

xbias (deg./s)
102
1
100
0.5
10−2
0 1 2 3 4 5 6 7 470 480 490 500 510 520 530 540 550
Constant gyroscope bias (deg./s) Time (s)
Pitch RMSE (deg.)

1.5

ybias (deg./s)
1
100
0.5
0 1 2 3 4 5 6 7 470 480 490 500 510 520 530 540 550
Constant gyroscope bias (deg./s) Time (s)
102
Roll RMSE (deg.)

zbias (deg./s)
4
101
2
0
100 −2
−4
10−1
0 1 2 3 4 5 6 7 470 480 490 500 510 520 530 540 550
Constant gyroscope bias (deg./s) Time (s)

DCM Reference
Madgwick Estimate
Mahony 1-sigma
Figure 9: Root mean squared errors of yaw pitch and roll angles
in the acceleration test as a function of added constant bias in Figure 10: An artificial 1 deg./s bias is added to all gyroscope
gyroscope measurements. measurements in the acceleration test. Gyroscope bias estimators
for bias around 𝑥 and 𝑦 converge rapidly towards the correct bias
(reference, the black slashed line). The corresponding standard
deviations estimated by the EKF are visualized in the same plots
IMU from 15∘ C to 35∘ C. The gyroscope and temperature
using 1-sigma distance to the reference bias.
measurements are shown in Figure 11. A linear bias model of
the measured temperature is plotted over the gyroscope mea-
surements in the figure. Similarly, stationary accelerometer
readings show a linear relationship to temperature, as shown state-of-the-art algorithms. Table 5 summarizes the main
in Figure 12. These results indicate that both accelerometer results and contributions of this paper.
and gyroscope measurements are temperature-dependent Results of the first test (rotation test in Figure 4 and
and that this relation can be modeled using a linear relation- Table 2) show that, in normal operation with fully calibrated
ship if working around room temperatures (25 ± 10∘ C). As data (if there are no gyroscope biases or large dynamic
can be seen, the change in gyroscope bias can be as large as accelerations present), the DCM method has error statistics
0.5 deg./s. quite similar to those for Mahony’s method and slightly
In addition to presenting linearly temperature-dependent smaller errors than those obtained using Madgwick’s method.
gyroscope and accelerometer measurements, we used our The cheaper SparkFun 6DOF IMU is noisier (larger variance)
24 parameter calibration method to calibrate our SparkFun and has larger RMSE, though the maximal errors are similar
6DOF Digital IMU. The acquired calibration parameters to those for the data of Inertia-Link. This test shows that
are presented in Table 4 using a notation presented in (17). our proposed DCM method performs as well as the other
As it can be noted, all temperature-dependent calibration methods in the fully calibrated case.
parameters 𝑎 are small but not negligible, which indicates Results of the acceleration test (in Figure 6 and Table 3)
the minor but existing temperature-dependent effect in the show that when temporary nongravitational acceleration is
accelerometer and the gyroscope. In addition, it can be noted applied, all methods other than our proposed DCM method
that bias terms are more dependent on the temperature than induce large temporary errors to the attitude estimate, espe-
gain parameters. cially to the roll and pitch angles. As can be seen from the
RMSE estimates in Table 3, errors caused by these accelera-
5. Discussion tions do not increase the mean squared errors significantly.
It should be noted that while testing for the effect of rapid
Experiments and Results tested our proposed algorithm with accelerations, the RMSE does not fully reveal the effects
multiple different tests and compared the results to two caused by these short and temporary accelerations. Instead,
International Journal of Navigation and Observation 13

Gyroscope and temperature measurements as a function of time Accelerometer measurements as a function of temperature
2.8 0.1
Gyrox (deg./s)

Accx (m/s2 )
2.6 0
2.4 −0.1
2.2 −0.2
2 −0.3
−0.4
0 500 1000 1500 2000 2500 15 20 25 30 35
Time (s) Temperature ( ∘ C)
−0.1
Gyroy (deg./s)

Accy (m/s2 )
−0.5 −0.2
−0.3
−1 −0.4
−0.5
−1.5 −0.6
0 500 1000 1500 2000 2500
15 20 25 30 35
Time (s)
Temperature ( ∘ C)
Gyroz (deg./s)

1
10
0.8

Accz (m/s2 )
0.6
9.5
0.4
0.2 9
0 500 1000 1500 2000 2500
Time (s) 15 20 25 30 35
Temperature ( ∘ C)
Measurements
Linear temperature model Figure 12: Accelerometer measurements as a function of tempera-
40 ture for calibration purposes. The SparkFun 6DOF Digital IMU is
Temperature ( ∘ C)

first cooled and then held stationary for a long period to warm up to
30
near steady state temperature. A linear model of data as a function
20 of temperature is drawn over plots.

10
0 500 1000 1500 2000 2500
Time (s) the attitude estimator usually requires robust behavior in all
Figure 11: Gyroscope and temperature measurements as a function cases, the behavior of the IMU algorithm should be tested for
of time for calibration purposes. The SparkFun 6DOF Digital IMU is these large temporary accelerations as well.
first cooled and then held stationary for a long period to warm up to Our DCM method shows the most robust behavior
a near steady state temperature. A linear model of bias as a function against these temporary nongravitational accelerations as
of temperature is drawn over the data. compared to the other algorithms. Madgwick’s method is
highly vulnerable to these events with errors nearly one
magnitude larger than the DCM method. Finally, the built-
Table 4: Calibration parameters for SparkFun 6DOF Digital IMU.
in estimate of Microstrain Inertia-Link performs much more
Parameter 𝑥-axis 𝑦-axis 𝑧-axis unreliably than any of the compared methods. This test also
𝑓𝑖
𝑎gain −0.00049316 0.00040477 0.00102091
reveals the only drawback of our proposed DCM method:
𝑓 the bias estimate around the yaw angle cannot be correctly
𝑏gain
𝑖
1.06731340 1.03869310 0.98073082 estimated if there are no rotations present in the data and
𝑓
𝑎bias
𝑖
−0.01551443 −0.00457992 −0.01929765 gravity is accurately aligned to 𝑧-axis of the sensor. Thus,
𝑓𝑖
𝑏bias 0.99740194 0.05868846 0.48880423 in this special case (the observability problem) with fully
𝜔𝑖
𝑎gain 0.00027886 −0.00122424 −0.00246201 calibrated data, the bias estimator impairs the heading angle
𝜔 estimation.
𝑏gain
𝑖
0.99130982 1.03580126 1.08040715
𝜔
The results of the gyroscope bias tolerance test are the
𝑎bias
𝑖
−0.01188330 −0.00845710 0.00542382 most important. As low-cost MEMS gyroscopes usually
𝜔𝑖
𝑏bias 0.24566657 0.19236960 −0.21682853 introduce a temperature-related bias term into the measure-
ments, and as angular velocities measured by the gyroscopes
must be integrated to estimate the attitude, the bias error (i.e.,
error statistics in the box-and-whiskers plotted in Figure 6 zero level error in angular velocity measurements) causes
better uncover the differences between these algorithms. As large errors in the attitude estimate. The results of the third
can be noted, these filters behave quite differently in the test (Figures 7, 8, and 9) show that our proposed DCM
presence of dynamic accelerations. In the literature, these method is able to handle even large bias errors and that as
rare events are typically ignored as outliers. However, as little as 1 deg./s error in the bias can lead to large errors in all
14 International Journal of Navigation and Observation

Table 5: Summary of performed experiments and results.

The test How it is tested? The main results

Rotations and movement in 6D using All algorithms perform similarly well in the
Rotation (Section 4.1) preprogrammed path performed at standard test without any bias. DCM-IMU was
different velocities. slightly precise compared to other algorithms.
DCM-IMU is robust against rapid
Linear movement separately to 𝑥, 𝑦, and
Acceleration (Section 4.2) nongravitational accelerations compared to
𝑧 directions at different velocities.
state-of-the-art algorithms.
Rotation test is performed with an Only DCM-IMU can accurately estimate
Biases with rotation (Section 4.3) artificially added gyroscope bias in the gyroscope biases independent of the amount of
measurement data. added bias.
DCM-IMU can estimate gyroscope biases for 𝑥
Acceleration test is performed with an
Biases with linear acceleration and no and 𝑦 axes. The gyroscope bias around 𝑧-axis is
artificially added gyroscope bias in the
rotations (Section 4.3) unobservable in this case, but the proposed
measurement data.
filter can handle the observability issue.
Gyroscope bias estimates change nearly by
Gyroscope and accelerometer are
0.5 deg./s as the temperature is changed by
Temperature (Section 4.4) measured as a function of temperature
20∘ C. Similarly, accelerometer readings change
while it is changed.
considerably.

the other compared methods. Mahony’s method is then more forces by using adaptive measurement covariance in the EKF
vulnerable to added bias than Madgwick’s method, which algorithm.
is able to cope with some bias errors around pitch and roll As indicated by our test results, the proposed DCM-IMU
angles. In contrast, our DCM method can calibrate gyroscope algorithm should be used with low-cost MEMS sensors when
biases online without any extra information from a compass at least one of the following is true: (a) only accelerometer and
or other sensors. gyroscope measurements are available, (b) there exist large
The last test and related results section demonstrate an temporary accelerations, (c) there exist unknown or drifting
example of bias errors in a low-cost MEMS gyroscope and gyroscope biases, or (d) measurements are collected using
accelerometer. As can be noted in Figure 11, the values of a variable sampling rate. However, our proposed method
the gyroscope bias estimates change nearly 0.5 deg./s as the should not be used if constant nongravitational accelerations
temperature is changed by 20∘ C within less than an hour. are present, nor would it be needed if gyroscopes are
This reveals the importance of calibrating gyroscopes and accurately calibrated and no drifting biases are present in
accelerometers using a temperature-dependent calibration the measurements (i.e., an error-free system). Long-term or
model, of stabilizing the temperature with a heater or cooler, constant nongravitational accelerations will be mixed with
or of using an online bias estimator. The calibration of MEMS the estimated gravitational force, as there are no external
sensors is crucial, difficult task, for which the calibration measurements to separate them.
parameters may still change over time. Nevertheless, using a Finally, our method is best suited for low-cost MEMS sen-
temperature-dependent calibration model and the proposed sors with drifting biases and erroneous measurements. It also
attitude estimation algorithm can enable the system to eliminates the need for commonly used magnetometers while
perform reliably within changing temperatures and drifting estimating biases for gyroscope measurements. The method,
gyroscope biases. however, is not designed to give an absolute heading; instead,
it is best suited for measuring absolute roll and pitch angles
and a minimally drifting relative yaw angle. An open source
6. Conclusion implementation of the proposed algorithm is available for
MATLAB and C++ at https://github.com/hhyyti/dcm-imu.
In this work, we have proposed a partial attitude estimation
algorithm for low-cost MEMS IMUs using a direction cosine
matrix (DCM) to represent orientation. The attitude estimate Appendix
is partial, as only the orientation towards the gravity vector
is estimated. The sensor fusion of triaxial gyroscopes and The proposed system model presented in (7) can be derived
accelerometers was accomplished using an adaptive extended from the continuous-time dynamic model:
Kalman filter. The filter accurately estimates gyroscope biases
online, thus enabling the filter to perform effectively even if ẋ (𝑡) = At x (𝑡) + Bt u (𝑡) , (A.1)
the calibration is inaccurate or some unknown slowly drifting
bias exists in the gyroscope measurements. The proposed where u is a control input (for angular velocities), At is a
DCM IMU is made more robust against temporary contact state-transition model, Bt is a control-input model, and x is
International Journal of Navigation and Observation 15

the state vector. The continuous-time dynamic model of the ̂ 𝑘|𝑘−1 is the unnormalized predicted a priori estimate of
P
system in At and Bt is formulated as the estimate covariance. The angular velocity tensor [U×]
is analogous to [𝑏 𝜔𝑛𝑏 ×] in (2). The only difference between
03×3 − [C3 ×] (𝑡)
At = [ ], these is that, in [U×], estimated gyroscope biases (bias states)
03×3 03×3 are reduced from measured angular velocities that are only
(A.2) present in (2). Hence, [U×] represents a bias corrected
[C3 ×] (𝑡) angular velocity tensor.
Bt = [ ].
03×3 Measurement update and a posteriori update of states
are computed using the Kalman filter algorithm [43] in the
In (A.2) At and Bt are state-dependent as the DCM following way:
vector [C3 ×] changes along the three first states. This makes
the filter nonlinear. The rotation operator [C3 ×] and the
control-input u are defined in (4). Thereby, the dynamic ỹ𝑘 = z𝑘 − Ĥx𝑘|𝑘−1 ,
model can be understood as a rotation caused by angular
velocity measurements and a countervice rotation caused ̂ 𝑘|𝑘−1 HT + R𝑘 ,
S𝑘 = HP
by the estimated gyroscope biases. The model is discretized
(A.5)
using the Euler method, and the following discrete nonlinear
̂ 𝑘|𝑘−1 HT S−1 ,
K𝑘 = P
state-transition function is formed: 𝑘

x̂𝑘|𝑘−1 = 𝑓𝑘−1 (x𝑘−1 , u𝑘−1 ) x̂𝑘|𝑘 = x̂𝑘|𝑘−1 + K𝑘 ỹ𝑘 .

I3 −Δ𝑡 [C3 ×]𝑘−1|𝑘−1
=[ ] x𝑘−1|𝑘−1
03×3 I3 (A.3) In (A.5), z𝑘 is a vector of accelerometer measurements,
H is the observation model, x̂𝑘|𝑘−1 is the predicted (a priori)
Δ𝑡 [C3 ×]𝑘−1|𝑘−1 state estimate, ỹ𝑘 is a measurement residual, P ̂ 𝑘|𝑘−1 is the
+[ ] u𝑘−1 .
03×3 unnormalized predicted (a priori) estimate covariance, R𝑘 is
the measurement noise covariance, and K𝑘 is the Kalman gain
Equation (A.3) is similar to (8); however, the notation is at time index 𝑘.
changed according to syntax in [43]. In the equation, 𝑘 | 𝑘 − 1
The estimate covariance matrix P is updated using the
denotes the predicted value at time step 𝑘 using the values
Joseph form of the covariance update equation, which is
of previous time step 𝑘 − 1. This complex notation is used to
computationally robust against rounding errors, and the
differentiate between prediction and measurement phases in
result is granted to be always positive definite and symmetric
Kalman filter [43]. Δ𝑡 is a sampling interval which can vary
[43, 54]:
in this formulation of the extended Kalman filter, x𝑘−1|𝑘−1 is
a normalized state estimate of the previous time step, and
x̂𝑘|𝑘−1 is an unnormalized predicted (a priori) state estimate of ̂ 𝑘|𝑘−1 [I − K𝑘 H]T + K𝑘 R𝑘 KT .
̂ 𝑘|𝑘 = [I − K𝑘 H] P
P 𝑘
(A.6)
current time step. In the EKF, u𝑘−1 is usually a control input
of the last round; however, in this case, we assume that our
gyroscope measurement at the current round is analogous In (A.6), K𝑘 is the Kalman gain, H is the observation
to the control of the last round. The 𝑘 − 1 notation is left ̂ 𝑘|𝑘−1 is the unnormalized predicted (a priori) esti-
model, P
to indicate the last round in order to be compatible with
mate covariance, R𝑘 is the measurement noise covariance,
standard Kalman filter notation [43]. A similar modification
and P ̂ 𝑘|𝑘 is the unnormalized updated (a posteriori) estimate
has previously been used by [14].
The estimate covariance matrix P is updated in the covariance at time index 𝑘.
prediction step using the following equations: Because the DCM vector presents the bottom row of a
rotation matrix, which is a unit vector, the magnitude of this
̂ 𝑘|𝑘−1 = F𝑘−1 P𝑘−1|𝑘−1 F𝑇 + Q𝑘−1 ,
P vector must always be exactly one. Therefore, it is important
𝑘−1
to implement this constraint into the proposed filter. For this
−Δ𝑡 [U×]𝑘−1|𝑘−1 −Δ𝑡 [C3 ×]𝑘−1|𝑘−1 purpose, we used the projection method by Julier and LaViola
F𝑘−1 = I6 + [ ],
03×3 03×3 Jr. [45]. In our filter, this is implemented by normalizing the
state vector x and dividing the DCM vector by its magnitude
[U×] (A.4) 𝑑:

0 − (𝑏 𝜔𝑧 − 𝑏 𝑏𝑧𝜔 ) 𝑏
𝜔𝑦 − 𝑏 𝑏𝑦𝜔
[ ] 1
=[
[
𝑏
𝜔𝑧 − 𝑏 𝑏𝑧𝜔 0 − (𝑏 𝜔𝑥 − 𝑏 𝑏𝑥𝜔 )]
].
I 0
x𝑘|𝑘 = 𝑔 (̂x𝑘|𝑘 ) = [ 𝑑 3 3×3 ] x̂𝑘|𝑘 ,
[03×3 I3 ]
𝑏 𝑏 𝜔 𝑏
[− ( 𝜔𝑦 − 𝑏𝑦 ) 𝜔𝑥 − 𝑏 𝑏𝑥𝜔 0 ] (A.7)
In (A.4), the linearized state-transition matrix F𝑘−1 is the
Jacobian of the nonlinear state-transition function in (A.3). 𝑑 = √𝑥̂12 + 𝑥̂22 + 𝑥̂32 .
16 International Journal of Navigation and Observation

The effects of normalization are projected into the esti- of the 5th International Symposium on Mechatronics and Its
mate covariance matrix P using Jacobian matrix as a linear Applications (ISMA ’08), pp. 1–9, Amman, Jordan, May 2008.
approximation of the normalization function 𝑔(̂x𝑘|𝑘 ): [7] H. J. Luinge and P. H. Veltink, “Inclination measurement of
human movement using a 3-D accelerometer with autocalibra-
̂ 𝑘|𝑘 JT ,
P𝑘|𝑘 = JP tion,” IEEE Transactions on Neural Systems and Rehabilitation
Engineering, vol. 12, no. 1, pp. 112–121, 2004.
𝜕𝑔 (̂x𝑘|𝑘 ) J1⋅⋅⋅3 03×3 [8] M. H. Raibert, Legged Robots That Balance, MIT Press, 1986.
J= =[ ],
𝜕̂x𝑘|𝑘 03×3 I3 [9] E. Edwan, J. Zhang, J. Zhou, and O. Loffeld, “Reduced DCM
(A.8) based attitude estimation using low-cost IMU and magnetome-
ter triad,” in Proceedings of the 8th Workshop on Positioning
𝑥̂22 + 𝑥̂32 −𝑥̂1 𝑥̂2 −𝑥̂1 𝑥̂3 Navigation and Communication (WPNC ’11), pp. 1–6, IEEE,
1 [ ]
J1⋅⋅⋅3 = 3[[ −𝑥̂1 𝑥̂2 𝑥̂12 + 𝑥̂32 −𝑥̂2 𝑥̂3 ]].
Dresden, Germany, April 2011.
𝑑 [10] E. Foxlin, “Inertial head-tracker sensor fusion by a comple-
2 2
[ −𝑥̂1 𝑥̂3 −𝑥̂2 𝑥̂3 𝑥̂1 + 𝑥̂2 ] mentary separate-bias Kalman filter,” in Proceedings of the IEEE
Virtual Reality Annual International Symposium, pp. 185–194,
In (A.8), J is the analytic solution for the Jacobian of the IEEE, Santa Clara, Calif, USA, April 1996.
normalization function and 𝑑 is the magnitude of the DCM
[11] S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan,
vector defined in (A.7).
“Estimation of IMU and MARG orientation using a gradient
descent algorithm,” in Proceedings of the IEEE International
Conflict of Interests Conference on Rehabilitation Robotics (ICORR ’11), pp. 1–7, IEEE,
Zürich, Switzerland, July 2011.
The authors declare that there is no conflict of interests [12] N. H. Q. Phuong, H.-J. Kang, Y.-S. Suh, and Y.-S. Ro, “A DCM
regarding the publication of this paper. based orientation estimation algorithm with an inertial mea-
surement unit and a magnetic compass,” Journal of Universal
Acknowledgments Computer Science, vol. 15, no. 4, pp. 859–876, 2009.
[13] D. Jurman, M. Jankovec, R. Kamnik, and M. Topič, “Calibration
This work was carried out as part of the Data to Intelligence and data fusion solution for the miniature attitude and heading
(D2I) Research Program funded by Tekes (the Finnish Fund- reference system,” Sensors and Actuators A: Physical, vol. 138, no.
ing Agency for Innovation) and a consortium of companies. 2, pp. 411–420, 2007.
The authors wish to thank Professor Ville Kyrki for the [14] M. Romanovas, L. Klingbeil, M. Trächtler, and Y. Manoli,
opportunity to use the KUKA LWR 4+ robot arm as well as “Efficient orientation estimation algorithm for low cost inertial
for all his comments. and magnetic sensor systems,” in Proceedings of the IEEE/SP 15th
Workshop on Statistical Signal Processing (SSP ’09), pp. 586–589,
IEEE, Cardiff, Wales, September 2009.
References
[15] R. Munguia and A. Grau, “Attitude and heading system based
[1] M. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel, “A on EKF total state configuration,” in Proceedings of the IEEE
complementary filter for attitude estimation of a fixed-wing International Symposium on Industrial Electronics (ISIE ’11), pp.
UAV,” in Proceedings of the IEEE/RSJ International Conference 2147–2152, IEEE, Gdańsk, Poland, June 2011.
on Intelligent Robots and Systems (IROS ’08), pp. 340–345, IEEE, [16] W. Li and J. Wang, “Effective adaptive Kalman filter for MEMS-
Nice, France, September 2008. IMU/magnetometers integrated attitude and heading reference
[2] V. Bistrov, “Performance analysis of alignment process of systems,” Journal of Navigation, vol. 66, no. 1, pp. 99–113, 2013.
MEMS IMU,” International Journal of Navigation and Observa- [17] D. Gebre-Egziabher, R. C. Hayward, and J. D. Powell, “Design
tion, vol. 2012, Article ID 731530, 11 pages, 2012. of multi-sensor attitude determination systems,” IEEE Transac-
[3] Z. Ercan, V. Sezer, H. Heceoglu et al., “Multi-sensor data fusion tions on Aerospace and Electronic Systems, vol. 40, no. 2, pp. 627–
of DCM based orientation estimation for land vehicles,” in Pro- 649, 2004.
ceedings of the IEEE International Conference on Mechatronics [18] J. K. Hall, N. B. Knoebel, and T. W. McLain, “Quaternion
(ICM ’11), pp. 672–677, IEEE, Istanbul, Turkey, April 2011. attitude estimation for miniature air vehicles using a multiplica-
[4] P. Zhou, M. Li, and G. Shen, “Use it free: instantly knowing your tive extended Kalman filter,” in Proceedings of the IEEE/ION
phone attitude,” in Proceedings of the 20th Annual International Position, Location and Navigation Symposium (PLANS ’08), pp.
Conference on Mobile Computing and Networking (MobiCom 1230–1237, IEEE, Monterey, Calif, USA, May 2008.
’14), pp. 605–616, Maui, Hawaii, USA, September 2014. [19] H. G. De Marina, F. J. Pereda, J. M. Giron-Sierra, and F.
[5] L. Lou, X. Xu, J. Cao, Z. Chen, and Y. Xu, “Sensor fusion- Espinosa, “UAV attitude estimation using unscented Kalman
based attitude estimation using low-cost MEMS-IMU for filter and TRIAD,” IEEE Transactions on Industrial Electronics,
mobile robot navigation,” in Proceedings of the 6th IEEE Joint vol. 59, no. 11, pp. 4465–4474, 2012.
International Information Technology and Artificial Intelligence [20] N. S. Kumar and T. Jann, “Estimation of attitudes from a low-
Conference (ITAIC ’11), pp. 465–468, IEEE, Chongqing, China, cost miniaturized inertial platform using Kalman Filter-based
August 2011. sensor fusion algorithm,” Sadhana, vol. 29, pp. 217–235, 2004.
[6] L. Sahawneh and M. A. Jarrah, “Development and calibration [21] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear com-
of low cost MEMS IMU for UAV applications,” in Proceedings plementary filters on the special orthogonal group,” IEEE
International Journal of Navigation and Observation 17

Transactions on Automatic Control, vol. 53, no. 5, pp. 1203–1218, [37] R. Mahony, T. Hamel, J. Trumpf, and C. Lageman, “Nonlinear
2008. attitude observers on SO(3) for complementary and compatible
[22] M. Ruizenaar, E. van der Hall, and M. Weiss, “Gyro bias esti- measurements: a theoretical study,” in Proceedings of the 48th
mation using a dual instrument configuration,” in Proceedings IEEE Conference on Decision and Control, Held Jointly with the
of the 2nd CEAS Specialist Conference on Guidance, Navigation 28th Chinese Control Conference (CDC/CCC ’09), pp. 6407–
& Control (EuroGNC ’13), Delft, The Netherlands, April 2013. 6412, Shanghai, China, December 2009.
[23] M.-D. Hua, G. Ducard, T. Hamel, R. Mahony, and K. Rudin, [38] A. Khosravian and M. Namvar, “Globally exponential estima-
“Implementation of a nonlinear attitude estimator for aerial tion of satellite attitude using a single vector measurement and
robotic vehicles,” IEEE Transactions on Control Systems Tech- gyro,” in Proceedings of the 49th IEEE Conference on Decision
nology, vol. 22, no. 1, pp. 201–213, 2014. and Control (CDC ’10), pp. 364–369, IEEE, Atlanta, Ga, USA,
December 2010.
[24] Z. Wu, Z. Sun, W. Zhang, and Q. Chen, “A novel approach for
attitude estimation using MEMS inertial sensors,” in Proceed- [39] A. Khosravian, J. Trumpf, R. Mahony, and C. Lageman,
ings of the IEEE (SENSORS ’14), pp. 1022–1025, IEEE, Valencia, “Observers for invariant systems on Lie groups with biased
Spain, November 2014. input measurements and homogeneous outputs,” Automatica,
[25] R. Zhu, D. Sun, Z. Zhou, and D. Wang, “A linear fusion vol. 55, pp. 19–26, 2015.
algorithm for attitude determination using low cost MEMS- [40] B. Siciliano and O. Khatib, Springer Handbook of Robotics,
based sensors,” Measurement, vol. 40, no. 3, pp. 322–328, 2007. Springer, 2008.
[26] B. Barshan and H. F. Durrant-Whyte, “Inertial navigation [41] E. Nebot and H. Durrant-Whyte, “Initial calibration and align-
systems for mobile robots,” IEEE Transactions on Robotics and ment of low-cost inertial navigation units for land vehicle
Automation, vol. 11, no. 3, pp. 328–342, 1995. applications,” Journal of Robotic Systems, vol. 16, no. 2, pp. 81–
[27] B. Huyghe, J. Doutreloigne, and J. Vanfleteren, “3D orientation 92, 1999.
tracking based on unscented kalman filtering of accelerometer [42] R. E. Kalman, “A new approach to linear filtering and prediction
and magnetometer data,” in Proceedings of the IEEE Sensors problems,” Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45,
Applications Symposium (SAS ’09), pp. 148–152, New Orleans, 1960.
La, USA, February 2009.
[43] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
[28] S. O. Madgwick, An efficient orientation filter for inertial and
Applications to Tracking and Navigation: Theory Algorithms and
inertial/magnetic sensor arrays, University of Bristol, Bristol,
Software, John Wiley & Sons, New York, NY, USA, 2001.
UK, 2010.
[29] G. Baldwin, R. Mahony, J. Trumpf, T. Hamel, and T. Cheviron, [44] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, MIT
“Complementary filter design on the Special Euclidean group S Press, Cambridge, Mass, USA, 2005.
E (3),” in Proceedings of the European Control Conference, vol. 1, [45] S. J. Julier and J. J. LaViola Jr., “On Kalman filtering with
p. 2, Greece, Athens, July 2007. nonlinear equality constraints,” IEEE Transactions on Signal
[30] T. Hamel and R. Mahony, “Attitude estimation on SO[3] based Processing, vol. 55, no. 6, pp. 2774–2784, 2007.
on direct inertial measurements,” in Proceedings of the IEEE [46] R. S. Jones, “Mathematical functions,” in The C Programmer’s
International Conference on Robotics and Automation (ICRA Companion: ANSI C Library Functions, Silicon Press, Summit,
’06), pp. 2170–2175, IEEE, Orlando, Fla, USA, May 2006. NJ, USA, 1st edition, 1991.
[31] H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi, “Globally [47] S.-H. P. Won and F. Golnaraghi, “A triaxial accelerometer
exponentially stable attitude and gyro bias estimation with calibration method using a mathematical model,” IEEE Trans-
application to GNSS/INS integration,” Automatica, vol. 51, pp. actions on Instrumentation and Measurement, vol. 59, no. 8, pp.
158–166, 2015. 2144–2153, 2010.
[32] A. Khosravian, J. Trumpf, R. Mahony, and T. Hamel, “Velocity
[48] MicroStrain, “Inertia-link inertial measurement unit and ver-
aided attitude estimation on SO(3) with sensor delay,” in
tical gyro,” 2007, http://www.microstrain.com/inertial/Inertia-
Proceedings of the IEEE 53rd Annual Conference on Decision and
Link.
Control (CDC ’14), pp. 114–120, IEEE, Los Angeles, Calif, USA,
December 2014. [49] Analog Devices, Digital Accelerometer ADXL345, Analog
[33] P. Martin and E. Salaün, “Design and implementation of a low- Devices, Norwood, Mass, USA, 2009.
cost observer-based attitude and heading reference system,” [50] InvenSense, ITG-3200 Product Specification, InvenSense, 2011.
Control Engineering Practice, vol. 18, no. 7, pp. 712–722, 2010.
[51] R. Bischoff, J. Kurth, G. Schreiber et al., “The KUKA-DLR
[34] M.-D. Hua, “Attitude estimation for accelerated vehicles using lightweight robot arm—a new reference platform for robotics
GPS/INS measurements,” Control Engineering Practice, vol. 18, research and manufacturing,” in Proceedings of the 41st Interna-
no. 7, pp. 723–732, 2010. tional Symposium on Robotics and the 6th German Conference
[35] H. Chao, C. Coopmans, L. Di, and Y. Q. Chen, “A compara- on Robotics (ISR-ROBOTIK ’10), pp. 1–8, VDE, Munich, Ger-
tive evaluation of low-cost IMUs for unmanned autonomous many, June 2010.
systems,” in Proceedings of the IEEE Conference on Multisensor [52] G. Schreiber, A. Stemmer, and R. Bischoff, “The fast research
Fusion and Integration for Intelligent Systems (MFI ’10), pp. 211– interface for the kuka lightweight robot,” in Proceedings of
216, IEEE, Salt Lake City, Utah, USA, September 2010. the IEEE Workshop on Innovative Robot Control Architectures
[36] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Survey of nonlinear for Demanding (Research) Applications: How to Modify and
attitude estimation methods,” Journal of Guidance, Control, and Enhance Commercial Controllers (ICRA 2010), Anchorage,
Dynamics, vol. 30, no. 1, pp. 12–28, 2007. Alaska, USA, May 2010.
18 International Journal of Navigation and Observation

[53] M. D. Comparetti, E. De Momi, T. Beyl, M. Kunze, J.

Raczkowsky, and G. Ferrigno, “Convergence analysis of an
iterative targeting method for keyhole robotic surgery,” Interna-
tional Journal of Advanced Robotic Systems, vol. 11, no. 1, article
60, 2014.
[54] D. Simon, Optimal State Estimation: Kalman, H Infinity, and
Nonlinear Approaches, John Wiley & Sons, 2006.
 International Journal of

Rotating
Machinery

International Journal of
The Scientific
Engineering Distributed
Journal of
Journal of

Hindawi Publishing Corporation

World Journal
Hindawi Publishing Corporation Hindawi Publishing Corporation
Sensors
Hindawi Publishing Corporation
Sensor Networks
Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014

Journal of

Control Science
and Engineering

Advances in
Civil Engineering
Hindawi Publishing Corporation Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014

Submit your manuscripts at

http://www.hindawi.com

Journal of
Journal of Electrical and Computer
Robotics
Hindawi Publishing Corporation
Engineering
Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014

VLSI Design
Advances in
OptoElectronics
International Journal of

International Journal of
Modelling &
Simulation
Aerospace
Hindawi Publishing Corporation Volume 2014
Navigation and
Observation
Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014
in Engineering
Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014
Engineering
Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014
Hindawi Publishing Corporation
http://www.hindawi.com
http://www.hindawi.com Volume 2014

International Journal of
International Journal of Antennas and Active and Passive Advances in
Chemical Engineering Propagation Electronic Components Shock and Vibration Acoustics and Vibration
Hindawi Publishing Corporation Hindawi Publishing Corporation Hindawi Publishing Corporation Hindawi Publishing Corporation Hindawi Publishing Corporation
http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014 http://www.hindawi.com Volume 2014

View publication stats