# Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Previous Work

## 3. Background Theory

**p**and

**q**, is defined as

^{A}v, expressed with respect to the A frame, can be expressed with respect to the B frame by the following operation:

^{A}v

_{q}and

^{B}v

_{q}are the observations of the vector v, in the two reference frames, written as pure quaternions as shown in Equation (6).

^{B}v relative to the frame A can be easily found by using the property of the conjugate quaternion and it is presented in Equation (7).

## 4. Quaternion from Earth-Field Observations

#### 4.1. Quaternion from Accelerometer Readings

#### 4.2. Quaternion from Magnetometer Readings

**l**vector from having negative x- component using the following procedure. If ${\text{L}}_{x}<0$ we rotate

**l**of 180° around the world z- frame, applying the quaternion ${\mathit{q}}_{\pi}={\left[\begin{array}{cccc}0& 0& 0& 1\end{array}\right]}^{\mathsf{T}}$ . Finally the rotated vector is used to find ${\mathit{q}}_{m\text{A}{\text{G}}^{+}}$, which has the same form of Equation (31) and aligns l with the magnetic north. The sequences of rotations are summarized in the quaternion multiplications below:

**Figure 1.**Simulated experiment showing the orientation output in quaternion form (

**left**) and in Euler representation (

**right**). (

**a**) Orientation reference; (

**b**) Orientation output of the presented method with noisy acceleration data and noise-free magnetometer data; (

**c**) Orientation output of the presented method with noise-free acceleration data and magnetic readings affected by magnetic disturbances.

## 5. Quaternion-Based Complementary Filter

**Figure 3.**Block diagram of the complementary filter for MARG implementation (with magnetometer data).

#### 5.1. Prediction

#### 5.2. Correction

#### 5.2.1. Accelerometer-Based Correction

**Figure 4.**2-D representation of two different interpolation methods between quaternion

**q**

_{a}and

**q**

_{b}resulting in quaternion

**q**

_{int}. On the left is represented the linear interpolation (LERP), whereas on the right the spherical linear interpolation (SLERP) showing the correct result.

#### 5.2.2. Magnetometer-Based Correction

#### 5.3. Adaptive Gain

**Figure 5.**The gain factor function used in the adaptive gain to reduce the attitude error deriving from the external linear acceleration because of vehicle motion.

#### 5.4. Filter Initialization and Bias Estimation

## 6. Experiments

#### 6.1. MAV Flight Experiment

**Figure 6.**2D top view (

**left**) and 3D side view (

**right**) of the trajectory traveled by the quadrotor during the “1LoopDown” experiment.

Dataset | Proposed | Madgwick | AscTec | EKF |
---|---|---|---|---|

1LoopDown | 0.0233 | 0.0370 | 0.0464 | 0.0287 |

2LoopsDown | 0.0292 | 0.0470 | 0.0338 | 0.0314 |

3LoopsDown | 0.0277 | 0.0405 | 0.0315 | 0.0331 |

Dataset | Proposed | Madgwick | AscTec | EKF |
---|---|---|---|---|

1LoopDown | 0.0209 | 0.0336 | 0.0369 | 0.0284 |

2LoopsDown | 0.0223 | 0.0369 | 0.0313 | 0.0384 |

3LoopsDown | 0.0202 | 0.0360 | 0.0329 | 0.0392 |

Dataset | Proposed | Madgwick | AscTec | EKF |
---|---|---|---|---|

1LoopDown | 0.1429 | 0.2543 | 0.3388 | 0.1888 |

2LoopsDown | 0.1309 | 0.9229 | 0.3182 | 0.3345 |

3LoopsDown | 0.2890 | 1.3327 | 0.3255 | 0.3545 |

**Figure 7.**Comparison of the orientation Euler angles output of the three methods during the “1LoopDown” experiment. (

**a**) Roll; (

**b**) Pitch; (

**c**) Yaw.

#### 6.2. Magnetic Disturbances

#### 6.3. High Nongravitational Acceleration

**Figure 8.**Comparison of the different estimation methods in the presence of magnetic disturbances. (

**a**) Norm of the measured magnetic field vector; (

**b**) Roll; (

**c**) Pitch; (

**d**) Yaw.

**Figure 9.**Pitch estimation under the condition of high nongravitational acceleration; (

**a**) Non gravitational acceleration applied during the experiment; (

**b**) Pitch angle output of the different algorithms

Algorithm | Average Time ($\mu s$) | Standard Deviation ($\mu s$) |
---|---|---|

Proposed | 1.4243 | 0.4761 |

Madgwick | 1.2839 | 0.7101 |

EKF | 7.0408 | 0.2342 |

## 7. Conclusions

