Unscented Kalman Filtering for Simultaneous Estimation of Attitude and Gyroscope Bias

Abstract

We present an unscented Kalman filtering algorithm for simultaneously estimating attitude and gyroscope bias from an inertial measurement unit (IMU). The algorithm is formulated as a discrete-time stochastic nonlinear filter, with state space given by the direct product matrix Lie group <inline-formula><tex-math notation= LaTeX >$\\text{SO(3)} \\times \\mathbb {R}^3$</tex-math></inline-formula>, and observations in <inline-formula><tex-math notation= LaTeX >$\\text{SO(3)}$</tex-math></inline-formula> reconstructed from IMU measurements of gravity and the earth s magnetic field. Computationally efficient implementations of our filter are made possible by formulating the state space dynamics and measurement equations in a way that leads to closed-form equations for covariance propagation and update. The resulting attitude estimates are invariant with respect to choice of fixed and moving reference frames. The performance advantages of our filter vis-à-vis existing state-of-the-art IMU attitude estimation algorithms are validated via numerical and hardware experiments involving both synthetic and real data.