Attitude and gyro bias estimation by the rotation of an inertial measurement unit

In navigation applications, the presence of an unknown bias in the measurement of rate gyros is a key performance-limiting factor. In order to estimate the gyro bias and improve the accuracy of attitude measurement, we proposed a new method which uses the rotation of an inertial measurement unit, which is independent from rigid body motion. By actively changing the orientation of the inertial measurement unit (IMU), the proposed method generates sufficient relations between the gyro bias and tilt angle (roll and pitch) error via ridge body dynamics, and the gyro bias, including the bias that causes the heading error, can be estimated and compensated. The rotation inertial measurement unit method makes the gravity vector measured from the IMU continuously change in a body-fixed frame. By theoretically analyzing the mathematic model, the convergence of the attitude and gyro bias to the true values is proven. The proposed method provides a good attitude estimation using only measurements from an IMU, when other sensors such as magnetometers and GPS are unreliable. The performance of the proposed method is illustrated under realistic robotic motions and the results demonstrate an improvement in the accuracy of the attitude estimation.


Introduction
Determining an accurate attitude is crucial for the navigation of robotic systems in a wide range of applications, such as mobile robots, aerial vehicle, and satellites [1]. The attitude (pitch, roll, and heading) can be estimated by integrating the rigid body kinematic equation of rotation using angular rates measured from a gyroscope [2]. However, the measurement of the gyroscope is subject to bias error. Bias error would hinder the estimation accuracy significantly in long-term navigation, especially with a low-cost inertial gyroscope [3]. It is therefore common to combine measurements provided by other sensors to improve the accuracy of attitude estimation, such as accelerometers [4], magnetometers [5], and GPS [3]. The tilt (pitch and roll) error can be estimated and corrected using the gravity vector from the measurement of the accelerometer, where a Kalman filter or complementary filter [4,5] is normally employed. But the correction of the compass heading (yaw) is not possible with just the accelerometer. Common solutions use heading-correcting measurements from a magnetometer, GPS, or other external aiding technologies, but these approaches are error-prone when the Earth's geomagnetic field is unpredictably distorted [6,7], when GPS signals are unavailable [8], or when external aiding sensors are impossible to install [9]. Therefore, an important problem is how to obtain a good attitude estimator in a long-term application with only an inertial measurement unit consisting of the gyroscope and the accelerometer. A typical application example is attitude estimation for indoor mobile robot navigation with a steel structure nearby [9].
The measurements from the sensors are commonly treated as vectors in existing nonlinear attitude estimation methods (e.g. the gravity vector is treated as the measurement from the accelerometer). Then the previously mentioned problem becomes how to improve the accuracy of the attitude estimation with only a single vector (the gravity vector). Wahba [10] proved that the attitude can be determined by using at least two known non-collinear vectors, and most exiting methods such as Martin and Salaun [11], Mahony [4], Trumpf [12], and Hua [13] make use of the measurements of two or more available vectors to obtain a more accurate attitude estimation. When considering only one single vector, Mahony [4] proved that a single vector observation is sufficient for attitude estimation under the condition of 'persistent excitation'. Khosravian [14] and Namvar [15] considered a satellite equipped with a three-axis gyro and a magnetometer, and proposed numerically stable adaptive observers by employing the International Geomagnetic Reference Field model. Nevertheless, attitude estimation with heading correction combining just the gravity vector provided by the accelerometer and rate gyro remains unsolved.
The rotational inertial navigation system (sometimes called carouseling) has received wide attention in recent years because it can achieve high precision without using costly inertial sensors. Yuan [16] proposed a 16-sequence rotation scheme for the dual-axis rotational inertial navigation system (INS), which can compensate the gyro drift errors without introducing extra system error accumulations. A navigation computation scheme is proposed in [17] to reduce the measurement accuracy requirement of the turntable rotation angles. In [18] the mounting errors are introduced into the sensor model of a dual-axis rotational INS and calibrated to improve the accuracy of the inertial measurement unit (IMU). However, all these rotational INS methods are proposed for navigation applications of ships and submarines, where the tilt angles change very little. In applications where the attitude of the system is varying continuously, since the error caused by the gyro bias would not contribute roughly equally at different attitudes, the carouseling of the IMU could not average out the total estimation error at the end of each period [19]. Therefore, these rotational INS methods cannot eliminate the gyro bias and are not suitable for these applications.
In order to solve the problem that existing methods are limited when magnetometers, GPS, or other external aiding sensors are not feasible in the navigation environment, we developed a novel rotation inertial measurement unit (RIMU) method that only needs a minimal sensor suite including a three-axis gyroscope and a three-axis accelerometer. This method provides attitude and bias estimation for inertial navigation systems with low acceleration when the attitude is varying continuously in the motion, such as a remote operated vehicle (ROV), a wheeled mobile robot, etc. A linear model of the tilt error driven by the unknown gyro bias under multiple orientations is constructed, which can be employed to estimate the bias and compensate to improve the estimation accuracy. Moreover, inspired by the work of Mahony [4] and Hua [13], a nonlinear attitude observer is incorporated into the RIMU method and extends its application to the gravity vector. Compared to their methods, the proposed method can be effective when there are strong magnetic disturbances or even the magnetic field data are unavailable. The designed algorithms are simple to implement and achieve good performance. To evaluate the effectiveness of the algorithms, experimental tests are performed using an industrial robot. This paper is organized as follows: In section 2 the problem is formulated mathematically. The attitude parameterization and the rigid body kinematic model are presented. In section 3 the RIMU method along with associated analyses are presented and two algorithms are developed. In section 4 the experimental results are reported. Finally, concluding remarks are given in section 5.