- Fast initialization in quaternion form allowing any starting configuration.
- Fast convergence of the orientation quaternion because of the algebraic formulation of the delta quaternions.
- Two different gains to separately filter acceleration and magnetic field noises.
- Magnetic distortion compensation that involves a two-fold advantage: it avoids the impact of the magnetic disturbances on the roll and pitch components of the orientation when the sensor is surrounded by unwanted magnetic flux and eliminates the need of a predefined magnetic field direction.

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- Wahba, G. A Least Square Estimate of Spacecraft Attitude. Soc. Ind. Appl. Math. (SIAM) Rev.
**1965**, 7, 409–1965. [Google Scholar] - Wertz, J.R. Spacecraft Attitude Determination and Control; Wertz, J.R., Ed.; Kluwer Academic Publishers: Dordrecht, The Netherlands, 1978; pp. 427–428. [Google Scholar]
- Shuster, M.; Oh, S. Three-axis attitude determination from vector observations. J. Guid. Control
**1981**, 4, 70–77. [Google Scholar] [CrossRef] - Keat, J. Analysis of Least-Squares Attitude Determination Routine DOAOP; Report CSC/TM-77/6034; Computer Sciences Corporation: Greenbelt, MD, USA, February 1977. [Google Scholar]
- Markley, F.L. Attitude Determination Using Vector Observations and the Singular Value Decomposition. J. Astronaut. Sci.
**1988**, 36, 245–258. [Google Scholar] - Bar-Itzhack, I.Y. Polar Decomposition for Attitude Determination from Vector Observations. In Proceedings of the AIAA Guidance, Navigation, and Control, Hilton Head Island, SC, USA, 10–12 August 1992.
- Mortari, D. Euler-2 and Euler-n Algorithms for Attitude Determination from Vector Observations. In Proceedings of the IFAC Space Technology International Conference on Intelligent Autonomous Control in Aerospace, Beijing, China, 14–16 August 1995.
- Markley, F.L. Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm. J. Astronaut. Sci.
**1993**, 41, 261–280. [Google Scholar] - Mortari, D. Energy Approach Algorithm for Attitude Determination from Vector Observations. Adv. Austronaut. Sci.
**1995**, 89, 773–784. [Google Scholar] - Markley, F.L.; Mortari, D. Quaternion Attitude Estimation Using Vector Observations. J. Austronaut. Sci.
**2000**, 48, 359–380. [Google Scholar] - Yun, X.; Bachmann, E.R.; Mcghee, R.B. A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements. IEEE Trans. Instrum. Meas.
**2008**, 57, 638–650. [Google Scholar] - Crassidis, J.L.; Markley, F.L.; Cheng, Y. Survey of nonlinear attitude estimation methods. J. Guid. Control Dyn.
**2007**, 30, 12–28. [Google Scholar] [CrossRef] - Gebre-Egziabher, D.; Hayward, R.C.; Powell, J.D. Design of Multi-sensor Attitude Determination Systems. IEEE Trans. Aerosp. Electron. Syst.
**2004**, 40, 627–649. [Google Scholar] [CrossRef] - Choukroun, D.; Bar-Itzhack, I.Y.; Oshman, Y. Novel Quaternion Kalman Filter. IEEE Trans. Aerosp. Electron. Syst.
**2006**, 42, 174–190. [Google Scholar] [CrossRef] - Tao, W.; Zheng, R.; Feng, H. Gait Analysis Using Wearable Sensors. Sensors
**2012**, 12, 2255–2283. [Google Scholar] [CrossRef] [PubMed] - Sabatini, A.M. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing. IEEE Trans. Biomed. Eng.
**2006**, 53, 1346–1356. [Google Scholar] [CrossRef] [PubMed] - Hung, T.N.; Suh, Y.S. Inertial Sensor-Based Two Feet Motion Tracking for Gait Analysis. Sensors
**2013**, 13, 5614–5629. [Google Scholar] [CrossRef] [PubMed] - Barshan, B.; Durrant-Whyte, H.F. Inertial navigation systems for mobile robots. IEEE Trans. Robot. Autom.
**1995**, 11, 328–342. [Google Scholar] [CrossRef] [Green Version] - Jun, M.; Roumeliotis, S.I.; Sukhatme, G.S. State estimation of an autonomous helicopter using Kalman filtering. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Kyongju, Korea, 17–21 October 1999; Volume 3, pp. 1346–1353.
- Baerveldt, A.J.; Klang, R. A low-cost and low-weight attitude estimation system for an autonomous helicopter. In Proceedings of the IEEE International Conference on Intelligent Engineering Systems, Budapest, Hungary, 15–17 September 1997; pp. 391–395.
- Euston, M.; Coote, P.; Mahony, R.; Hamel, T. A complementary filter for attitude estimation of a fixed-wing UAV. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 340–345.
- Mahony, R.; Hamel, T.; Pflimlin, J. Nonlinear Complementary Filters on the Special Orthogonal Group. IEEE Trans. Autom. Control
**2008**, 53, 1203–1218. [Google Scholar] [CrossRef] [Green Version] - Marins, J.L.; Yun, X.; Bachmann, E.R.; McGhee, R.B.; Zyda, M.J. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, HI, USA, 29 October–3 November 2001; pp. 2003–2011.
- Sabatini, A.M. Kalman-Filter-Based Orientation Determination Using Inertial/Magnetic Sensors: Observability Analysis and Performance Evaluation. Sensors
**2011**, 11, 9182–9206. [Google Scholar] [CrossRef] [PubMed] - Bachmann, E.R.; McGhee, R.B.; Yun, X.; Zyda, M.J. Inertial and magnetic posture tracking for inserting humans into networked virtual environments. In Proceedings of the ACM Symposium on Virtual Reality Software and Technology VRST, Baniff, AB, Canada, 15–17 November 2001; pp. 9–16.
- Madgwick, S.O.H.; Harrison, A.J.L.; Vaidyanathan, A. Estimation of IMU and MARG orientation using a gradient descent algorithm. In Proceedings of the IEEE International Conference on Rehabilitation Robotics, Zurich, Switzerland, 29 June–1 July 2011; pp. 1–7.
- Alam, F.; ZhaiHe, Z.; Jia, H. A Comparative Analysis of Orientation Estimation Filters using MEMS based IMU. In Proceedings of the International Conference on Research in Science, Engineering and Technology, Dubai, UAE, 21–22 March 2014.
- Tian, Y.; Wei, H.; Tan, J. An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments. IEEE Trans. Neural Syst. Rehabil. Eng.
**2013**, 21, 254–264. [Google Scholar] [CrossRef] [PubMed] - Fourati, H.; Manamanni, N.; Afilal, L.; Handrich, Y. Complementary observer for body segments motion capturing by inertial and magnetic sensors. IEEE/ASME Trans. Mechatronics
**2014**, 19, 149–157. [Google Scholar] [CrossRef] - Yoo, T.S.; Hong, S.K.; Yoon, H.M.; Park, S. Gain-Scheduled Complementary Filter Design for a MEMS Based Attitude and Heading Reference System. Sensors
**2011**, 11, 3816–3830. [Google Scholar] [CrossRef] [PubMed] - Li, W.; Wang, J. Effective adaptive kalman filter for mems-imu/magnetometers integrated attitude and heading reference systems. J. Navig.
**2013**, 66, 99–113. [Google Scholar] [CrossRef] - Makni, A.; Fourati, H.; Kibangou, A.Y. Adaptive kalman filter for MEMS-IMU based attitude estimation under external acceleration and parsimonious use of gyroscopes. In Proceedings of the European Control Conference (ECC), Strasbourg, France, 24–27 June 2014; pp. 1379–1384.
- Keun Lee, J.; Park, E.J.; Robinovitch, S.N. Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions. IEEE Trans. Instrum. Meas.
**2012**, 61, 2262–2273. [Google Scholar] - Breckenridge, W.G. Quaternions—Proposed Standard Conventions; NASA Jet Propulsion Laboratory, Technical Report Interoffice Memorandum IOM 343-79-1199; 1999. [Google Scholar]
- De Franceschi, M.; Zardi, D. Evaluation of Cut-Off Frequency and Correction of Filter-Induced Phase Lag and Attenuation in Eddy Covariance Analysis of Turbulence Data. Bound.-Layer Meteorol.
**2003**, 108, 289–303. [Google Scholar] [CrossRef] - Shoemaker, K. Animating Rotation with Quaternion Curves. In Proceedings of the Special Interest Group on Graphics and Interactive Techniques (SIGGRAPH), 22–26 July 1985; pp. 245–254.
- Lee, G.H.; Achtelik, M.; Fraundorfer, F.; Pollefeys, M.; Siegwart, R. A Benchmarking Tool for MAV Visual Pose Estimation. In Proceedings of the Conference on Control, Automation, Robotics and Vision (ICARCV), Singapore, 7–10 December 2010; pp. 1541–1546.

© 2015 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Valenti, R.G.; Dryanovski, I.; Xiao, J.
Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. *Sensors* **2015**, *15*, 19302-19330.
https://doi.org/10.3390/s150819302

**AMA Style**

Valenti RG, Dryanovski I, Xiao J.
Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. *Sensors*. 2015; 15(8):19302-19330.
https://doi.org/10.3390/s150819302

**Chicago/Turabian Style**

Valenti, Roberto G., Ivan Dryanovski, and Jizhong Xiao.
2015. "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" *Sensors* 15, no. 8: 19302-19330.
https://doi.org/10.3390/s150819302