International Journal of Navigation and Observation

Volume 2015, Article ID 503814, 18 pages

http://dx.doi.org/10.1155/2015/503814

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

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

Academic Editor: Aleksandar Dogandzic

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.

#### 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 -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 . This solution is based on the Special Orthogonal group , 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 convention [40], which have the following relation to the DCM:

The notation “” in (1) refers to sine and “” to cosine, to roll, to pitch, and to yaw in Euler angles. The direction cosine matrix , that is, the rotation matrix, defines rotation from the body-fixed frame () to the navigation frame (). It is integrated from initial DCM using an angular velocity tensor formed from triaxial gyroscope measurements in the body-fixed frame according to [41]where

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 which can be updated using which is derived from (1), (2), and (3) [12]. In (4), , are measured angular velocities and , 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 . 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]where , are accelerometer measurements (forming a measurement vector ) in the body-fixed frame and is the magnitude of the Earth’s gravitation field. This simple model assumes that the gravity is aligned parallel to the -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, refers to the EKF state vector defined in (6), is an unnormalized predicted state, is a measurement, is a control input, and 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, , and nongravitational accelerations, , because nongravitational acceleration is estimated using the predicted state in the EKF, thus allowing these two to be separated.