Indirect Kalman Filter for 3D Attitude Estimation

1 Indirect Kalman Filter for 3D Attitude Estimation Nikolas Trawny and Stergios I. Roumeliotis Department of Computer Science & Engineering University of Minnesota Multiple Autonomous Robotic Systems Laboratory Technical Report Number 2005-002. March 2005. Dept. of Computer Science & Engineering University of Minnesota 4-192 EE/CS Building 200 Union St. Minneapolis, MN 55455. Tel: (612) 625-2217. Fax: (612) 625-0572. URL: trawny Indirect Kalman Filter for 3D Attitude Estimation Nikolas Trawny and Stergios I. Roumeliotis Department of Computer Science & Engineering University of Minnesota Multiple Autonomous Robotic Systems Laboratory, TR-2005-002. March 2005. 1 Elements of Quaternion Algebra Quaternion Definitions The quaternion is generally defined as q = q4 + q1 i + q2 j + q3 k (1).

2 Where i, j, and k are hyperimaginary numbers satisfying i2 = 1 , j2 = 1 , k2 = 1 , ij = ji = k , jk = kj = i , ki = ik = j (2). Note that this does not correspond to the Hamilton notation. It rather is a convention resulting in multiplications of quaternions in natural order (see also section and [5, p. 473]). This is in accordance with the JPL Proposed Standard Conventions [2]. The quantity q4 is the real or scalar part of the quaternion, and q1 i + q2 j + q3 k is the imaginary or vector part. The quaternion can therefore also be written in a four-dimensional column matrix q , given by . q T. q = = q1 q2 q3 q4 (3). q4. If the quantities q and q4 fulfill . kx sin( /2). q = ky sin( /2) = k sin( /2), q4 = cos( /2) (4). kz sin( /2).

3 The elements q1 , .. , q4 are called quaternion of rotation or Euler symmetric parameters [5]. In this notation, k . describes the unit vector along the axis and the angle of rotation. The quaternion of rotation is a unit quaternion, satisfying q p |q | = q T q = |q|2 + q42 = 1 (5). Henceforth, we will use the term quaternion to refer to a quaternion of rotation. The quaternion q and the quaternion q describe a rotation to the same final coordinate system position, i. e. the angle axis representation is not unique [5, p. 463]. The only difference is the direction of rotation to get to the target configuration, with the quaternion with positive scalar element q4 describing the shortest rotation [2]. 2. Quaternion Multiplication The quaternion multiplication is defined as q p = (q4 + q1 i + q2 j + q3 k) (p4 + p1 i + p2 j + p3 k).

4 = q4 p4 q1 p1 q2 p2 q3 p3 + (q4 p1 + q1 p4 q2 p3 + q3 p2 ) i + (q4 p2 + q2 p4 q3 p1 + q1 p3 ) j + (q4 p3 + q3 p4 q1 p2 + q2 p1 ) k . q4 p1 + q3 p2 q2 p3 + q1 p4. q3 p1 + q4 p2 + q1 p3 + q2 p4 . = . q2 p1 q1 p2 + q4 p3 + q3 p4 . q1 p1 q2 p2 q3 p3 + q4 p4. where we have used the relations defined in Eq. (2). The quaternion multiplication can alternatively be written in matrix form. For this, we first introduce the matrix- notation for the cross-product using the skew-symmetric matrix operator bq c, defined as . 0 q3 q2. bq c = q3 0 q1 (6). q2 q1 0. The cross product can then be written as . i j k q2 p3 q3 p2 0 q3 q2 p1.. q p = q1 q2 q3 = q3 p1 q1 p3 = q3 0 q1 p2 = bq cp (7). p1 p2 p3 q1 p2 q2 p1 q2 q1 0 p3. The quaternion multiplication can now be rewritten in matrix form as q p = L(q )p.

5 Q4 q3 q2 q1 p1. q3 q4 q q . 2 p2.. q p = 1 (8). q2 q1 q4 q3 p3 . q1 q2 q3 q4 p4.. q4 I3 3 bq c q p q p = (9). qT q4 p4.. q4 p + p 4 q q p =. q4 p4 qT p . p4 q + pq4 + bp cq =. p4 q4 pT q . p4 I3 3 + bp c p q q p =. pT p4 q4.. p4 p3 p2 p1 q1. p3 p4 p1 p2 . q p = q2 (10). p2 p1 p4 p 3 q3 . p1 p2 p3 p4 q4. q p = R(p )q . Properties of L and R. L(q 1 ) = LT (q ) (11). 1 T. R(p ) = R (p ) (12). (13). TR-2005-002 3.. L = (q ) q (14).. R = (p ) p (15). (16). with matrices and defined as . q4 I3 3 bq c = (17). qT.. p4 I3 3 + bp c = (18). pT. If two rotations, CA and CB are related, such that CA CX = CX CB (hand-eye calibration), then the related matrix L(q A ) R(q B ) is skew-symmetric, and of rank 2. In particular, q A4 = q B4 and ||qA || = ||qB ||.

