A quaternion-based Attitude Estimate System Based on a Low Power Consumption Inertial Measurement Unit

Accurate and real-time tracking of the orientation or attitude of rigid bodies has traditional applications in robotics, aerospace, underwater vehicles, human body motion capture, etc. Towards human body motion capture, especially wearable devices, the use of a longer time has always been a challenge for several weeks or several months continuously, so a low-cost chip and a low computational cost algorithm are necessary .The paper presented a quaternion-based algorithm that integrated the sensor output with the Kalman filtering algorithm, and a low power consumption Inertial Measurement Unit (IMU) for the attitude estimation. The low power consumption IMU with an inner Digital Motion Processor(DMP) from InvenSense Inc. called MPU9150, which contains triaxial accelerometers, triaxial gyroscopes, triaxial magnetometers and inner DMP. Firstly, we got attitude quaternion from DMP, and used the factored quaternion algorithm (FQA) to calculate course angle quaternion component. Then the Kalman Filtering algorithm was used to mix them together to acquire the accurate and good real-time performance attitude .The experimental results showed that Kalman filtering algorithm to mix DMP output and magnetometers data have better performance than gradient descent algorithm and complementary filter algorithm even in static performance and dynamic performance and power consumption.


1.INTRODUCTION
Accurate real-time tracking of the orientation or attitude of rigid bodies has traditional applications in robotics, aerospace, underwater vehicles, human body motion capture, etc .For human body motion capture applications, the human body can be viewed as an articulated rigid body consisting of 15 links [1].If the orientation relative to a fixed reference frame can be determined by each link, then the overall posture of the human can accurately be rendered in real time .The attitude of a static or slow-moving individual limb segment can be measured through the attachment of an IMU module .Such IMU modules typically contain a triad of orthogonally mounted accelerometers, a triad of orthogonally mounted gyroscope and a triad of orthogonally mounted magnetometers .The gyroscope is used to measure the angular rate and its real-time performance is very good, but its performance is highly affected by the temperature.The accelerometer is used to measure the gravity vector that is relative to the coordinate frame of the IMU module, and the magnetometers offer a similar function for the local magnetic field vector .Combining accelerometers data with magnetometers data , we can get the accurate attitude, but this method is not good enough in real-time performance [2].So we can integrate three of them to get accurate attitude and have a good real-time performance.In this paper, we used a low power consumption MEMS chip which contains triaxial accelerometers, triaxial gyroscopes, triaxial magnetometers and inner DMP .We first get attitude quaternion from DMP , and use the factored quaternion algorithm (FQA) to calculate course angle quaternion component [3][4], then using Kalman Filtering algorithm to mix them together to acquire the accurate and good real-time performance attitude.

