^{1}

^{2}

^{3}

^{3}

^{1}

^{2}

^{3}

Orientation estimation from magnetic, angular rate, and gravity (MARG) sensor array is a key problem in mechatronic-related applications. This paper proposes a new method in which a quaternion-based Kalman filter scheme is designed. The quaternion kinematic equation is employed as the process model. With our previous contributions, we establish the measurement model of attitude quaternion from accelerometer and magnetometer, which is later proved to be the fastest (computationally) one among representative attitude determination algorithms of such sensor combination. Variance analysis is later given enabling the optimal updating of the proposed filter. The algorithm is implemented on real-world hardware where experiments are carried out to reveal the advantages of the proposed method with respect to conventional ones. The proposed approach is also validated on an unmanned aerial vehicle during a real flight. Results show that the proposed one is faster than any other Kalman-based ones and even faster than some complementary ones while the attitude estimation accuracy is maintained.

In many robotic applications, the system needs to obtain the orientation parameters of some vital mechanical parts so that it can precisely control the actuators [

Attitude can often be acquired from integral of angular rates from gyroscope triads or least-square problem formed by vector observations [

Early concerns about the MARG-sensor fusion are almost raised around 1990s when some researchers started to develop fusion algorithms based on complementary filters. Foxlin first used MARG sensors to monitor the attitude motion of human’s head [

The above ones focus on complementary filter design of attitude estimator. Complementary filters are flexible for application on common platforms but still have their drawbacks. The main concern is that the complementary gain is always empirically given for one time and would result in terrible estimates for another quite different performance. The second one is that some complementary gains degenerate to constants for the sake of convenience in engineering practice which significantly limits the estimation accuracy and convergence speed. To overcome such problem, Kalman filter is employed. The Kalman filter is an optimal estimator for real-time inputs in the sense of minimal mean-squared error (MMSE) [

Therefore, we can see that it is necessary to find one method that is accurate statistically but fast even compared with complementary filters. In fact, in our previous research, the attitude determination from accelerometer is computed via linear quaternion transformations. In this paper, this motivates us to develop a novel quaternion measurement algorithm from accelerometer and magnetometer jointly. The algorithm serves as the observation model of the Kalman filter where the observation variance is systematically derived. Using the quaternion kinematic equation as the process model, the Kalman filter is established. Experiments are conducted to show the performance of the proposed filter with respect to representative methods. The results show that the filter is better for time consumption and maintains high attitude accuracy.

This paper is structured as follows: Section

The attitude determination can be modeled with the following system:

The magnetometer equation can be rewritten as

Combining the previous result of quaternion from accelerometer, that is,

Initialization accuracy:

(

(

(

(

(a)

(b)

(c)

(

In our design, the quaternion kinematic equation is employed as the process model. The gyroscope’s bias is not concerned in the current form. The gyroscope’s bias and biases of the accelerometer and magnetometer for observation model are cancelled before the filter update. Using the attitude determination algorithm proposed in the last section as the observation model, we model the system by

State prediction:

Covariance prediction:

Kalman gain:

State correction:

Covariance update:

In previous descriptions, we have noted that the Kalman filter can not be applied to systems with correlation noises. As given in (

In this section, we conduct several experiments using the MicroStrain 3DM-GX3-25 attitude and heading reference system (AHRS). The hardware is shown in Figure

The MicroStrain 3DM-GX3-25 AHRS for experimental validation.

Designed upper monitor for visual display of attitude estimation.

In this experiment, we compare the attitude estimates from the proposed filter and AHRS so that the absolute accuracy of our method can be verified. The variances of gyroscope, accelerometer, and magnetometer are computed via statistical data when the AHRS is still on the table. The parameters are

Raw sensor data of angular rate, acceleration, and magnetic field.

Estimated Euler angles and reference angles.

We can see that the proposed filter outputs accurate attitude estimates which coincide with the reference angles very well. The MSEs of roll, pitch, and yaw angles with respect to the reference are 0.0134°, 0.0286°, and 0.335°, respectively, which show that the proposed algorithm is accurate.

To verify the observation model and its variance, the measurement quaternion and its

Measurement quaternion and its standard deviation bounds.

Experiment

Using the hardware presented in Figure

Estimated Euler angles from the proposed filter, LMA-CO, and WCF.

We can see that the differences between LMA-CO and the proposed filter are quite small while that between WCF and the proposed filter seems to be relatively larger. Throughout computations of MSEs with respect to reference angles, the results are shown in Table

MSEs of attitude angles.

Algorithm | Roll | Pitch | Yaw |
---|---|---|---|

LMA-CO | 0.1514° | 0.1052° | 0.7143° |

WCF | 1.2833° | 1.0069° | 0.5688° |

Proposed method | 0.0893° | 0.0726° | 0.1346° |

This shows that the proposed filter is more accurate than the two representative methods. In engineering practice, the time consumption of one certain algorithm is another critical character. The comparison of time consumption of these algorithms is tested and plotted in Figure

Tested time consumption of different algorithms.

Compared with LMA-CO and WCF, we may find out that the proposed filter owns the least time consumption. This is because LMA-CO uses more complicated LMA and WCF adopts SVD for attitude estimation. The proposed filter, however, has only one computationally-complex process, that is, the inversion operation. This shows that the proposed filter is faster than these representative complementary filters.

In this subsection, we also evaluate the results from a recent representative Kalman filter, that is, the Algebraic QUAternion Kalman Filter (AQUA-KF) by Valenti et al. [

Estimated Euler angles from the proposed filter and AQUA-KF.

We can see that the AQUA-KF seems to have some minor differences with the proposed filter. For AQUA-KF, the MSEs of roll, pitch, and yaw angles with respect to reference angles are 2.24°, 1.56°, and 3.67°. The details of the proposed filter on this data sample have been shown in Table

Moreover, we also evaluate the time consumption performance of the two filters. The tested time consumption results are shown in Figure

Time consumption results of the proposed filter and AQUA-KF.

In this experiment, the proposed approach is validated on an unmanned aerial vehicle. The designed platform is composed of an MEMS inertial sensor array, a vision-inertial AHRS, a micro controller, USB debugger, and a transmitter (see Figure

Mounted test platform with inertial sensor array, vision-inertial AHRS, micro controller, transmitter, and so forth.

The vision-inertial AHRS is made by YunHeng Tech. Inc., Chengdu, China, giving high-resolution and precision attitude outputs according to local vision and inertial measurements. The XBee S3B Pro transmitter is used for wireless data transmission. The raw sensor outputs and referenced information are processed with the micro controller, an STM32F407VG board with 168 MHz clock speed, and abundant interfaces for data interaction. The sensor biases are cancelled before the flight was done according to the method by Zhang [

Raw inertial sensor data of the flight test.

Estimated and reference Euler angles during the flight test.

Besides, from Figure

The variance of estimated quaternions and their 3

Orientation estimation from MARG sensors is revisited in this paper. Using our previous research contributions, we establish one observation model for determining attitude quaternions from accelerometer and magnetometer with linear quaternion transformations. Adopting the quaternion kinematic equation as the process model, we form a novel Kalman filter with minimum state dimension. The variance analysis of the observation model is systematically derived. Since the observation procedure employs merely several simple matrix multiplications, the computational speed is quite faster. Experiments on real-world hardware are carried out showing the correctness of the measurement quaternion and observation variance. Throughout comparisons of the attitude estimation results between the proposed filter and commercial AHRS, other representative filters, we not only prove that the proposed filter is accurate, but also show that the proposed filter is faster than these representative ones including complementary filters and other Kalman filters. The source code has been uploaded to

Letting

The source code of the proposed algorithm is uploaded on

The authors declare that there are no conflicts of interest regarding the publication and related funding of this paper.

This research is supported by the National High Technology Research and Development Program of China (863 Program) no. 2015AA015408. The authors also thank Professor Hassen Fourati for providing them with the source code of LMA-CO.