Sequential Pose Estimation Using Linearized Rotation Matrices

We present a new formulation for pose estimation using an extended Kalman filter that takes advantage of the Lie group structure of rotations. Using the exponential map along with linearized rotations for updates and errors permits a graceful filter formulation that avoids the awkward representation of Euler angles and the required norm constraint for quaternions. We demonstrate this approach with an implementation that uses sensors commonly found in consumer tablets and mobile phones: a camera and gyroscope, which we use to estimate attitude, position, and gyroscope bias. We use gyroscope measurements for prediction, and vision-based measurements for correction. We show results and discuss the performance of our pose estimation method using ground truth data obtained via a motion capture system.