# Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Problem Statement

## 3. Unit Quaternion Operations

#### 3.1. Euclidean Tangent Space and Rotation Vector Parametrization

#### 3.2. Sum, Subtraction, and Weighted Mean Operations

#### 3.3. Quaternion Unscented Transform

## 4. Mathematic Modeling

#### 4.1. Kinematic Model of Attitude

#### 4.2. Observation Model

## 5. State Estimators

#### 5.1. Quaternion-Based UKF

#### 5.2. Adaptive Covariance Matrix

#### 5.3. Outlier Rejection

#### 5.4. Quaternion-Based Robust Adaptive Unscented Kalman Filter

## 6. Experimental Results and Discussion

#### 6.1. Magnetic Field Distortion

#### 6.2. Linear Acceleration Disturbance

#### 6.3. Rotations about the Origin

## 7. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Jang, J.S.; Liccardo, D. Small UAV automation using MEMS. IEEE Aerosp. Electron. Syst. Mag.
**2007**, 22, 30–34. [Google Scholar] [CrossRef] - Groves, P.D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems; Artech House: Norwood, MA, USA, 2013. [Google Scholar]
- Yadav, N.; Bleakley, C. Accurate orientation estimation using AHRS under conditions of magnetic distortion. Sensors
**2014**, 14, 20008–20024. [Google Scholar] [CrossRef] - Vartiainen, P.; Bragge, T.; Arokoski, J.P.; Karjalainen, P.A. Nonlinear State-Space Modeling of Human Motion Using 2-D Marker Observations. IEEE Trans. Biomed. Eng.
**2014**, 61, 2167–2178. [Google Scholar] [CrossRef] - Ding, Y.; Gao, T.; Wu, Y.; Zhu, S. The research of a new data glove based on MARG sensor and magnetic localization technology. In Proceedings of the AIP Conference Proceedings, Xi’an, China, 20–21 January 2018; Volume 1955, p. 040098. [Google Scholar]
- Michel, T.; Genevès, P.; Fourati, H.; Layaïda, N. On attitude estimation with smartphones. In Proceedings of the IEEE International Conference on Pervasive Computing and Communications, Kona, HI, USA, 13–17 March 2017; pp. 267–275. [Google Scholar] [CrossRef]
- Gui, H.; de Ruiter, A.H. Quaternion Invariant Extended Kalman Filtering for Spacecraft Attitude Estimation. J. Guid. Control Dyn.
**2017**, 41, 863–878. [Google Scholar] [CrossRef] - Pereira, G.A.S.; Iscold, P.; Torres, L.A.B. Airplane attitude estimation using computer vision: Simple method and actual experiments. Electron. Lett.
**2008**, 44, 1303–1304. [Google Scholar] [CrossRef] - Costanzi, R.; Fanelli, F.; Monni, N.; Ridolfi, A.; Allotta, B. An Attitude Estimation Algorithm for Mobile Robots Under Unknown Magnetic Disturbances. IEEE/ASME Trans. Mechatron.
**2016**, 21, 1900–1911. [Google Scholar] [CrossRef] - Chiella, A.C.B.; Teixeira, B.O.S.; Pereira, G.A.S. Robust attitude estimation using an adaptive unscented Kalman filter (forthcoming). In Proceedings of the 2019 IEEE International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; pp. 7748–7754. [Google Scholar]
- Sipos, B.J. Application of the manifold-constrained unscented Kalman filter. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 30–43. [Google Scholar] [CrossRef]
- Yuan, X.; Yu, S.; Zhang, S.; Wang, G.; Liu, S. Quaternion-based unscented Kalman filter for accurate indoor heading estimation using wearable multi-sensor system. Sensors
**2015**, 15, 10872–10890. [Google Scholar] [CrossRef] - Valenti, R.G.; Dryanovski, I.; Xiao, J. A linear Kalman filter for MARG orientation estimation using the algebraic quaternion algorithm. IEEE Trans. Instrum. Meas.
**2016**, 65, 467–481. [Google Scholar] [CrossRef] - Lefferts, E.J.; Markley, F.L.; Shuster, M.D. Kalman filtering for spacecraft attitude estimation. J. Guid. Control Dyn.
**1982**, 5, 417–429. [Google Scholar] [CrossRef] - Crassidis, J.L.; Markley, F.L. Unscented filtering for spacecraft attitude estimation. J. Guidance Control Dyn.
**2003**, 26, 536–542. [Google Scholar] [CrossRef] - Hertzberg, C.; Wagner, R.; Frese, U.; Schröder, L. Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds. Inf. Fusion
**2013**, 14, 57–77. [Google Scholar] [CrossRef][Green Version] - Menegaz, H.T.; Ishihara, J.Y. Unscented and square-root unscented Kalman filters for quaternionic systems. Int. J. Robust Nonlinear Control
**2018**, 1–28. [Google Scholar] [CrossRef] - Mehra, R. Approaches to adaptive filtering. IEEE Trans. Autom. Control
**1972**, 17, 693–698. [Google Scholar] [CrossRef] - Li, X.R.; Bar-Shalom, Y. A recursive multiple model approach to noise identification. IEEE Trans. Aerosp. Electron. Syst.
**1994**, 30, 671–684. [Google Scholar] [CrossRef] - Hide, C.; Moore, T.; Smith, M. Adaptive Kalman filtering algorithms for integrating GPS and low cost INS. In Proceedings of the Position Location and Navigation Symposium, Monterey, CA, USA, 26–29 April 2004; pp. 227–233. [Google Scholar]
- Moghaddamjoo, A.; Kirlin, R.L. Robust adaptive Kalman filtering with unknown inputs. IEEE Trans. Acoust. Speech Signal Process.
**1989**, 37, 1166–1175. [Google Scholar] [CrossRef] - Pearson, R.K.; Neuvo, Y.; Astola, J.; Gabbouj, M. Generalized Hampel Filters. EURASIP J. Adv. Signal Process.
**2016**, 2016, 87. [Google Scholar] [CrossRef] - Soken, H.E.; Hajiyev, C. Pico satellite attitude estimation via robust unscented Kalman filter in the presence of measurement faults. ISA Trans.
**2010**, 49, 249–256. [Google Scholar] [CrossRef] - Lee, D.; Vukovich, G.; Lee, R. Robust Adaptive Unscented Kalman Filter for Spacecraft Attitude Estimation Using Quaternion Measurements. J. Aerosp. Eng.
**2017**, 30, 04017009. [Google Scholar] [CrossRef] - Ghobadi, M.; Singla, P.; Esfahani, E.T. Robust Attitude Estimation from Uncertain Observations of Inertial Sensors Using Covariance Inflated Multiplicative Extended Kalman Filter. IEEE Trans. Instrum. Meas.
**2018**, 67, 209–217. [Google Scholar] [CrossRef] - Tong, X.; Li, Z.; Han, G.; Liu, N.; Su, Y.; Ning, J.; Yang, F. Adaptive EKF Based on HMM Recognizer for Attitude Estimation Using MEMS MARG Sensors. IEEE Sens. J.
**2018**, 18, 3299–3310. [Google Scholar] [CrossRef] - Euston, M.; Coote, P.; Mahony, R.; Kim, J.; Hamel, T. A complementary filter for attitude estimation of a fixed-wing UAV. In Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 340–345. [Google Scholar] [CrossRef]
- Madgwick, S.O.H.; Harrison, A.J.L.; Vaidyanathan, R. Estimation of IMU and MARG orientation using a gradient descent algorithm. In Proceedings of the IEEE International Conference on Rehabilitation Robotics, Zurich, Switzerland, 27 June–1 July 2011; pp. 1–7. [Google Scholar] [CrossRef]
- 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. [Google Scholar] [CrossRef] - Wang, L.; Fu, L.; Hu, X. A nonlinear complementary filter approach for MAV 3D-attitude estimation with low-cost MARG/ADS. In Proceedings of the 2016 IEEE/ION Position, Location and Navigation Symposium (PLANS), Savannah, GA, USA, 11–14 April 2016; pp. 42–47. [Google Scholar] [CrossRef]
- Menegaz, H.M.T.; Ishihara, J.Y.; Kussaba, H.T.M. Unscented Kalman Filters for Riemannian State-Space Systems. IEEE Trans. Autom. Control
**2019**, 64, 1487–1502. [Google Scholar] [CrossRef] - Madgwick, S.O. An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays; Technical Report; University of Bristol: Bristol, UK, 2010. [Google Scholar]
- Kim, S.; Haschke, R.; Ritter, H. Gaussian mixture model for 3-Dof orientations. Robot. Auton. Syst.
**2017**, 87, 28–37. [Google Scholar] [CrossRef] - Markley, F.L.; Cheng, Y.; Crassidis, J.L.; Oshman, Y. Averaging quaternions. J. Guid. Control Dyn.
**2007**, 30, 1193–1197. [Google Scholar] [CrossRef] - Markley, F.L.; Crassidis, J.L. Fundamentals of Spacecraft Attitude Determination and Control; Springer: New York, NY, USA, 2014; Volume 33. [Google Scholar]
- Julier, S.J.; Uhlmann, J.K. Unscented filtering and nonlinear estimation. Proc. IEEE
**2004**, 92, 401–422. [Google Scholar] [CrossRef] - Xiong, K.; Zhang, H.; Chan, C. Performance evaluation of UKF-based nonlinear filtering. Automatica
**2006**, 42, 261–270. [Google Scholar] [CrossRef] - Davies, L.; Gather, U. The Identification of Multiple Outliers. J. Am. Stat. Assoc.
**1993**, 88, 782–792. [Google Scholar] [CrossRef] - Pearson, R.K. Outliers in process modeling and identification. IEEE Trans. Control Syst. Technol.
**2002**, 10, 55–63. [Google Scholar] [CrossRef]