Sensor model and problem description
Consider a rigid body moving in inertial space equipped with an IMU composed of a three-axis accelerometer and a three-axis gyroscope. Let {A} denote the navigation frame (n-frame) of reference. As shown in figure 1, {A} is associated with the vector (x, y, z). Let {B} be a body-fixed frame (X, Y, Z), whose center coincides with the mass center of the rigid body. The orientation of the body can be modeled by a rotation matrix R = R : (3) is an orthogonal rotation matrix. The classical representation of R is the Euler parameterization with three angles. Let φ, θ, and ψ denote the Euler angles corresponding to the roll, pitch, and heading, respectively, which are commonly used in the aerospace field. The order of the Euler Angle sequence is: The first rotation is about the zaxis by − ψ (from x-y-z to N-′ y -z), the second rotation is about the N-axis by θ, and the third rotation is about the Y-axis by φ, as shown in figure 1.
Then, the attitude matrix R (from {B} to {A},) can be written as [20]: with C and S corresponding to cos(•) and sin(•) operators. Thus, the Euler angle rate of the IMU is given by [20]: where ( ) Ω Ω Ω , , x y z T is the rotating rate around the 3-axis gyro of the IMU in the body fixed frame.
Since the gyroscope suffers from drift, the measurement model is given by [22]: , , x y z T is the true rate of the IMU, Ω m is the measured angular rate, and , , x y z T is the bias (nonzero output of the gyro when the gyro stays stationary) which is assumed to be slowly time-varying compared to the rotational dynamics of the ridgid body [13].
The attitude can be directly calculated by (2) through the integration of the rate measured by the three-axis gyroscope. However, the attitude estimation error caused by the gyro bias will be significant in long-term applications. To improve the accuracy of the attitude estimation, the gyro bias needs to be estimated and compensated.
The accelerometer measures the acceleration of the rigid body, which includes the gravity acceleration, acceleration due to motion, and noise. Here we assume that the rigid body is under low acceleration, hence it is reliable to use the gravity part of acceleration to estimate the attitude. The output of accelerometer A m , under the mentioned assumption can be considered equal to the gravity vector , hence the gravity vector expressed in {B} is given by: With (4) the tilt angles (roll and pitch) can be calculated from the measurements of the three-axis accelerometer of the IMU:

RIMU method and algorithm design
In order to provide a solution to overcome the problem discussed in section 2, we present a method to estimate the gyro bias with only the accelerometer.

Analysis of the effect of gyro bias on heading error
For the k-th measurement of attitude, let θ k and φ k denote the pitch and roll angle of the IMU, respectively. Assuming that the pitch and roll angle rates within the sampling interval time ∆t are constant, then the tilt change calculated by the angular rate measured by the gyro can be derived from equation (2) [20]: T denote the variation of the pitch and roll angle calculated by the acceleration from the accelerometer. Then, an observable difference of the same quantity (variation of roll and pitch) is formed when two independent measurements are made from the gyro and the accelerometer, respectively. The difference is given by: Since the accelerometer measurement results are stable for a long time, and the difference of the variation of tilt angles r k as the tilt error. Now, in light of equations (6)- (8), the relationship between the gyro bias and tilt error can be written by: This relation points out that under every single attitude k, only two formulas from (9) can be derived using the observable difference of tilt changes between the gyro and accelerometer, whereas there are three unknown biases which are unrelated to each other.

