Academic Editor:Aleksandar Dogandzic
Autonomous Systems Research Group, Department of Electrical Engineering and Automation, School of Electrical Engineering, Aalto University, P.O. Box 15500, 00076 Aalto, Finland
Received 16 July 2015; Revised 29 October 2015; Accepted 4 November 2015
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.
1. Introduction
Inertial measurement units (IMUs) are widely used in attitude estimation in mobile robotics, aeronautics, and navigation. An IMU consists of a triaxial accelerometer and a triaxial gyroscope and it is used for measuring accelerations and angular velocities in three orthogonal directions. The attitude, which is a 3D orientation of the IMU with respect to the Earth coordinate system, can be estimated by combining integrated angular velocities and acceleration measurements. Microelectromechanical-system (MEMS) IMUs are small, light, and low-cost solutions for attitude estimation. They are widely used in mobile robotics, such as unmanned aerial vehicles (UAVs) [1]. MEMS IMUs are also used in combination with other sensors, such as global navigation satellite systems (GNSS) [2, 3], light detection and ranging (LIDAR) sensors, or cameras in various applications. In addition, MEMS IMUs are commonly included in modern mobile phones [4].
Unfortunately, the use of low-cost MEMS IMUs introduces several challenges compared to high-precision measurement devices. Low-cost MEMSs are noisy and their measurements usually include various errors. These errors consist of an unknown zero level, that is, bias error, and unknown scale factor, that is, gain error [5]. Moreover, the gain and the bias tend to drift over time and are affected by temperature change. Therefore, sensor calibration has become one of the most challenging issues in inertial navigation [6].
Other problems with IMUs are related to the standard design of the sensor fusion algorithms. Usually, the roll and pitch angles are estimated using the measured angle of the Earth's gravitation force as reference to the integrated angle obtained from angular velocity measurements. This works perfectly if no other forces than gravity exist in the system. Unfortunately, this is rarely the case. In practical use, when an IMU is attached to a moving platform or object, these accelerations are unavoidable. In various cases, as in the estimation of either a human body [7] or a mobile phone [4] position, or in a legged robot [8], these accelerations can become significantly large. Therefore, their presence should be taken into account.
Another problem with standard IMU implementations concerns heading angle estimation. The Earth's gravitation field gives no information about this. Therefore, the integration of triaxial accelerometers and gyroscopes cannot provide an absolute heading angle. This is commonly overcome using an extra sensor, usually a triaxial magnetometer [2, 9-16] or satellite navigation [3, 5, 17-20]. Triaxial magnetometers offer a good solution if the magnetic field measurement can be trusted. In practice, this is usually not the case at least in robotics, since robots are usually made of magnetic metal, have high current electronics and motor drives, and may travel within locations that are surrounded by power lines, magnets, and magnetic metals. Moreover, because the magnetic sensor observes the sum field caused by all magnets and electrically induced magnetic fields, the Earth's magnetic field cannot be easily separated. Furthermore, satellite navigation can be of little help, as it does not work well inside buildings or caves or under dense forest foliage.
The ability to reliably estimate attitude with a minimal number of sensors would also increase the robustness of the system in two ways. Firstly, as fewer sensors would be needed, there would be less risk of sensor failures. Secondly, it would be beneficial to run an IMU algorithm on the background as a backup method, even when other measurements are available. In addition, these results could be compared between the different algorithms to detect failures and possibly compensate for those faults.
Our proposed method solves presented challenges by firstly formulating a calibration method as a function of measured temperature for gains and biases and secondly using an extended Kalman filter to estimate the bias in gyroscope measurements online. We have purposely omitted magnetometer and other possible measurements from our 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 from the results of the proposed filter, it is not an absolute heading. Instead, it is an integrated bias-corrected angular velocity around [figure omitted; refer to PDF] -axis. The absolute yaw could be estimated in a separate filter using any extra measurements. By doing this, we can increase the robustness of the proposed sensor fusion algorithm and enhance gyroscope bias estimation. If the magnetometer or other sensors would fail catastrophically, only the heading estimate would fail, leaving pitch and roll estimates unaffected. This can offer significant advantages for robotic applications such as flying UAVs or other robots.
In contrast to other proposed solutions [21-24], introduced in more detail in the next chapter, we implement an extended Kalman filter to tune the bias estimates when such information is available. Furthermore, we do not rely on constant gains for updating bias. Instead, we use an extended Kalman filter to employ available information in system and measurement covariances in order to tune the gains for updating bias and attitude estimates. Later, in Experiments and Results, we also show in practice that even in the worst case scenario, the filter remains stable.
In addition to the calibration and robust estimation of gyroscope biases, we adapt our measurement covariances to increase the quality of the filter against changing dynamic conditions. IMU algorithms usually use the direction of gravity (through measured accelerations) to reduce accumulating errors in integrated angular velocities during attitude estimation. This causes unwanted errors in the attitude estimate when nongravitational accelerations or contact forces are present. We also use a variable-measurement-covariance method to reduce errors in the attitude estimation caused by rapid and temporary nongravitational accelerations. It is affordable to do, as in the DCM representation of rotation; the bottom row of rotation matrix represents the direction of gravitational force. As a result, our implementation of the extended Kalman filter is able to use only six states for estimating the attitude, gyroscope biases, and the gravity vector.
In Experiments and Results, we show that our solution is significantly more accurate than the compared algorithms in those situations in which either temporary accelerations are present or significant gyroscope biases exist. With fully calibrated bias-free data without nongravitational accelerations, our algorithm performs as well as the compared algorithms, since our gyroscope bias estimates do not disturb attitude estimation.
Our work provides a complete solution that integrates low-cost MEMS IMU, temperature calibration, online bias estimator, and a robust extended Kalman filter which is able to handle large temporary accelerations and changes in sampling rate. We verify our algorithm using multiple different tests and we also obtain accurate reference measurements using the KUKA LWR 4+ robot arm and comparisons to other freely available state-of-the-art algorithms. The used calibration and measurement data for our tests and the proposed algorithm (in MATLAB and C++) are published as open source at https://github.com/hhyyti/dcm-imu.
2. Related Work
Numerous attitude estimation algorithms have become available. Most of these consist of various Kalman filter solutions [2, 3, 5, 12, 13, 16, 25], usually extended Kalman filters (EKF) [7, 9, 10, 15, 17, 18, 20, 26], and some unscented Kalman filters (UKF) [14, 19, 27], though some non-Kalman filter solutions also exist [1, 4, 11, 21, 28-30] as well as some geometric methods [31-34]. In addition, Chao et al. [35] have carried out a comparative study of low-cost IMU filters. Existing algorithms often rely on data obtained from military-grade IMUs, which are usually subject to export restrictions and high cost that limit commercial applications [29]. Several authors have reported that cheaper commercial grade IMUs commonly include non-Gaussian noise in their gyroscope and accelerometer measurements, often leading to instability in connecting classical Kalman and extended Kalman filter algorithms [29, 30]. The same has been noted in an extensive survey of nonlinear attitude estimation methods by Crassidis et al. [36]. They also state that EKF is not as good a solution as other filtering schemes. The survey was published a few years prior to most of the papers presenting DCM based methods [3, 9, 12, 21], and their possibilities were therefore not considered in their review.
Low-cost MEMSs are usually subject to time-dependent errors, such as drifting gyroscope biases. Therefore, all IMU algorithms for low-cost sensors should have an online bias estimator. Unfortunately, few of the previously published algorithms developed for only triaxial accelerometers and gyroscopes have included online bias estimates for gyroscopes. In many algorithms, other measurements are needed in addition to gyroscopes and accelerometers in order to estimate gyroscope biases. The most commonly used sensors are triaxial magnetometers [2, 9-16, 25] and satellite navigation [3, 5, 17-20]. In addition to our work, few filters [21-24, 30] have been able to estimate gyroscope biases without extra sensors in addition to the triaxial accelerometer and gyroscope.
The development of a filter that uses only accelerometer and gyroscope measurements is difficult and can lead to an observability problem. This problem arises when a single vector measurement, such as the gravity through acceleration measurements, gives only information to correct estimates of attitude angles, as well as the related biases, which could rotate that vector. The single vector measurement provides no information about the rotation around that vector. Hamel and Mahony [30] have discussed the problem of orientation and gyroscope bias estimation using passive complementary filter. They have also proposed a solution to estimate biases even in the single vector case. Subsequently, Mahony et al. [21] derived a nonlinear observer, termed the explicit complementary filter, which similarly requires only accelerometer and gyro measurements. In another theoretical study [37], they discussed observability and stability issues that arise especially while using single vector measurements. Finally, they proved that, in these single vector cases, the derivation leads to asymptotically stable observers if they assume persistent excitation of rigid-body motion.
Similar ideas have been later used in the work by Khosravian and Namvar [38], who proposed a nonlinear observer using a magnetometer as a single vector measurement. In addition to their work, Hua et al. [23] have implemented a nonlinear attitude estimator that allows the compensation of gyroscope biases of a low-cost IMU using an antiwindup integration technique. They also show valuable aspects of a practical implementation of a filter. Finally, the observability problem for systems, in which the measurement of system input is corrupted by an unknown constant bias, is tackled in [39].
The observability problem can also be avoided. Ruizenaar et al. [22] solve the problem by adding a second IMU to the system. They propose a filter that uses two sets of triaxial accelerometers and gyroscopes attached in a predefined orientation with respect to each other in order to overcome the observability problem. After their work, Wu et al. [24] overcome the same problem by actively rotating their IMU device. Rather than having two separate measurement devices or an instrumented rotating mechanism, a simpler, cheaper solution to this problem could be obtained, if we could overcome the problem algorithmically.
Currently, the most commonly used (at least among hobbyists with low-cost MEMS) and freely available IMU algorithms are Madgwick's [11] and Mahony's [21] non-Kalman filter methods. Both of these methods are computationally simpler than any Kalman filter implementation. Open-source implementations by Madgwick are used to compare these two state-of-the-art implementations to our work. These implementations are freely available for C and MATLAB at http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/. The explicit complementary filter by Mahony et al. is used as a primary attitude estimation system on several MAV vehicles worldwide [21]. However, neither of these Madgwick's implementations was able to estimate biases using only accelerometer and gyroscope measurements. Nevertheless, we used these, as other implementations were not available at the time of writing.
Mahony's and Baldwin's IMU algorithm [21, 29] is based on an idea roughly similar to our solution. In contrast to our EKF solution, they derive their direct and complementary filters using tools from differential geometry on the Lie group [figure omitted; refer to PDF] . This solution is based on the Special Orthogonal group [figure omitted; refer to PDF] , which is the underlying Lie group structure for space of rotation matrices. Our solution is in principle defined similarly to their explicit complementary filter with bias correction ; however, instead of their constant gains for measurement update and bias estimator, we use EKF to tune these gains.
Madgwick's implementation [11, 28] is a quaternion implementation of Mahony's observer [21], and it uses a gradient descent algorithm in the orientation estimation. Madgwick's implementation [11] also uses simple algebraic modifications [21] to ensure separation of the roll and pitch estimation error from the yaw estimation. This addition helps to deal with unreliable magnetic field measurements. Madgwick's solution is computationally efficient because only one iteration of the gradient descent algorithm is performed for each measurement. Therefore, the filter is better suited for high measurement frequencies. If used sampling rate is larger than 50 Hz, the remaining error is negligible [11]. In our tests, we used a significantly larger rate of 150 Hz (the maximum we could record using Microstrain Inertia-Link).
The effects of dynamic motion and nongravitational accelerations have previously been taken into account in [14-16, 18, 27]. Most of this previous work compares the magnitude of accelerometer measurements to the expected magnitude of Earth's gravitation field [15, 18, 27]. It is not a perfect approach, as exactly the same magnitude of accelerometer measurements can occur in multiple configurations (e.g., free fall and an acceleration of 1 g in any direction). A more exact approach to do this would be using a separate estimate for nongravitational accelerations as in [14]. Although they use three separate filter states for estimating linear acceleration components, this unnecessarily increases the computational load of the filter. One solution would be making an adaptation in the covariance for acceleration measurements using the magnitude of the difference between an estimated gravity vector and a measured acceleration [16]. We implemented this method for our DCM-type representation of orientation. In our representation of rotation, it is efficient to use an estimated gravity vector, as it is included in our state estimates representing the rotation.
If there were extra measurements for the velocity, for example, from satellite navigation sensors, then the nongravitational acceleration could be added to the filter as a state to be estimated. This would make it possible to avoid our proposed method to tune the measurement covariance of accelerometers and to improve the accuracy of the proposed filter, as it would no longer be vulnerable to nontemporary or constant accelerations. Many velocity aided attitude estimation methods are dependent on measurement of linear velocity in addition to gyro and accelerometer measurements in order to reliably compensate for nongravitational accelerations [31-34]. Instead of using velocity measurements, we use only a low-cost triaxial set of an accelerometer and a gyroscope.
3. Methods
3.1. DCM Based Partial Attitude Estimation
Our proposed partial attitude estimation builds upon the work by Phuong et al. [12]. It is based on the relation between the estimated direction of gravity and measured accelerations. The direction of gravity is estimated by integrating measured angular velocities using a partial direction cosine matrix (DCM). The results are translated into Euler angles in the [figure omitted; refer to PDF] convention [40], which have the following relation to the DCM: [figure omitted; refer to PDF]
The notation " [figure omitted; refer to PDF] " in (1) refers to sine and " [figure omitted; refer to PDF] " to cosine, [figure omitted; refer to PDF] to roll, [figure omitted; refer to PDF] to pitch, and [figure omitted; refer to PDF] to yaw in Euler angles. The direction cosine matrix [figure omitted; refer to PDF] , that is, the rotation matrix, defines rotation from the body-fixed frame ( [figure omitted; refer to PDF] ) to the navigation frame ( [figure omitted; refer to PDF] ). It is integrated from initial DCM using an angular velocity tensor [figure omitted; refer to PDF] formed from triaxial gyroscope measurements in the body-fixed frame according to [41] [figure omitted; refer to PDF] where [figure omitted; refer to PDF]
Normally, in DCM-type filters, the whole DCM matrix would be updated; however, in this partial-attitude-estimation case, only the bottom row of the matrix in (1) is estimated for the proposed filter. These bottom-row elements are collected into a row vector [figure omitted; refer to PDF] which can be updated using [figure omitted; refer to PDF] which is derived from (1), (2), and (3) [12]. In (4), [figure omitted; refer to PDF] , [figure omitted; refer to PDF] are measured angular velocities and [figure omitted; refer to PDF] , [figure omitted; refer to PDF] are bottom-row elements of the DCM, which are used as an estimate of the partial attitude. Later in this paper, these three bottom-row elements are called DCM states, which form a DCM vector [figure omitted; refer to PDF] . [figure omitted; refer to PDF] is defined as a rotation operator which rotates the current DCM vector according to the measured angular velocities.
The observation model is constructed using accelerometer measurements, which are compared to the current estimate of the direction of gravity by [12] [figure omitted; refer to PDF] where [figure omitted; refer to PDF] , [figure omitted; refer to PDF] are accelerometer measurements (forming a measurement vector [figure omitted; refer to PDF] ) in the body-fixed frame [figure omitted; refer to PDF] and [figure omitted; refer to PDF] is the magnitude of the Earth's gravitation field. This simple model assumes that the gravity is aligned parallel to the [figure omitted; refer to PDF] -axis and that there is no other acceleration than gravity. Later in this paper, this assumption is relaxed with the application of a variable measurement covariance in the extended Kalman filter algorithm.
3.2. Adaptive Extended Kalman Filter with Gyroscope Bias Estimation and Variable Covariances
An extended Kalman 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 original nonlinear functions, and the covariances are predicted and updated with a linearized mapping. In this case, the measurement model is linear, and the state update is nonlinear. Some attitude estimation algorithms based on previously presented Kalman filters [3, 5, 12, 13, 25] use a simpler linear model; however, in our work, we use a nonlinear state-transition model for a purely nonlinear problem.
We enhance the commonly used standard EKF algorithm through a few additions. First, the filter is adapted to changing measurements by using a variable time-dependent state-prediction and acceleration-dependent measurement covariances. Second, the filter is simplified and made computationally more feasible by using gyroscope measurements as control inputs in the EKF. Third, the magnitude of the DCM vector is constrained to be always exactly one. Finally, the filter is formulated for a variable sampling interval to tolerate jitter or changes in sampling rate.
The filter principle is shown as a simplified block diagram in Figure 1. The accelerometer measurements are used as a measurement for the EKF, and gyroscope measurements are used as control inputs in the prediction subsection. Later on, the EKF is used to fuse these measurements in the update subsection. In Figure 1, [figure omitted; refer to PDF] refers to the EKF state vector defined in (6), [figure omitted; refer to PDF] is an unnormalized predicted state, [figure omitted; refer to PDF] is a measurement, [figure omitted; refer to PDF] is a control input, and [figure omitted; refer to PDF] is a measurement residual. The colored blocks in the figure are updated or estimated online. The accelerometer measurement is drawn using different subblocks for gravitational, [figure omitted; refer to PDF] , and nongravitational accelerations, [figure omitted; refer to PDF] , because nongravitational acceleration is estimated using the predicted state in the EKF, thus allowing these two to be separated.
Figure 1: A simplified block diagram of the DCM IMU filter. The covariance computation has been hidden to simplify the work flow of the EKF filter. The colored blocks are adjusted online.
[figure omitted; refer to PDF]
The state-transition model to update angular velocities to DCM states (4) and the measurement model to incorporate accelerations (5) are formulated into an extended Kalman filter that has six states: three for orientation (the DCM vector, [figure omitted; refer to PDF] ) and three for gyroscope biases (the bias vector, [figure omitted; refer to PDF] ): [figure omitted; refer to PDF]
The state-space model for the proposed system is the following: [figure omitted; refer to PDF] In (7), [figure omitted; refer to PDF] is the discrete nonlinear state-transition function at time index [figure omitted; refer to PDF] . The state-transition function is presented in (8) (see Appendix for the derivation). The function uses the state vector [figure omitted; refer to PDF] in (6) and gyroscope measurements as control input [figure omitted; refer to PDF] . The measurement [figure omitted; refer to PDF] in (7) is derived with a linear and static observation model [figure omitted; refer to PDF] presented in (9). In (8), [figure omitted; refer to PDF] is a rotation operator defined in (4), and [figure omitted; refer to PDF] is a sampling interval which can vary in this formulation of extended Kalman filter. In (9), [figure omitted; refer to PDF] is the magnitude of the gravity [figure omitted; refer to PDF]
In (7), [figure omitted; refer to PDF] and [figure omitted; refer to PDF] represent a zero mean Gaussian white noise, and [figure omitted; refer to PDF] is a simplified time-dependent model for the state-prediction noise. This simplified model is derived assuming a Wiener white noise process in angular velocity measurements (used as control inputs) which are integrated into the DCM state and bias estimates over time 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 noise covariance allows the filter to behave more robustly to changing sampling interval. When this is applied to a covariance model, we simplify and assume that there is no cross-correlation between states, thus yielding the following equation for process noise covariance [figure omitted; refer to PDF] : [figure omitted; refer to PDF]
In (10), there are two parameters. First, [figure omitted; refer to PDF] is the DCM-state-prediction variance which is mainly driven by the control input u (angular velocities), and thus the value of the parameter can be approximated as the variance of noise of gyroscope measurements. Second, [figure omitted; refer to PDF] is the bias-state-prediction variance. In this work, it is assumed that since gyroscope biases drift very slowly, setting a tiny value for the prediction variance of corresponding bias states is reasonable (see experimental parameters in Table 1). This forces the filter to trust its own bias estimate much more than its attitude estimate, and bias estimates change only slightly during each measurement update. Our work omits the optimal estimation of these experimental parameters as acceptable parameters can be found manually.
Table 1: Experimental parameters for IMU algorithms.
Symbol | Quantity | Value |
[figure omitted; refer to PDF] | The acceleration of gravity (around Helsinki, Finland) | 9.8189 m/s2 |
| ||
[figure omitted; refer to PDF] | Sampling interval | ~1/150 s |
| ||
[figure omitted; refer to PDF] | Initial variance of the DCM state | 12 (rad/s)2 |
| ||
( [figure omitted; refer to PDF] )2 | Initial variance of bias states | 0.12 (rad/s)2 |
| ||
[figure omitted; refer to PDF] | DCM-state-prediction variance | 0.12 (rad/s)2 |
| ||
( [figure omitted; refer to PDF] )2 | Bias-state-prediction variance | 0.00012 (rad/s)2 |
| ||
[figure omitted; refer to PDF] | Variance of accelerometer measurement | 0.52 (m/s2 )2 |
| ||
[figure omitted; refer to PDF] | Variance of estimated acceleration | 102 (m/s2 )2 |
| ||
[figure omitted; refer to PDF] | A tuning parameter of Madgwick's filter | 0.1 |
| ||
[figure omitted; refer to PDF] | A tuning parameter of Mahony's filter | 0.5 |
The measurement covariance [figure omitted; refer to PDF] in (7) is adjusted according to the acceleration measurement as follows: [figure omitted; refer to PDF] which is a robust covariance for acceleration measurements based on the work by Li and Wang [16]. The proposed measurement covariance [figure omitted; refer to PDF] is built upon two parts: first, a constant part which represents a variance of a measurement noise for a triaxial accelerometer [figure omitted; refer to PDF] and second, a variable part which represents a constant variance of estimated acceleration [figure omitted; refer to PDF] scaled using the magnitude of the estimated nongravitational acceleration [figure omitted; refer to PDF] , which is the difference between the measured acceleration and the estimated gravity. These nongravitational accelerations are estimated using a relation between a predicted DCM vector [figure omitted; refer to PDF] (the first part of the predicted state vector in the EKF) and the current accelerometer measurement [figure omitted; refer to PDF] deriving from (5): [figure omitted; refer to PDF]
The proposed measurement covariance adaptation is more effective than the standard approach, since it modifies the algorithm to become more robust against rapid accelerations. However, if measurement errors are correlated (i.e., there exists a long-term or constant nongravitational acceleration), the assumption underlying the proposed model would no longer hold. Therefore, this solution is only limited to cases where nongravitational acceleration [figure omitted; refer to PDF] in (12) can be assumed to be temporary.
For simplicity, since the sampling time of accelerometer is assumed to be constant, these parameters (measurement variances [figure omitted; refer to PDF] and [figure omitted; refer to PDF] ) should be tuned to include the effect of the used sampling time. Time-dependent modifications could be added similar to that for [figure omitted; refer to PDF] in (10). However, it is not necessarily needed, as measurement updates can be avoided if a measurement is lost, and the physical sampling time in practice usually remains constant, although the sampling interval might change or measurements might be lost. For a practical implementation, parameter [figure omitted; refer to PDF] (the gain for estimated gravitational acceleration) should be set to a much larger value than [figure omitted; refer to PDF] (measurement noise). This makes the adaptation to changing acceleration values significant compared to measurement noise. For the values used, see experimental parameters in Table 1.
The constraint, [figure omitted; refer to PDF] , in (7) keeps the DCM vector [figure omitted; refer to PDF] used in the first three states as a unit vector. The constraint is defined according to [figure omitted; refer to PDF]
Many different nonlinear filters could be used to solve the proposed estimation problem, for example, Gaussian filters like extended and unscented Kalman filters [44]. We selected an extended Kalman filter [43] as we wanted to minimize computational load of the filter. We used the projection method by Julier and LaViola Jr. [45] to handle the constraint in (7). The derivation and practical implementation of the proposed EKF and the constraint projection are explained in Appendix.
3.3. Computation of Euler Angles from Filter States
To compare our results to other filters and to use the estimate, the estimated attitude should be able to be translated into an Euler angle representation. As the proposed attitude 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 Euler angle representation is not trivial. While the yaw angle should be integrated separately, pitch [figure omitted; refer to PDF] and roll [figure omitted; refer to PDF] angles can be estimated using the following equations that can be derived from (1): [figure omitted; refer to PDF] where [figure omitted; refer to PDF] is an inverse tangent function with two arguments to distinguish angles in all four quadrants [46] and [figure omitted; refer to PDF] is the [figure omitted; refer to PDF] th element of the DCM vector at time index [figure omitted; refer to PDF] .
Yaw angle, [figure omitted; refer to PDF] , can be integrated from bias-corrected angular velocities with (1) and (2) using previously computed roll, pitch, and yaw angles as a starting point. The yaw can be resolved from the full DCM matrix [figure omitted; refer to PDF] using the following equation: [figure omitted; refer to PDF] where [figure omitted; refer to PDF] is the [figure omitted; refer to PDF] th index of the DCM matrix at time index [figure omitted; refer to PDF] . Note that indices [figure omitted; refer to PDF] and [figure omitted; refer to PDF] are not estimated in the proposed filter. In (15), since the upper rows of the matrix are needed, the whole rotation matrix needs to be computed outside the filter if the yaw angle estimate is required.
3.4. Temperature Calibration Method
MEMS gyroscopes and accelerometers usually show at least some bias error, though some gain error might be present as well. To be able to reduce the effect of these errors and to achieve the best performance of the IMU, it is important to calibrate the system before feeding the data into any attitude estimation algorithm. The gains and biases of the MEMS gyroscope and accelerometer vary over time, a large part of which can be explained by temperature changes in the physical instrument. Our observations (Figures 11 and 12) indicate that while working 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 accelerometers 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 measurements. The measurement model is derived from [6] by adding the linear model of temperature for each parameter. The used measurement model for each sensor axis [figure omitted; refer to PDF] is [figure omitted; refer to PDF]
In (16) and (17), [figure omitted; refer to PDF] is a function of accelerometer gain/bias as a function of temperature [figure omitted; refer to PDF] , and [figure omitted; refer to PDF] is a function of gyroscope gain/bias as a function of temperature, both separately for each axis [figure omitted; refer to PDF] . In the equation, [figure omitted; refer to PDF] is the accelerometer measurement, and [figure omitted; refer to PDF] is the gyroscope measurement for each axis [figure omitted; refer to PDF] . All accelerations and angular velocities are in body-fixed frame [figure omitted; refer to PDF] . In (17), the temperature-dependent linear model is expressed for all different sensors and sensor axes for bias and gain. In the equation, [figure omitted; refer to PDF] and [figure omitted; refer to PDF] are interchangeable similar to subscripts gain and bias. As we used the linear model for all biases and gains as a function of temperature, there are a total of four parameters for each sensor axis in the calibration model for the accelerometer and the gyroscope, thus yielding 24 unknown calibration parameters to tune.
The calibration of accelerometers can be performed without accurate reference positions using the iterative mathematical calibration method by Won and Golnaraghi [47]. According to the method, the three-axis accelerometer is placed in six different positions and held stationary during each calibration measurement. Measurements from these six positions are then used in the algorithm iteratively to optimize gains and biases for each axis of the accelerometer sensor. Gyroscope biases and gains are estimated by comparing rotations to a reference measurement and forming a linear model of gains and biases for all axes of the sensor. Similar to Sahawneh and Jarrah [6] who use an instrumented rotation plate to perform gyroscope calibration, we use one rotation axis of the robot arm to accomplish the same task. Whereas their method has 12 parameters, we use 24 unknown parameters. Our parameters can be acquired by using two different strategies and single-temperature methods [6, 47].
The first strategy is having two different constant temperatures and performing the calibration [6] at these two different temperatures. From the resulting two sets of calibration 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 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 calibration measurements as a function of temperature while the device is gradually heated. Next, a linear regression line can be fitted into the data for each sensor axis as a function of temperature (similarly as in Figures 11 and 12). This regression model can then be used to estimate constant temperature averages for two different temperatures. These estimates could be used similarly to the average measurements in the first strategy.
4. Experiments and Results
Experiments were conducted using two independent IMU devices, MicroStrain Inertia-Link [48] and a low-cost SparkFun 6DOF Digital IMU breakout board (combination of an ADXL345 accelerometer [49] and an ITG-3200 gyroscope [50]). The reference measurement was acquired using a KUKA Lightweight Robot 4+ [51] and a Fast Research interface [52] for measuring the pose of the IMUs fixed to the tool of the robot arm. Comparetti et al. [53] measured KUKA LWR 4+ accuracy to be on average 1.18 mm and 0.95° for the translation and rotation components, respectively. Both of the IMU devices were installed coaxially to the robot arm (Figure 2) and aligned to have an axis orientation similar to that for the end effector of the robot. The built-in calibration procedure for MicroStrain Inertia-Link was performed prior to data collection [48].
Figure 2: KUKA robot arm and the two independent IMU devices fixed to the tool. MicroStrain Inertia-Link is installed coaxially below SparkFun 6DOF Digital IMU which is inside the topmost aluminum enclosure.
[figure omitted; refer to PDF]
Since the developed algorithm should be robust for different parameters between different accelerometers and gyroscopes, only one common choice of experimental parameters was used for both measurement devices. The comparison between proposed algorithm and comparison algorithms is thus more general, as parameter tuning plays a less important role in the paper. The experimental parameters for the proposed filter and comparison algorithms are shown in Table 1. These same parameters (measurement and state-prediction covariances and initial values) were used in both IMUs for simplicity. These parameters were tuned using a separate set of measurement data and a robot trajectory as the reference measurement prior to the tests presented later in this work.
The variance of the accelerometer was estimated using measured accelerometer noise in a static situation, and DCM-state-prediction variance was similarly estimated using measured gyroscope noise. Bias-state-prediction variance is manually selected as an arbitrary value that is significantly smaller than DCM-state-prediction variance. Similarly, initial values and the variance of estimated acceleration are initially selected as arbitrary values that are large enough and then tuned manually. The reference measurement was compared to the estimate of the proposed algorithm, and the arbitrarily selected parameters were manually changed until the accuracy became satisfactory. The experimental parameters for the comparison methods by Madgwick and Mahony were selected as their default values (Table 1).
The used data sequence consists of two similar calibration sequences to calibrate accelerometers and gyroscopes before and after the actual test data. These calibration sequences are used to calibrate measurements and remove any bias and gain errors from the measurements using the temperature based calibration method described in Section 3.4, except for Inertia-Link measurements which were calibrated using only a simpler temperature-non-dependent method [6]. Temperature changes could not be taken into account in the 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, thus limiting the possibility of any remaining bias and gain errors in the Inertia-Link test data after calibration.
After calibration of the test data, we separated the test data into two separate test sequences. The first sequence, the acceleration test, is designed to test the magnitude of errors caused by induced linear accelerations. Our hypothesis is that the larger linear acceleration is induced, the more likely it is that there will be larger errors in the attitude estimate. The second test sequence, the rotation test, is designed to test dynamic performance of gyroscopes at different velocities moving according to a preprogrammed path in 6D. The rotation test is designed to be long enough to truly measure drifts and give reliable error statistics. In addition, tests are designed to have the same path in four times at different velocities to cover different frequencies.
In addition to comparing different algorithms with fully calibrated data, the sensitivity of the algorithms to gyroscope biases was tested by adding artificial bias to fully calibrated test data. In the third test, we added different magnitudes of artificial bias to the test data (the same for all sensor axes for simplicity). The root mean squared errors (RMSE) to the reference measurement for yaw, pitch, and roll angles were compared at different added biases. As Madgwick's implementations of his and Mahony's algorithms do not include an online bias estimator, this bias test is not completely fair; however, as there were no other freely available implementations to use, we performed our comparison for only these algorithms (see details in Section 2). The gyroscope bias tolerance test is presented only with Microstrain Inertia-Link data, which is less noisy and has better accuracies with all algorithms in the other tests. The lower-cost, lower-quality SparkFun 6DOF Digital IMU would have yielded very similar results between the compared algorithms.
For the first three tests, the orientation measurement of the KUKA robot arm was used as a reference measurement which was compared to yaw, pitch, and roll angles computed using the proposed DCM-IMU algorithm as well as Madgwick's [11] and Mahony's [21] algorithms as a comparison. The built-in estimate of Microstrain Inertia-Link was also used as a comparison. Errors to the reference measurement are plotted separately for yaw, pitch, and roll angles in Figure 3. The reference measurement is plotted to the topmost subplot in the figure. The figure also shows the acceleration test and the rotation test sequences. As can be noted, exact differences between the compared algorithms are difficult to discern from this plot. Therefore, differences between the algorithms are later studied using a box-and-whiskers plot and root mean squared errors.
Figure 3: The three compared IMU algorithms, a built-in estimate of Microstrain Inertia-Link, and the reference trajectory which is performed using the KUKA LWR 4+ robot arm. An error to the reference measurement is shown for each algorithm for yaw, pitch, and roll angles. The visible time range covers only the performed tests. The calibration sequences before and after the tests are omitted from the figure.
[figure omitted; refer to PDF]
Finally, at the end of results section, we present the effects of temperature change on the used low-cost IMU device, SparkFun 6DOF Digital IMU [49, 50]. This section is important, as it demonstrates the need for our linearly temperature-dependent sensor calibration model and the proposed extension to the previous calibration methods. We also present our temperature-dependent calibration values for our low-cost device.
4.1. Rotation Test
In the rotation test, IMUs were rotated in 6D using a preprogrammed path. The same path was driven four times, first at full speed and then by halving the target velocity at each iteration. To use some well-known standard rotation formalism, the quality of algorithms is compared in Euler angles, which is easily computed for all compared algorithms. To highlight the differences, the errors are visualized using a box-and-whiskers plot in Figure 4. The statistics in the figure are computed from the errors to the reference measurement during the rotation test sequence. The upper subplot shows yaw errors, and lower subplot shows combined roll and pitch errors. As revealed in the figure, roll and pitch errors behave quite similarly, as the measured gravity helps similarly in their estimation (in the [figure omitted; refer to PDF] convention [40]); therefore, their errors are combined into the same statistics plot in Figure 4. The initial yaw error (caused by errors before the test sequence) is reduced from the yaw estimates of all the compared methods.
Figure 4: A box-and-whiskers plot showing error statistics in the rotation test. The red line shows the median, the blue box is drawn between the first and the last quadrants, and whiskers are drawn to the most distant measurements.
[figure omitted; refer to PDF]
All algorithms are computed for two separate devices, Microstrain Inertia-Link ("A" in Figure 4) and SparkFun 6DOF Digital IMU ("B" in Figure 4). The label "Inertia-Link" refers to the built-in orientation estimate of Microstrain Inertia-Link, recorded in addition to raw triaxial accelerations and angular velocities. As our paper focuses on partial attitude estimation (roll and pitch), the main focus of the test is given to the lower subplot in Figure 4. The yaw-error plot (the upper subplot in Figure 4) indicates that yaw drift is not significantly larger than other methods, although its value is integrated outside the proposed partial attitude estimator. The surprisingly good result observed in the yaw angle can be explained by the accurate gyroscope bias estimates in the proposed filter. The root mean squared errors (RMSE) are also counted for all methods in Table 2, where the most accurate results are highlighted for each angle. In this test, the DCM method produced the most accurate algorithm with Inertia-Link data and performed slightly worse than Mahony's method with the SparkFun data.
Table 2: Root mean squared errors of the rotation test.
Method | Yaw RMSE (deg) | Pitch RMSE (deg) | Roll RMSE (deg) |
DCM [figure omitted; refer to PDF] | 1.57 [figure omitted; refer to PDF] | 0.56 [figure omitted; refer to PDF] | 0.61 [figure omitted; refer to PDF] |
Madgwick [figure omitted; refer to PDF] | 2.40 | 0.98 | 1.52 |
Mahony [figure omitted; refer to PDF] | 2.49 | 0.68 | 0.82 |
DCM [figure omitted; refer to PDF] | 3.91 | 1.29 | 1.46 |
Madgwick [figure omitted; refer to PDF] | 4.28 | 1.46 | 1.71 |
Mahony [figure omitted; refer to PDF] | 4.20 | 1.22 | 1.05 |
Inertia-Link | 5.68 | 1.17 | 2.09 |
Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B).
[figure omitted; refer to PDF] The most accurate value in the test.
4.2. Acceleration Test
In the acceleration test, the IMUs were rotated minimally, moving the KUKA robot linearly in the [figure omitted; refer to PDF] , [figure omitted; refer to PDF] , and [figure omitted; refer to PDF] directions separately. The test sequence, shown in Figure 5, consists of back and forth movement, first using maximal velocity and then halving the target velocity at each iteration. The reference velocity, measured using KUKA robot hand, is drawn on top of the figure. The purpose of the test is to show unintended rotations in attitude estimates during temporary accelerations. These errors caused by rapid accelerations have not usually been considered in other papers, but these accelerations are present in practical cases, such as the estimation of a human body [7], a mobile phone [4], or a legged robot [8] orientation.
Figure 5: Reference velocities and accelerations during the acceleration test, at first [figure omitted; refer to PDF] -axis, then [figure omitted; refer to PDF] -axis, and last [figure omitted; refer to PDF] -axis movement separately.
[figure omitted; refer to PDF]
The statistics for the acceleration test using fully calibrated data are presented using a box-and-whiskers plot in Figure 6. In addition, root mean squared errors are computed for all methods in Table 3, where the most accurate results are highlighted. As can be seen from the results, the proposed DCM method is much more robust against rapid accelerations 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 caused by the bias estimate around the yaw axis in the DCM method that is not working perfectly in this test, as there are hardly any rotations present in the test data. In addition, this test reveals that Mahony's method is much more robust than Madgwick's method, which is highly prone to errors caused by rapid accelerations in pitch and roll angles.
Table 3: Root mean squared errors of the acceleration test.
Method | Yaw RMSE (deg) | Pitch RMSE (deg) | Roll RMSE (deg) |
DCM [figure omitted; refer to PDF] | 1.19 | 0.42 | 0.17 [figure omitted; refer to PDF] |
Madgwick [figure omitted; refer to PDF] | 0.31 | 0.93 | 0.87 |
Mahony [figure omitted; refer to PDF] | 0.31 | 0.40 | 0.32 |
DCM [figure omitted; refer to PDF] | 2.29 | 0.26 [figure omitted; refer to PDF] | 0.70 |
Madgwick [figure omitted; refer to PDF] | 0.14 [figure omitted; refer to PDF] | 0.78 | 0.79 |
Mahony [figure omitted; refer to PDF] | 0.16 | 0.30 | 0.40 |
Inertia-Link | 2.61 | 0.50 | 3.45 |
Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B).
[figure omitted; refer to PDF] The most accurate value in the test.
Figure 6: A box-and-whiskers plot showing error statistics in the acceleration test. The red line indicates the median, the blue box is drawn between the first and the last quadrants, and whiskers are drawn to the most distant measurements which are not considered as outliers (red + signs).
[figure omitted; refer to PDF]
4.3. Gyroscope Bias Tolerance Test
The bias test was performed in the same manner for the rotation test (results in Figure 8) and for the acceleration test (results in Figure 9) sequences. To save computation time, all algorithms were cold-started at the beginning of the test sequence, and bias estimates are computed during the test sequence; that is, the filter is reset to the default values as the test begins. This cold start reduces the measured quality of our DCM method, as the biases are assumed to be zero at the start of each test. This first part of the test, when bias estimates are converging, adds most of the measured errors to the DCM method with larger values of induced bias.
For an example of biased behavior of all the compared algorithms, a rotation test where the same bias of 1 deg./s is added to all gyroscope measurements is shown in Figure 7. As can be seen from the figure, for pitch and roll, Madgwick's method performs almost as well as the DCM method, while Mahony's method shows the largest error. For the yaw angle, our DCM is the only method that is able to cope with biased gyroscope measurements and to obtain reliable measurements. The reason for this is the fact that since our method can reliably estimate gyroscope biases for each sensor axis, the end result contains less integrated bias error. The start transient caused by the cold start is also visible in the figure.
Figure 7: Errors in yaw, pitch, and roll as an artificial 1 deg./s bias is added to all gyroscope measurements in the rotation test.
[figure omitted; refer to PDF]
Figure 8: Root mean squared errors of yaw pitch and roll angles in the rotation test as a function of added constant bias in gyroscope measurements.
[figure omitted; refer to PDF]
Figure 9: Root mean squared errors of yaw pitch and roll angles in the acceleration test as a function of added constant bias in gyroscope measurements.
[figure omitted; refer to PDF]
To test bias tolerance, test sequences were computed using different added biases changing from zero to seven degrees per second separately for rotation and acceleration tests. For both of the test sequences (the rotation test in Figure 8 and the acceleration test in Figure 9), the proposed DCM method is able to successfully find the induced bias and estimate it for pitch and roll angles. The bias around the yaw angle for the acceleration test in Figure 9 is not correctly estimated in the EKF, since the test data includes minimally rotations (only linear accelerations). The filter cannot estimate the induced bias around the [figure omitted; refer to PDF] -axis due to the lack of information about the bias present in this test data (see the discussion about the observability problem in Section 2). In the rotation test (results in Figure 8), bias estimates work for all angles, and the effect of induced bias is minimal compared to all other presented algorithms. As can be noted from the figures, the proposed DCM method has the smallest error in nearly all cases where there is at least some unknown gyroscope bias present.
Convergence of the bias estimate in the acceleration test is interesting, as the test is a special pathological case for bias estimate around the [figure omitted; refer to PDF] -axis (corresponds to the yaw angle seen in the upper subplot in Figures 6 and 9, as there are no rotations). In this case, the yaw angle and gyroscope bias estimates around the [figure omitted; refer to PDF] -axis are not observable. The behavior of the proposed filter, while estimating biases and their corresponding variances, is shown in Figure 10 when there is an induced bias of 1 deg./s in each gyroscope axis. In the figure, biases for pitch and roll converge rapidly towards the true value of 1 deg./s, whereas the bias estimate around yaw converges very slowly. This happens as the filter receives marginal information about the tiny rotations present in the data. Even in this pathological special case with the observability problem, the presented filter behaves correctly, as demonstrated by the absence of any increase in the variance of the bias estimate; that is, the filter remains stable.
Figure 10: An artificial 1 deg./s bias is added to all gyroscope measurements in the acceleration test. Gyroscope bias estimators for bias around [figure omitted; refer to PDF] and [figure omitted; refer to PDF] 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 using 1-sigma distance to the reference bias.
[figure omitted; refer to PDF]
Figure 11: Gyroscope and temperature measurements as a function of time for calibration purposes. The SparkFun 6DOF Digital IMU is first cooled and then held stationary for a long period to warm up to a near steady state temperature. A linear model of bias as a function of temperature is drawn over the data.
[figure omitted; refer to PDF]
Figure 12: Accelerometer measurements as a function of temperature for calibration purposes. The SparkFun 6DOF Digital IMU is first cooled and then held stationary for a long period to warm up to near steady state temperature. A linear model of data as a function of temperature is drawn over plots.
[figure omitted; refer to PDF]
4.4. Effects of Temperature to Bias
To test the sensitivity of low-cost MEMS IMUs to temperature changes, a long calibration sequence over different temperatures was performed for the SparkFun 6DOF Digital IMU. First, the device was cooled down and then held stationary for over 40 minutes. During that time, the excess heat from the electronics warmed the IMU from 15°C to 35°C. The gyroscope and temperature measurements are shown in Figure 11. A linear bias model of the measured temperature is plotted over the gyroscope measurements in the figure. Similarly, stationary accelerometer readings show a linear relationship to temperature, as shown in Figure 12. These results indicate that both accelerometer and gyroscope measurements are temperature-dependent and that this relation can be modeled using a linear relationship if working around room temperatures ( [figure omitted; refer to PDF] °C). As can be seen, the change in gyroscope bias can be as large as 0.5 deg./s.
In addition to presenting linearly temperature-dependent gyroscope and accelerometer measurements, we used our 24 parameter calibration method to calibrate our SparkFun 6DOF Digital IMU. The acquired calibration parameters are presented in Table 4 using a notation presented in (17). As it can be noted, all temperature-dependent calibration parameters [figure omitted; refer to PDF] are small but not negligible, which indicates the minor but existing temperature-dependent effect in the accelerometer and the gyroscope. In addition, it can be noted that bias terms are more dependent on the temperature than gain parameters.
Table 4: Calibration parameters for SparkFun 6DOF Digital IMU.
Parameter | [figure omitted; refer to PDF] -axis | [figure omitted; refer to PDF] -axis | [figure omitted; refer to PDF] -axis |
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
| |||
[figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] | [figure omitted; refer to PDF] |
5. Discussion
Experiments and Results tested our proposed algorithm with multiple different tests and compared the results to two state-of-the-art algorithms. Table 5 summarizes the main results and contributions of this paper.
Table 5: Summary of performed experiments and results.
The test | How it is tested? | The main results |
Rotation (Section 4.1) | Rotations and movement in 6D using preprogrammed path performed at different velocities. | All algorithms perform similarly well in the standard test without any bias. DCM-IMU was slightly precise compared to other algorithms. |
| ||
Acceleration (Section 4.2) | Linear movement separately to [figure omitted; refer to PDF] , [figure omitted; refer to PDF] , and [figure omitted; refer to PDF] directions at different velocities. | DCM-IMU is robust against rapid nongravitational accelerations compared to state-of-the-art algorithms. |
| ||
Biases with rotation (Section 4.3) | Rotation test is performed with an artificially added gyroscope bias in the measurement data. | Only DCM-IMU can accurately estimate gyroscope biases independent of the amount of added bias. |
| ||
Biases with linear acceleration and no rotations (Section 4.3) | Acceleration test is performed with an artificially added gyroscope bias in the measurement data. | DCM-IMU can estimate gyroscope biases for [figure omitted; refer to PDF] and [figure omitted; refer to PDF] axes. The gyroscope bias around [figure omitted; refer to PDF] -axis is unobservable in this case, but the proposed filter can handle the observability issue. |
| ||
Temperature (Section 4.4) | Gyroscope and accelerometer are measured as a function of temperature while it is changed. | Gyroscope bias estimates change nearly by 0.5 deg./s as the temperature is changed by 20°C. Similarly, accelerometer readings change considerably. |
Results of the first test (rotation test in Figure 4 and Table 2) show that, in normal operation with fully calibrated data (if there are no gyroscope biases or large dynamic accelerations present), the DCM method has error statistics quite similar to those for Mahony's method and slightly smaller errors than those obtained using Madgwick's method. The cheaper SparkFun 6DOF IMU is noisier (larger variance) and has larger RMSE, though the maximal errors are similar to those for the data of Inertia-Link. This test shows that our proposed DCM method performs as well as the other methods in the fully calibrated case.
Results of the acceleration test (in Figure 6 and Table 3) show that when temporary nongravitational acceleration is applied, all methods other than our proposed DCM method induce large temporary errors to the attitude estimate, especially to the roll and pitch angles. As can be seen from the RMSE estimates in Table 3, errors caused by these accelerations do not increase the mean squared errors significantly. It should be noted that while testing for the effect of rapid accelerations, the RMSE does not fully reveal the effects caused by these short and temporary accelerations. Instead, error statistics in the box-and-whiskers plotted in Figure 6 better uncover the differences between these algorithms. As can be noted, these filters behave quite differently in the presence of dynamic accelerations. In the literature, these rare events are typically ignored as outliers. However, as the attitude estimator usually requires robust behavior in all cases, the behavior of the IMU algorithm should be tested for these large temporary accelerations as well.
Our DCM method shows the most robust behavior against these temporary nongravitational accelerations as 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-in estimate of Microstrain Inertia-Link performs much more unreliably than any of the compared methods. This test also reveals the only drawback of our proposed DCM method: the bias estimate around the yaw angle cannot be correctly estimated if there are no rotations present in the data and gravity is accurately aligned to [figure omitted; refer to PDF] -axis of the sensor. Thus, in this special case (the observability problem) with fully calibrated data, the bias estimator impairs the heading angle estimation.
The results of the gyroscope bias tolerance test are the most important. As low-cost MEMS gyroscopes usually introduce a temperature-related bias term into the measurements, and as angular velocities measured by the gyroscopes must be integrated to estimate the attitude, the bias error (i.e., zero level error in angular velocity measurements) causes large errors in the attitude estimate. The results of the third test (Figures 7, 8, and 9) show that our proposed DCM method is able to handle even large bias errors and that as little as 1 deg./s error in the bias can lead to large errors in all the other compared methods. Mahony's method is then more vulnerable to added bias than Madgwick's method, which is able to cope with some bias errors around pitch and roll angles. In contrast, our DCM method can calibrate gyroscope biases online without any extra information from a compass or other sensors.
The last test and related results section demonstrate an example of bias errors in a low-cost MEMS gyroscope and accelerometer. As can be noted in Figure 11, the values of the gyroscope bias estimates change nearly 0.5 deg./s as the temperature is changed by 20°C within less than an hour. This reveals the importance of calibrating gyroscopes and accelerometers using a temperature-dependent calibration model, of stabilizing the temperature with a heater or cooler, or of using an online bias estimator. The calibration of MEMS sensors is crucial, difficult task, for which the calibration parameters may still change over time. Nevertheless, using a temperature-dependent calibration model and the proposed attitude estimation algorithm can enable the system to perform reliably within changing temperatures and drifting gyroscope biases.
6. Conclusion
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 is partial, as only the orientation towards the gravity vector is estimated. The sensor fusion of triaxial gyroscopes and accelerometers was accomplished using an adaptive extended Kalman filter. The filter accurately estimates gyroscope biases online, thus enabling the filter to perform effectively even if the calibration is inaccurate or some unknown slowly drifting bias exists in the gyroscope measurements. The proposed DCM IMU is made more robust against temporary contact forces by using adaptive measurement covariance in the EKF algorithm.
As indicated by our test results, the proposed DCM-IMU algorithm should be used with low-cost MEMS sensors when at least one of the following is true: (a) only accelerometer and gyroscope measurements are available, (b) there exist large temporary accelerations, (c) there exist unknown or drifting gyroscope biases, or (d) measurements are collected using a variable sampling rate. However, our proposed method should not be used if constant nongravitational accelerations are present, nor would it be needed if gyroscopes are accurately calibrated and no drifting biases are present in the measurements (i.e., an error-free system). Long-term or constant nongravitational accelerations will be mixed with the estimated gravitational force, as there are no external measurements to separate them.
Finally, our method is best suited for low-cost MEMS sensors with drifting biases and erroneous measurements. It also eliminates the need for commonly used magnetometers while estimating biases for gyroscope measurements. The method, 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 implementation of the proposed algorithm is available for MATLAB and C++ at https://github.com/hhyyti/dcm-imu.
Acknowledgments
This work was carried out as part of the Data to Intelligence (D2I) Research Program funded by Tekes (the Finnish Funding Agency for Innovation) and a consortium of companies. The authors wish to thank Professor Ville Kyrki for the opportunity to use the KUKA LWR 4+ robot arm as well as for all his comments.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
[1] M. Euston, P. Coote, R. Mahony, J. Kim, T. Hamel, "A complementary filter for attitude estimation of a fixed-wing UAV," in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '08), pp. 340-345, IEEE, Nice, France, September 2008.
[2] V. Bistrov, "Performance analysis of alignment process of MEMS IMU," International Journal of Navigation and Observation , vol. 2012, 2012.
[3] Z. Ercan, V. Sezer, H. Heceoglu, C. Dikilitas, M. Gokasan, A. Mugan, S. Bogosyan, "Multi-sensor data fusion of DCM based orientation estimation for land vehicles," in Proceedings of the IEEE International Conference on Mechatronics (ICM '11), pp. 672-677, IEEE, Istanbul, Turkey, April 2011.
[4] P. Zhou, M. Li, G. Shen, "Use it free: instantly knowing your phone attitude," in Proceedings of the 20th Annual International Conference on Mobile Computing and Networking (MobiCom '14), pp. 605-616, Maui, Hawaii, USA, September 2014.
[5] L. Lou, X. Xu, J. Cao, Z. Chen, Y. Xu, "Sensor fusion-based attitude estimation using low-cost MEMS-IMU for mobile robot navigation," in Proceedings of the 6th IEEE Joint International Information Technology and Artificial Intelligence Conference (ITAIC '11), pp. 465-468, IEEE, Chongqing, China, August 2011.
[6] L. Sahawneh, M. A. Jarrah, "Development and calibration of low cost MEMS IMU for UAV applications," in Proceedings of the 5th International Symposium on Mechatronics and Its Applications (ISMA '08), pp. 1-9, Amman, Jordan, May 2008.
[7] H. J. Luinge, P. H. Veltink, "Inclination measurement of human movement using a 3-D accelerometer with autocalibration," IEEE Transactions on Neural Systems and Rehabilitation Engineering , vol. 12, no. 1, pp. 112-121, 2004.
[8] M. H. Raibert Legged Robots That Balance , MIT Press, 1986.
[9] E. Edwan, J. Zhang, J. Zhou, O. Loffeld, "Reduced DCM based attitude estimation using low-cost IMU and magnetometer triad," in Proceedings of the 8th Workshop on Positioning Navigation and Communication (WPNC '11), pp. 1-6, IEEE, Dresden, Germany, April 2011.
[10] E. Foxlin, "Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter," in Proceedings of the IEEE Virtual Reality Annual International Symposium, pp. 185-194, IEEE, Santa Clara, Calif, USA, April 1996.
[11] S. O. H. Madgwick, A. J. L. Harrison, R. Vaidyanathan, "Estimation of IMU and MARG orientation using a gradient descent algorithm," in Proceedings of the IEEE International Conference on Rehabilitation Robotics (ICORR '11), pp. 1-7, IEEE, Zürich, Switzerland, July 2011.
[12] N. H. Q. Phuong, H.-J. Kang, Y.-S. Suh, Y.-S. Ro, "A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass," Journal of Universal Computer Science , vol. 15, no. 4, pp. 859-876, 2009.
[13] D. Jurman, M. Jankovec, R. Kamnik, M. Topic, "Calibration and data fusion solution for the miniature attitude and heading reference system," Sensors and Actuators A: Physical , vol. 138, no. 2, pp. 411-420, 2007.
[14] M. Romanovas, L. Klingbeil, M. Trachtler, Y. Manoli, "Efficient orientation estimation algorithm for low cost inertial 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.
[15] R. Munguia, A. Grau, "Attitude and heading system based on EKF total state configuration," in Proceedings of the IEEE International Symposium on Industrial Electronics (ISIE '11), pp. 2147-2152, IEEE, Gdansk, Poland, June 2011.
[16] W. Li, J. Wang, "Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems," Journal of Navigation , vol. 66, no. 1, pp. 99-113, 2013.
[17] D. Gebre-Egziabher, R. C. Hayward, J. D. Powell, "Design of multi-sensor attitude determination systems," IEEE Transactions on Aerospace and Electronic Systems , vol. 40, no. 2, pp. 627-649, 2004.
[18] J. K. Hall, N. B. Knoebel, T. W. McLain, "Quaternion attitude estimation for miniature air vehicles using a multiplicative extended Kalman filter," in Proceedings of the IEEE/ION Position, Location and Navigation Symposium (PLANS '08), pp. 1230-1237, IEEE, Monterey, Calif, USA, May 2008.
[19] H. G. De Marina, F. J. Pereda, J. M. Giron-Sierra, F. Espinosa, "UAV attitude estimation using unscented Kalman filter and TRIAD," IEEE Transactions on Industrial Electronics , vol. 59, no. 11, pp. 4465-4474, 2012.
[20] N. S. Kumar, T. Jann, "Estimation of attitudes from a low-cost miniaturized inertial platform using Kalman Filter-based sensor fusion algorithm," Sadhana , vol. 29, pp. 217-235, 2004.
[21] R. Mahony, T. Hamel, J.-M. Pflimlin, "Nonlinear complementary filters on the special orthogonal group," IEEE Transactions on Automatic Control , vol. 53, no. 5, pp. 1203-1218, 2008.
[22] M. Ruizenaar, E. van der Hall, M. Weiss, "Gyro bias estimation using a dual instrument configuration," in Proceedings of the 2nd CEAS Specialist Conference on Guidance, Navigation & Control (EuroGNC '13), Delft, The Netherlands, April 2013.
[23] M.-D. Hua, G. Ducard, T. Hamel, R. Mahony, K. Rudin, "Implementation of a nonlinear attitude estimator for aerial robotic vehicles," IEEE Transactions on Control Systems Technology , vol. 22, no. 1, pp. 201-213, 2014.
[24] Z. Wu, Z. Sun, W. Zhang, Q. Chen, "A novel approach for attitude estimation using MEMS inertial sensors," in Proceedings of the IEEE (SENSORS '14), pp. 1022-1025, IEEE, Valencia, Spain, November 2014.
[25] R. Zhu, D. Sun, Z. Zhou, D. Wang, "A linear fusion algorithm for attitude determination using low cost MEMS-based sensors," Measurement , vol. 40, no. 3, pp. 322-328, 2007.
[26] B. Barshan, H. F. Durrant-Whyte, "Inertial navigation systems for mobile robots," IEEE Transactions on Robotics and Automation , vol. 11, no. 3, pp. 328-342, 1995.
[27] B. Huyghe, J. Doutreloigne, J. Vanfleteren, "3D orientation tracking based on unscented kalman filtering of accelerometer and magnetometer data," in Proceedings of the IEEE Sensors Applications Symposium (SAS '09), pp. 148-152, New Orleans, La, USA, February 2009.
[28] S. O. Madgwick An efficient orientation filter for inertial and inertial/magnetic sensor arrays , University of Bristol, Bristol, UK, 2010.
[29] G. Baldwin, R. Mahony, J. Trumpf, T. Hamel, T. Cheviron, "Complementary filter design on the Special Euclidean group S E (3)," in Proceedings of the European Control Conference, vol. 1, pp. 2, Greece, Athens, July 2007.
[30] T. Hamel, R. Mahony, "Attitude estimation on SO[3] based on direct inertial measurements," in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '06), pp. 2170-2175, IEEE, Orlando, Fla, USA, May 2006.
[31] H. F. Grip, T. I. Fossen, T. A. Johansen, A. Saberi, "Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration," Automatica , vol. 51, pp. 158-166, 2015.
[32] A. Khosravian, J. Trumpf, R. Mahony, T. Hamel, "Velocity aided attitude estimation on SO(3) with sensor delay," in Proceedings of the IEEE 53rd Annual Conference on Decision and Control (CDC '14), pp. 114-120, IEEE, Los Angeles, Calif, USA, December 2014.
[33] P. Martin, E. Salaün, "Design and implementation of a low-cost observer-based attitude and heading reference system," Control Engineering Practice , vol. 18, no. 7, pp. 712-722, 2010.
[34] M.-D. Hua, "Attitude estimation for accelerated vehicles using GPS/INS measurements," Control Engineering Practice , vol. 18, no. 7, pp. 723-732, 2010.
[35] H. Chao, C. Coopmans, L. Di, Y. Q. Chen, "A comparative evaluation of low-cost IMUs for unmanned autonomous systems," in Proceedings of the IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI '10), pp. 211-216, IEEE, Salt Lake City, Utah, USA, September 2010.
[36] J. L. Crassidis, F. L. Markley, Y. Cheng, "Survey of nonlinear attitude estimation methods," Journal of Guidance, Control, and Dynamics , vol. 30, no. 1, pp. 12-28, 2007.
[37] R. Mahony, T. Hamel, J. Trumpf, C. Lageman, "Nonlinear attitude observers on SO(3) for complementary and compatible measurements: a theoretical study," in Proceedings of the 48th IEEE Conference on Decision and Control, Held Jointly with the 28th Chinese Control Conference (CDC/CCC '09), pp. 6407-6412, Shanghai, China, December 2009.
[38] A. Khosravian, M. Namvar, "Globally exponential estimation of satellite attitude using a single vector measurement and gyro," in Proceedings of the 49th IEEE Conference on Decision and Control (CDC '10), pp. 364-369, IEEE, Atlanta, Ga, USA, December 2010.
[39] A. Khosravian, J. Trumpf, R. Mahony, C. Lageman, "Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs," Automatica , vol. 55, pp. 19-26, 2015.
[40] B. Siciliano, O. Khatib Springer Handbook of Robotics , Springer, 2008.
[41] E. Nebot, H. Durrant-Whyte, "Initial calibration and alignment of low-cost inertial navigation units for land vehicle applications," Journal of Robotic Systems , vol. 16, no. 2, pp. 81-92, 1999.
[42] R. E. Kalman, "A new approach to linear filtering and prediction problems," Journal of Basic Engineering , vol. 82, no. 1, pp. 35-45, 1960.
[43] Y. Bar-Shalom, X. R. Li, T. Kirubarajan Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software , John Wiley & Sons, New York, NY, USA, 2001.
[44] S. Thrun, W. Burgard, D. Fox Probabilistic Robotics , MIT Press, Cambridge, Mass, USA, 2005.
[45] S. J. Julier, J. J. LaViola Jr., "On Kalman filtering with nonlinear equality constraints," IEEE Transactions on Signal Processing , vol. 55, no. 6, pp. 2774-2784, 2007.
[46] R. S. Jones, "Mathematical functions," The C Programmer's Companion: ANSI C Library Functions , Silicon Press, Summit, NJ, USA, 1991., 1st.
[47] S.-H. P. Won, F. Golnaraghi, "A triaxial accelerometer calibration method using a mathematical model," IEEE Transactions on Instrumentation and Measurement , vol. 59, no. 8, pp. 2144-2153, 2010.
[48] MicroStrain, "Inertia-link inertial measurement unit and vertical gyro," 2007, http://www.microstrain.com/inertial/Inertia-Link
[49] Analog Devices Digital Accelerometer ADXL345 , Analog Devices, Norwood, Mass, USA, 2009.
[50] InvenSense ITG-3200 Product Specification , InvenSense, 2011.
[51] R. Bischoff, J. Kurth, G. Schreiber, R. Koeppe, A. Albu-Schaffer, A. Beyer, O. Eiberger, S. Haddadin, A. Stemmer, G. Grunwald, G. Hirzinger, "The KUKA-DLR lightweight robot arm-a new reference platform for robotics research and manufacturing," in Proceedings of the 41st International Symposium on Robotics and the 6th German Conference on Robotics (ISR-ROBOTIK '10), pp. 1-8, VDE, Munich, Germany, June 2010.
[52] G. Schreiber, A. Stemmer, R. Bischoff, "The fast research interface for the kuka lightweight robot," in Proceedings of the IEEE Workshop on Innovative Robot Control Architectures for Demanding (Research) Applications: How to Modify and Enhance Commercial Controllers (ICRA 2010), Anchorage, Alaska, USA, May 2010.
[53] M. D. Comparetti, E. De Momi, T. Beyl, M. Kunze, J. Raczkowsky, G. Ferrigno, "Convergence analysis of an iterative targeting method for keyhole robotic surgery," International 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.
You have requested "on-the-fly" machine translation of selected content from our databases. This functionality is provided solely for your convenience and is in no way intended to replace human translation. Show full disclaimer
Neither ProQuest nor its licensors make any representations or warranties with respect to the translations. The translations are automatically generated "AS IS" and "AS AVAILABLE" and are not retained in our systems. PROQUEST AND ITS LICENSORS SPECIFICALLY DISCLAIM ANY AND ALL EXPRESS OR IMPLIED WARRANTIES, INCLUDING WITHOUT LIMITATION, ANY WARRANTIES FOR AVAILABILITY, ACCURACY, TIMELINESS, COMPLETENESS, NON-INFRINGMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Your use of the translations is subject to all use restrictions contained in your Electronic Products License Agreement and by using the translation functionality you agree to forgo any and all claims against ProQuest or its licensors for your use of the translation functionality and any output derived there from. Hide full disclaimer
Copyright © 2015 Heikki Hyyti and Arto 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.
Abstract
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.
You have requested "on-the-fly" machine translation of selected content from our databases. This functionality is provided solely for your convenience and is in no way intended to replace human translation. Show full disclaimer
Neither ProQuest nor its licensors make any representations or warranties with respect to the translations. The translations are automatically generated "AS IS" and "AS AVAILABLE" and are not retained in our systems. PROQUEST AND ITS LICENSORS SPECIFICALLY DISCLAIM ANY AND ALL EXPRESS OR IMPLIED WARRANTIES, INCLUDING WITHOUT LIMITATION, ANY WARRANTIES FOR AVAILABILITY, ACCURACY, TIMELINESS, COMPLETENESS, NON-INFRINGMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Your use of the translations is subject to all use restrictions contained in your Electronic Products License Agreement and by using the translation functionality you agree to forgo any and all claims against ProQuest or its licensors for your use of the translation functionality and any output derived there from. Hide full disclaimer