An Extended Kalman Filter for Quaternion-Based Attitude Estimation

Abstract: This paper presents an extended Kalman filter for real-time estimation of rigid body orientation using the newly developed MARG (Magnetic, Angular Rate, and Gravity) sensors. Each MARG sensor contains a three-axis magnetometer, a three-axis angular rate sensor, and a three-axis accelerometer. The filter represents rotations using quaternions rather than Euler angles, which eliminates the long-standing problem of singularities associated with attitude estimation. A process model for rigid body angular motions and angular rate measurements is defined. The process model converts angular rates into quaternion rates, which are integrated to obtain quaternions. The Gauss-Newton iteration algorithm is utilized to find the best quaternion that relates the measured acceleration