RIMU technique
The method we proposed is based on actively rotating the IMU to obtain multiple orientations independent from the motion of the rigid body itself, as shown in figure 2. By controlling the inertial measurement unit about the axis ( ) = u u u u , , x y z T through a rotating mechanism, an ego rotation of Δα, which is independent of the motion of the ridge body, is generated. Therefore, the relative orientation between the IMU frame and the inertial frame has changed. Let S denote the rotation matrix between the IMU coordinate system ox y z b b b 1 1 1 before rotation and the IMU coordinate system ox y z b b b 2 2 2 after rotation, given by Rodrigues' rotation formula [23]: where × u is the cross product matrix of u I is the identity matrix After we rotated the IMU about the axis u by Δα, the attitude matrix and the Euler angles are changed. Consequently, the r k and V k in equation (9) are changed and we can derive a new relation between the gyro bias and tilt error.
The attitude matrix R after rotation is denoted by: Let θ ′ k and φ ′ k denote the new pitch and roll angle derived from R . From equations (6) and (7) we can derive r k and V k respectively. Hence, from equation (8) a new formula containing the gyro bias and variation of the tilt angle is given by: Since the gyro bias is slowly time-varying, for n different k , using (12) we can obtain 2n formulas, which can be written as: x y z T is the gyro bias to estimate. As b is assumed to be slowly varying with respect to the dynamics, we can treat b as a constant before and after the rotation. Consequently, equation (13) is an overdetermined equation. Therefore, b can be solved by the least squares method: When n ⩾ 2, which means the rotation of the IMU produces no fewer than two different attitudes, the gyro bias can be estimated through equation (14). Then, the attitude estimation error, including the heading error, can be effectively reduced by compensating the output of the gyro by using the estimated bias.
To sum up, by setting the IMU under a different orientation with a controlled rotation, we have provided a linear mathematical model which has sufficient relations, so that it is solvable to estimate the gyro bias.
Based on the principle of RIMU and the analysis of ridge body dynamics, an algorithm (algorithm 1) is designed as follows: Step 1: Compute the variance of tilt angle ( ) the measurements by the gyroscope Ω m using equation (6).
Step 2: Compute the variance of tilt angle ( ) the measurements by the accelerometer Am.
Step 4: Form the formula = r Vb k k using the tilt error and current tilt angles.
Step 5: Rotate the IMU by the rotating mechanism to change its orientation, and iterate steps 1-4 to form multiple formulas.
Step 6: Compute the gyro bias using equation (14), and correct the attitude estimation results by compensating the gyro bias.
Step 7: Return R k . Figure 3 shows the flowchart of algorithm 1: Since the initial attitude of the IMU is known, and the rotation axis vector ( ) = u u u u , , x y z T together with fixed rotation angle Δα can be pre-calibrated, and the attitude of the robot embarking the IMU can be determined using the attitude of IMU, u, and Δα. It is noteworthy that there surely would be some misalignment error in the rotation axis even after calibration. However, since the computation of the relation between the IMU and the rigid body is the last step of the algorithm, the misalignment error will not be integrated and is a fixed value, hence it would not affect the effectiveness of the method.

Nonlinear observer with rotation of the IMU
Section 3.2 has already provided a straightforward algorithm to estimate the gyro bias. However, using Euler angle representations during the calculation would sometimes encounter the singularity problem. So in this subsection we present another algorithm to solve the singularity problem.
The kinematic equation of R satisfies the Poisson equation [13]: = Ω × R Ṙ (15) where By using this rigid body dynamics model, the singularity would be avoided during the computation process.
The measurement of the accelerometer Let A m be the gravity vector. Then the problem of limitation in correcting the heading angle using just the accelerometer can be stated as observing the gyro bias with only a single vector. Mahony [12] theoretically illustrated that in the case where a single measurement is made in the body-fixed-frame of a constant reference direction in the inertial frame, the system is unobservable. Consequently, existing methods cannot guarantee that the gravity vector derived from the accelerometer satisfies the 'persistent excitation' condition.
However, by incorporating the RIMU method, we ensure that the vector's direction is permanently varying in the bodyfixed frame.  As shown in figure 4 the proposed non-linear observer is the explicit complementary filter and has dynamics: is the normalized gravity vector; K b , K x are the positive gains; b est is the gyro bias estimation; δ is an intermediate variable. The gravity vector x derived from the measurements of the accelerometer is expressed by The difference between x and x for the x-estimator is determined by × x x. Assuming that the gyro bias is slowly varying with respect to the observer dynamics, we can prove that the gyro bias estimation b est converges with the true value of gyro bias b when with the rotation of the IMU. First, we introduce a Lyapunov function E for the system under the following form: is the bias estimation error. The Lyapunov time derivative of E is then written as: Replacing δ˙T using equation (16), function E becomes: From equation (19), where the function derivative is negative and semi-definite, Lyapunov's argument and LaSalle's Theorem conclude that × x x tends to zero and therefore the estimate x will converge with the true value x. From equations (15) and (16), the derivative of − x x is given by: From equation (20), the convergence of × x x to zero will yield to: Apparently, with the rotation of the IMU, the direction of the gravity vector is continuously changing and is not reduced to a constant vector; equation (21) ensures the bias estimation error converges to zero. Consequently, the proposed method RIMU ensures → b b est , therefore the heading estimation can be integrated with low drift due to the corrected estimation of b. Once the attitude of the IMU is corrected, the attitude of the rigid body can be quickly computed using u and Δα, which are pre-calibrated according to the rotating mechanism.
The algorithm (algorithm 2) is designed as follows: Step 1: Calculate the normalized gravity vector x by using the current rotation matrix, which is derived from the angular rate measurement of gyros Ω m .

