A DCM Based Attitude Estimation Algorithm For Low-Cost Mems Imus
A DCM Based Attitude Estimation Algorithm For Low-Cost Mems Imus
net/publication/285392548
CITATIONS READS
18 2,084
2 authors:
Some of the authors of this publication are also working on these related projects:
All content following this page was uploaded by Heikki Hyyti on 01 December 2015.
Research Article
A DCM Based Attitude Estimation Algorithm for Low-Cost
MEMS IMUs
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.
[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
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
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
DCMA
MadgwickA
Mahony A
DCMB
MadgwickB
Mahony B
Inertia-Link
Pitch
Yaw errors (deg.)
10
0 10
(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.)
Reference measurement and accelerations during the acceleration test Error statistics in the acceleration test
Reference velocity (m/s)
1
4
0.5
3
DCMA
MadgwickA
Mahony A
DCMB
MadgwickB
Mahony B
Inertia-Link
5
Accx (m/s2 )
(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 )
Errors to the reference measurement with a bias of 1 deg./s RMSEs as a function of gyroscope bias (rotation test)
10 103
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
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
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
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: 𝑘
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
Rotating
Machinery
International Journal of
The Scientific
Engineering Distributed
Journal of
Journal of
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
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