FUSING DMP OUTPUT AND MAGNETIC DATA WITH KALMAN FILTER
The chip we choose is called MPU9150 which contains an inner DMP.The inner DMP uses a high performance algorithm to fuse accelerometers measure data and gyroscope measure data and output attitude quaternion which has a good real-time performance [5], but the course angle of this attitude will drift with time because this algorithm didn't count magnetometers data in.Thus we used Kalman filtering algorithm to mix DMP output and magnetometers data, so we can get the attitude data which have a good real-time and a high precision, and we can use low power consumption work mode to prolong the life time of the system.
In the first step, we extracted three components from the DMP output quaternion.The way to do this is to figure out azimuth angle θ, roll angle φ and elevation angle ψ at first, and then figure out the corresponding quaternion.Let us suppose the DMP output quaternion in this moment as follow [6] = So, three angles are The corresponding quaternion is In the second step, we used this azimuth quaternion and last azimuth quaternion which we reextracted from DMP output to work out the increment quaternion r = * Then, add this increment quaternion to the azimuth quaternion that we estimated last time to get the first azimuth quaternion which we will use to do Kalman filtering [7].
q represents the optimal estimation of azimuth quaternion last time.
In the third step, we used the FQA algorithm [8]which was developed by Xiaoping Yun in 2008 to work out the second azimuth quaternion which we will use to do Kalman filtering.as shown in the following steps.
At first, we use the current roll quaternion and elevation quaternion to rotate the measurements vector of magnetic fields m = [ m m m ] to the earth-based coordinate system [9] = * * * * In this expression, m stands for the pure vector quaternion of the 3-D vector ,i.e.m = [0 m m m ] .
In the absence of measurement error, m should agree with the known local normalized magnetic field vector n = [n n n ] , except for the effects of azimuthrotation on the sensor magnetometer readings.In such a case, n = m , and is the azimuth angle we want to figure out [10].Before proceeding further, we should notice that the only difference is the direction of these two 2-D vectors, but considering that the presence of magnetic interference and measurement noise they may also differ in length.To compensate for this effect, the vectors on both sides of ( 7) can be normalized.
With these definitions, (7) become and can be solved as The azimuth angle ψ is restricted to the range(-π < ≤ ), according to the half-angle formula, we could compute the half-angle sine and cosine values for ψ = The azimuth quaternion is then given by Now, we get two azimuth quaternion q and q which will be used in Kalman filter, in Kalman filter application, we treat q as the predictions, and q as the measurements.
In the first step of Kalman filtering -we predict current azimuth quaternion based on the optimal estimation last time.As (5), q is the optimal estimation of last time, r is the predicted increment, q is the predictions of this moment [11].In the second step of Kalman filtering -update the corresponding auto-covariance.For simplifying the computation, we use algebraic sum in this paper P is the auto-covariance of last state and Q is the a uto-covariance of system procedures.
In the third step of Kalman filtering -combine predictions with measurements to get the optimal estimation azimuth quaternion In this formula, slerp represents the interpolation calculation of quaternion, q is the starting point and q is the ending point of interpolation calculation, Kg is interpolation coefficient (gain of Kalman filter).The way to compute Kg is as following In the fourth step of Kalman filtering -So far we have obtained the optimal estimation of the state, but for the next step, we should update the auto-covariance of current state The most critical value in the process of designing Kalman filter are Q and R , which determine the velocity of error correction process and attitude smooth degree.
Since we have got the optimal azimuth quaternion, we should combine the three quaternion again to get a complete attitude quaternion, methods are as follows So the complete Algorithm Flow is as follows

3.EXPERIMENTAL RESULTS
To compare the arithmetic performance, we present two studies to compare with gradient descent algorithm [12][13] and complementary filter algorithm in static and dynamic performance.

A. Static performance experiment
In this experiment, we place the sensor module static for 20 seconds at first, and then the sensor samples the inertia data continuously for 12 seconds, the sampling frequency is 50 Hz.The method for data processing is to calculate the mean value of output value in each axis Then calculation squared variance is We evaluate the performance by comparing the squared covariance.The result of this experiment is as follows:

2) Complementary filter algorithm
To simplify calculations , we take the same correct coefficient α and β as 0.02, the same as gradient descent algorithm, when β < 0.02 the correct effect can't refrain integral floating of gyroscope [14].

Fig. 3. Static performance of complementary filter algorithm
3) Kalman filtering algorithm to mix DMP output and magnetometers data The core of this algorithm is to determine the auto covarianceQ of system and auto covariance R of measurements in Kalman filtering .We do not care the pitch angle and roll angle because these two angles are from DMP directly.We list only two cases in graphics, and others will be archived as tables.

B. Dynamic performance experiment
In this experiment, we make sure that it is done under the static condition and in order to test the dynamic performance of each algorithm, the sampling rate is 50Hz.then the pitch angle increases from 0° to 90° and then decreases from 90° to 0°.
From above section, we choice the β = 0.02 times gradient as the step size of gradient descent algorithm, 0.02 as the correct coefficient of complementary filter algorithm and Q = 0.00001, R = 0.1 as the coefficients of Kalman filtering algorithm to observe the dynamic performance, results are as follows [15][16]