A A A
, , x y z T from the measurements of the accelerometer A m .
Step 3: Calculate the estimated gyro bias b est and the compensated angular rate by Ω Ω = − b m est , using the mathematic model of equation (6) and the angular rate measurement Ω m .
Step 4: Compensate the angular rate of gyro Ω , then calculate the rotation matrix R k of the IMU.
Step 5: Output R k .
It is noteworthy that there exists a situation when the IMU is rotated about the gravity vector and the gravity vector will not change. But this situation is rare in most applications and can be avoided by the IMU orientation setting at the initial state of the navigation.

Experimental results
In this section, we present the experimental results to show the efficiency of the proposed methods. The experiments were performed on an industrial robot platform. For the IMU the lowcost MEMS inertial measurement unit MTi-100 of Xsens was used. The bias specification of MTi-100 is 10°/h [24]. The sensors were sampled at a frequency of 100 Hz. An industrial robot MOTOMAN SK6 was used to generate the motion of the IMU.
Experiments were carried out for the following scenario: the IMU was fixed to the robot and rotated by the end-effector. The attitude estimated by the proposed algorithms was compared with the 'ground truth' determined by the industrial robot. The IMU was rotated around a pre-calibrated axis with a 5 s intermittent time, and Δα was set to 90°. The initial angles of the heading and pitch were set to zero and the roll was set at a different initial orientation, and each of the estimation terms lasted for 3600 s with a total of 30 groups of data.
The experimental procedure is set as follows: (1) Attach the rotation mechanism to the end effector of the industrial robot. (2) Set the IMU to rotate about the axis continuously.
(3) Compute the attitude and bias estimation and store the result. (4) Control the industrial robot to move freely at different attitudes and store the attitude results of the end effector. (5) The attitude and gyro bias estimated by the algorithms is compared with the 'ground truth'. The true gyro bias is obtained by the true attitude from the attitude output of the industrial robot (calculated by the axis encoders of the industrial robot).
The strategy of choosing the values of K x and K b was inspired by the complementary filtering in [21]. K x is chosen for the best crossover frequency between a low-pass filter of the accelerometer measurements and a high-pass filter of the attitude measurement from the gyroscope measurements. K b governs the dynamics of the gyro bias estimation. Therefore, K b should be chosen at least 10 times smaller than K x in order to ensure a good time-scale separation between the estimation dynamics of the angles and the gyro bias [21]. In practice, we chose K b 20 times smaller than K x and found the suitable value K x that the algorithm has a good estimation performance.
The gyro-bias estimation results of Algorithm 1 are illustrated in figure 5(a). The gyro bias estimation is very close to the ground truth, with an average error of less than 3° h −1 , which means that the bias has been properly estimated and can be mostly compensated. Although the bias cannot be totally removed, the effectiveness of the bias estimation would improve the performance of the attitude measurement significantly. Figure 5(b) shows the difference in attitude estimation with or without using the RIMU method. Here the performance of the heading angle is mainly examined, since the correction of the pitch and roll angles has already been well investigated and implemented in existing studies. As can be seen, the heading angle drift without using the proposed method is more than 15° after an hour, while after correction the remaining drift is less than 4° h −1 . Figure 6 illustrates the estimation results of algorithm 2. As stated above, we set the values of the parameters K x and K b at = = K K 2, 0.1.
x b Figure 6(a) shows that the estimated gyro biases are also close to the true value. Figure 6(b) shows the good performance of using the RIMU method in correcting heading errors. To sum up, the experimental results of both algorithms confirm experimentally the validation of the proposed method.

Conclusion
In this paper, a novel method for attitude and gyro bias estimation is proposed. The principle of the method is to combine measurements from an IMU consisting of a gyro and accelerometer when rotating the IMU around a precalibrated axis in a controlled manner independent from the rigid body motion. The approach allows for the estimation of the gyro bias by using only the measurement of accelerometers, hence providing a solution when the existing methods are limited by the invalidation of the magnetometer and other external aiding sensors. The experimental results supported the effectiveness of the method in improving the performance of attitude estimation, including the compass heading.