Pose estimation using linearized rotations and quaternion algebra

Abstract

In this paper we revisit the topic of how to formulate error terms for estimation problems that involve rotational state variables. We present a first-principles linearization approach that yields multiplicative error terms for unit-length quaternion representations of rotations, as well as for canonical rotation matrices. Quaternion algebra is employed throughout our derivations. We show the utility of our approach through two examples: (i) linearizing a sun sensor measurement error term, and (ii) weighted-least-squares point-cloud alignment.

Introduction

The classic results of multivariate state estimation are rooted in both probability theory and vector calculus. This presents challenges for many practical estimation problems involving rotational state variables, which are not members of a vector space. Rather, the set of rotations constitutes a non-commutative group, called SO(3). Regardless of the choice of representation (e.g., rotation matrix, unit-length quaternion, Euler angles), a rotation has exactly three degrees of freedom. All rotational representations involving exactly three parameters have singularities [27] and all representations having more than three parameters have constraints. The question of how best to parametrize and handle rotations in state estimation is by no means new. There are many rotational parameterizations available, each with its unique advantages and disadvantages [22]. In spacecraft attitude and robotics estimation, the 4×1 unit-length quaternion (a.k.a., Euler–Rodrigues symmetric parameters), the standard 3×3 rotation matrix, and Euler angles are all common [4].

Unit-length quaternions are appealing in that they are free of singularities and compact in their representation; however, the unit-length constraint must be considered carefully, particularly for estimation problems. The question of whether to make updates to unit-length quaternions ‘additively’ or ‘multiplicatively’ has been debated often [17], [16]. For online attitude estimation, two variants of the Kalman filter [11] have been introduced: The Additive Extended Kalman Filter (AEKF) [1] and the Multiplicative Extended Kalman Filter (MEKF) [12], [15]. Both of these require the estimated quaternion to be re-normalized after the filter update step to restore the unit-length constraint. However, it was only recently shown that re-normalization within the Kalman filter framework is in fact a direct consequence of performing optimal estimation under the unit-length constraint [32]. More generally, Shuster [23], [24] provides a detailed examination of performing both ‘constrained’ (e.g., sensitive to but not enforcing the unit-length quaternion constraint during the update step) and ‘unconstrained’ (e.g., not sensitive to the unit-length quaternion constraint), demonstrating that constraint-sensitive estimation is preferable.

This paper builds on the work of Shuster [23], [24], by deriving constraint-sensitive linearizations of both unit-length quaternions and rotation matrices from a simple first-principles linearization perspective. Under this approach, the multiplicative perturbations arise quite naturally. We show how these perturbations may be used in the calculation of an arbitrary ‘measurement sensitivity matrix’ (i.e., partial derivative of the measurement function with respect to the rotation) [23] and more generally in the linearization of any expression involving a unit-length quaternion or rotation matrix. In our handling of unit-length quaternions, we exploit ‘quaternion algebra’ [10] quite heavily, which permits parallels to be drawn between the unit-length quaternion and rotation matrix results. To demonstrate the utility of our approach, we provide an example of linearizing a sun sensor measurement error term as well as an extended example of a batch weighted-least-squares pose (i.e., position and orientation) estimation problem. Pose estimation is an important problem in such aerospace applications as spacecraft rendezvous and docking, as well as motion estimation of planetary rovers.

The paper is organized as follows. We first introduce the notation and operations we will use for handling quaternions. We then derive our general expressions for linearizing expressions involving both unit-length quaternions and rotation matrices. Finally, we demonstrate these through the sun sensor and weighted-least-squares examples.