6 Quaternions also have an neutral element with respect to multiplication, which is defined as T. q 0 = 0 0 0 1 (19). q q 0 = q 0 q = q (20). The inverse rotation is described by the inverse or complex conjugate quaternion, denoted as . q k sin( /2) k sin( /2). q 1 = = = (21). q4 cos( /2) cos( /2). q q 1 = q 1 q = q 0 (22). (q p ) 1 = p 1 q 1 (23). Useful Identities Properties of the cross product skew-symmetric matrix b c Anti-Commutativity b c = b cT (24). ba cb = bb ca (25). T T. a bb c = b ba c (26). Distributivity over Addition ba c + bb c = ba + b c (27). Scalar Multiplication c b c = bc c (28). Cross Product of Parallel Vectors T. (c ) = c b c = c T b c = 03 1 (29). TR-2005-002 4. Lagrange's Formula . ba cbb c = baT aT b I3 3 (30).

7 T T. a (b c) = b(a c) c(a b) (31). ba cbb c + abT = bb cba c + baT (32). b(a b) c = baT abT (= (a b) c) (33). Jacobi Identity ba cbb cc + bb cbc ca + bc cba cb = 0 (34). Rotations bCa c = Cba cCT (35). C(a b) = (Ca) (Cb) (36). Cross product of vectors in quaternion notation If we define the quaternions . a b c a b a = , b = , c = = (37). 0 0 0 0. we can show that 1 . c = b a + a b 1 (38). 2. 1 . = (L(b ) + RT (b))a (39). 2 . 1 bb c bb c b b a = (40). 2 bT + bT 0 0. TR-2005-002 5. Powers of b c b c2 = T | |2 I3 3 (41).. b c3 = T | |2 I3 3 b c = T b c | |2 b c T. = ( b c ) | |2 b c T. = ( ) | |2 b c = | |2 b c (42). b c4 = b c3 b c = | |2 b c2 (43). b c5 = b c3 b c2.. = | |2 b c T | |2 I3 3. = +| |4 b c (44). b c6 = b c5 b c = +| |4 b c2 (45).

8 B c7 = b c5 b c2. = | |6 b c (46). and so on. TR-2005-002 6. Properties of the matrix . The matrix appears in the product of a vector and a quaternion, and is used for example in the quaternion derivative. It has the following properties: . 0 z y x z 0 x y . ( ) = y x (47). 0 z . x y z 0.. b c . = (48). T 0.. 2 b c2 T b c . ( ) =. T b c T .. | |2 I3 3 03 1. =. 01 3 | |2. = | |2 I4 4 (49). ( )3 = | |2 ( ) (50). ( )4 = | |4 I4 4 (51). ( )5 = | |4 ( ) (52). ( )6 = | |6 I4 4 (53). and so on. Properties of the matrix . The matrix (q ) appears in the multiplication of a vector with a quaternion. The relationship between (q ) and (a). is equivalent to that between the multiplication matrices L(q ) and R(p ) (cf. section ). It is defined as.

9 Q4 q3 q2 . q4 q3 q2 q1. q3 q4 q1 q I + bq c (q ) = q2 q1 = 4 3x3 T , T (q ) = q3 q4 q1 q2 (54). q4 q q2 q1 q4 q3. q1 q2 q3. and it can be shown that T (q ) (q ) = I3 3 (55). T T. (q ) (q ) = I4 4 q q (56). T (q )q = 03 1 (57). The relationship between and is given by [3, eq. (60)]. (a)q = (q )a (58). Relationship between Quaternion and Rotational Matrix Given a vector p we define the corresponding quaternion as . p p = (59). 0. TR-2005-002 7. We will be using the following two relations between vectors expressed in different coordinate frames L. p=L G. G C(q ) p (60). where q = L L. G q and G C(q ) is the (3 3) rotational matrix that expresses the (global) frame {G} with respect to the (local) frame {L}. A vector can also be transformed from one coordinate frame to another by pre- and postmultiplying its quaternion by the rotation quaternion and its inverse, respectively.

10 L L. p = G p L. G q G q . 1. (61).. q4 I3 3 bq c q p = GL q . 1. qT q4 0.. q4 p q p q = . qT p q4. 2 T.. q4 p q4 q p + qq p + q4 p q (q p) q =. +q4 qT p q4 qT p qT (q p). 2 . q4 p 2q4 q p + qqT p (1 q42 I3 3 ) qqT p =. qT bq cp 2 G . 2q4 1 I3 3 2q4 bq c + 2qqT p =. 0 0. This gives us the relationship between a quaternion and its corresponding rotational matrix. L. 2 T. G C(q ) = 2q4 1 I3 3 2q4 bq c + 2qq (62). which can also be written as L. G C(q ) = T (q ) (q ) (63). using the definitions of and from Section A similar form can be derived for the triple product of quaternions, although without an obvious physical interpre- tation. q p q 1 (64). T. =L(q )R (q )p (65).. C(q ) 0 p = (66). 0 1 p4.. C(q )p = (67). p4. In case of only a very small rotation q , we can use the small angle approximation to simplify the above expression.