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.
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, ) and three for gyroscope biases (the bias vector, ):
The state-space model for the proposed system is the following:In (7), is the discrete nonlinear state-transition function at time index . The state-transition function is presented in (8) (see Appendix for the derivation). The function uses the state vector in (6) and gyroscope measurements as control input . The measurement in (7) is derived with a linear and static observation model presented in (9). In (8), is a rotation operator defined in (4), and is a sampling interval which can vary in this formulation of extended Kalman filter. In (9), is the magnitude of the gravity
In (7), and represent a zero mean Gaussian white noise, and 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 :
In (10), there are two parameters. First, 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, 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.
The measurement covariance in (7) is adjusted according to the acceleration measurement as follows:which is a robust covariance for acceleration measurements based on the work by Li and Wang [16]. The proposed measurement covariance is built upon two parts: first, a constant part which represents a variance of a measurement noise for a triaxial accelerometer and second, a variable part which represents a constant variance of estimated acceleration scaled using the magnitude of the estimated nongravitational acceleration , 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 (the first part of the predicted state vector in the EKF) and the current accelerometer measurement deriving from (5):
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 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 and ) should be tuned to include the effect of the used sampling time. Time-dependent modifications could be added similar to that for 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 (the gain for estimated gravitational acceleration) should be set to a much larger value than (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, , in (7) keeps the DCM vector used in the first three states as a unit vector. The constraint is defined according to
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 and roll angles can be estimated using the following equations that can be derived from (1):where is an inverse tangent function with two arguments to distinguish angles in all four quadrants [46] and is the th element of the DCM vector at time index .
Yaw angle, , 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 using the following equation: where is the th index of the DCM matrix at time index . Note that indices and 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 is
In (16) and (17), is a function of accelerometer gain/bias as a function of temperature , and is a function of gyroscope gain/bias as a function of temperature, both separately for each axis . In the equation, is the accelerometer measurement, and is the gyroscope measurement for each axis . All accelerations and angular velocities are in body-fixed frame . In (17), the temperature-dependent linear model is expressed for all different sensors and sensor axes for bias and gain. In the equation, and 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].
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.
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 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.
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.
4.2. Acceleration Test
In the acceleration test, the IMUs were rotated minimally, moving the KUKA robot linearly in the , , and 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.
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.
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.
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 -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 -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 -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.
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 (°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 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.
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.
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 -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.
Appendix
The proposed system model presented in (7) can be derived from the continuous-time dynamic model:where is a control input (for angular velocities), is a state-transition model, is a control-input model, and is the state vector. The continuous-time dynamic model of the system in and is formulated as
In (A.2) and are state-dependent as the DCM vector changes along the three first states. This makes the filter nonlinear. The rotation operator and the control-input are defined in (4). Thereby, the dynamic model can be understood as a rotation caused by angular velocity measurements and a countervice rotation caused by the estimated gyroscope biases. The model is discretized using the Euler method, and the following discrete nonlinear state-transition function is formed:
Equation (A.3) is similar to (8); however, the notation is changed according to syntax in [43]. In the equation, denotes the predicted value at time step using the values of previous time step . This complex notation is used to differentiate between prediction and measurement phases in Kalman filter [43]. is a sampling interval which can vary in this formulation of the extended Kalman filter, is a normalized state estimate of the previous time step, and is an unnormalized predicted (a priori) state estimate of current time step. In the EKF, is usually a control input of the last round; however, in this case, we assume that our gyroscope measurement at the current round is analogous to the control of the last round. The notation is left to indicate the last round in order to be compatible with standard Kalman filter notation [43]. A similar modification has previously been used by [14].
The estimate covariance matrix is updated in the prediction step using the following equations:
In (A.4), the linearized state-transition matrix is the Jacobian of the nonlinear state-transition function in (A.3). is the unnormalized predicted a priori estimate of the estimate covariance. The angular velocity tensor is analogous to in (2). The only difference between these is that, in , estimated gyroscope biases (bias states) are reduced from measured angular velocities that are only present in (2). Hence, represents a bias corrected angular velocity tensor.
Measurement update and a posteriori update of states are computed using the Kalman filter algorithm [43] in the following way:
In (A.5), is a vector of accelerometer measurements, is the observation model, is the predicted (a priori) state estimate, is a measurement residual, is the unnormalized predicted (a priori) estimate covariance, is the measurement noise covariance, and is the Kalman gain at time index .
The estimate covariance matrix is updated using the Joseph form of the covariance update equation, which is computationally robust against rounding errors, and the result is granted to be always positive definite and symmetric [43, 54]:
In (A.6), is the Kalman gain, is the observation model, is the unnormalized predicted (a priori) estimate covariance, is the measurement noise covariance, and is the unnormalized updated (a posteriori) estimate covariance at time index .
Because the DCM vector presents the bottom row of a rotation matrix, which is a unit vector, the magnitude of this vector must always be exactly one. Therefore, it is important to implement this constraint into the proposed filter. For this purpose, we used the projection method by Julier and LaViola Jr. [45]. In our filter, this is implemented by normalizing the state vector x and dividing the DCM vector by its magnitude :
The effects of normalization are projected into the estimate covariance matrix using Jacobian matrix as a linear approximation of the normalization function :
In (A.8), is the analytic solution for the Jacobian of the normalization function and is the magnitude of the DCM vector defined in (A.7).
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
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.