**Figure 1.**The architecture of the quaternion-based robust adaptive Kalman filter. In the left, the MARG sensor provides the measurement information. The filtering algorithm uses the gyros measurement ${\omega}_{\mathrm{m}}\in {\mathbb{R}}^{3}$ in the forecast step. The UT block is used to propagate the accelerometer ${a}_{\mathrm{m}}\in {\mathbb{R}}^{3}$ and magnetometer ${b}_{\mathrm{m}}\in {\mathbb{R}}^{3}$ measurements through a nonlinear function, computing a unit quaternion, used as a pseudo-measurement in the robust noise estimation and data-assimilation steps.

**Figure 2.**Experimental setup using the MicroStrain 3DM-GX1${}^{\circledR}$ IMU and the Comau Smart Six${}^{\circledR}$ robot.

**Figure 3.**Results for abrupt magnetic disturbances experiment, scenario (i). In the left column, linear acceleration ${a}_{\mathrm{m}}$ and magnetic field ${b}_{\mathrm{m}}$ measurements, in the right column, the attitude error.

**Figure 4.**Results for slow magnetic disturbances experiment (second experiment), scenario (ii). In the left column, linear acceleration ${a}_{\mathrm{m}}$ and magnetic field ${b}_{\mathrm{m}}$ measurements, in the right column, the attitude error.

**Figure 5.**Bias estimate of angular rate ${\omega}_{\mathrm{y}}$ and ${\omega}_{\mathrm{z}}$, respectively, measured by the gyros for second experiment, scenario (ii).

**Figure 6.**Results for slow magnetic disturbances experiment (third experiment), scenario (ii). In the left column, linear acceleration ${a}_{\mathrm{m}}$ and magnetic field ${b}_{\mathrm{m}}$ measurements, in the right column, the attitude error.

**Figure 7.**Angular rate ${\omega}_{\mathrm{y}}$ and ${\omega}_{\mathrm{z}}$, respectively, measured by the gyros for the third experiment, scenario (ii).

**Figure 8.**Results for linear acceleration disturbance experiment, scenario (iii). (

**a**) shows the measured linear accelerations ${a}_{\mathrm{m}}$; (

**b**–

**d**) show the estimation error for $\varphi $, $\theta $ and $\psi $ angles, respectively.

**Figure 9.**Results for individual axis rotation about the origin, scenario (iv). (

**a**) shows actual orientation for individual axis movements; (

**b**–

**d**) show the estimation error for $\varphi $, $\theta $ and $\psi $ angles, respectively.

**Figure 10.**Results for simultaneous axes rotation about the origin, scenario (v). (

**a**) shows actual orientation for simultaneous axes movements; (

**b**–

**d**) show the estimation error for $\varphi $, $\theta $ and $\psi $ angles, respectively.

**Table 1.**Root Mean Square Error (RMSE) in degrees for disturbance scenarios (i) and (ii). The lowest RMSE results are highlighted in bold.

Abrupt Magnetic | Slow Magnetic 1 | Slow Magnetic 2 | |||||||
---|---|---|---|---|---|---|---|---|---|

Algorithm | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ |

QRAUKF | 0.05 | 0.04 | 0.07 | 0.07 | 0.09 | 1.84 | 0.40 | 0.19 | 3.11 |

QUKF | 0.05 | 0.05 | 0.20 | 0.98 | 0.51 | 13.0 | 1.66 | 0.27 | 12.52 |

CF | 0.07 | 0.06 | 0.14 | 9.22 | 11.00 | 28.90 | 3.20 | 0.86 | 8.97 |

3DM-GX1 | 0.16 | 0.18 | 0.09 | 0.12 | 0.09 | 11.28 | 0.35 | 0.27 | 9.8 |

**Table 2.**Root Mean Square Error (RMSE) in degrees for disturbance scenarios (iii), (iv), and (v). The lowest RMSE results are highlighted in bold.

Linear Acceleration | Individual Rotations | Simultaneous Rotations | |||||||
---|---|---|---|---|---|---|---|---|---|

Algorithm | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ | $\tilde{\varphi}$ | $\tilde{\theta}$ | $\tilde{\psi}$ |

QRAUKF | 0.28 | 0.87 | 0.16 | 1.08 | 1.31 | 0.91 | 2.88 | 1.94 | 1.37 |

QUKF | 4.0 | 3.78 | 2.36 | 1.97 | 1.73 | 2.23 | 2.76 | 2.08 | 4.20 |

CF | 1.87 | 1.60 | 0.53 | 1.17 | 1.61 | 1.17 | 2.97 | 2.54 | 2.03 |

3DM-GX1 | 0.37 | 0.99 | 0.39 | 1.08 | 1.34 | 1.37 | 2.88 | 2.17 | 2.06 |

© 2019 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 (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Chiella, A.C.B.; Teixeira, B.O.S.; Pereira, G.A.S. Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. *Sensors* **2019**, *19*, 2372.
https://doi.org/10.3390/s19102372

**AMA Style**

Chiella ACB, Teixeira BOS, Pereira GAS. Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. *Sensors*. 2019; 19(10):2372.
https://doi.org/10.3390/s19102372

**Chicago/Turabian Style**

Chiella, Antônio C. B., Bruno O. S. Teixeira, and Guilherme A. S. Pereira. 2019. "Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter" *Sensors* 19, no. 10: 2372.
https://doi.org/10.3390/s19102372