Indirect Kalman Filter for 3D Attitude Estimation A Tutorial for Quaternion Algebra
Nikolas Trawny and Stergios I. Roumeliotis Department of Computer Science & Engineering University of Minnesota
Multiple Multiple Autonomous Autonomous Robotic Systems Laboratory Technical Report Number 2005-002, Rev. 57 March 2005
Dept. of Computer Science & Engineering University of Minnesota 4-192 EE/CS Building 200 Union St. S.E. Minneapolis, MN 55455 Tel: (612) 625-2217 Fax: (612) 625-0572 625-0572 URL: http://www.cs.umn.edu/˜trawny
Indirect Kalman Filter for 3D Attitude Estimation A Tutorial for Quaternion Algebra Nikolas Trawny and Stergios I. Roumeliotis Department of Computer Science & Engineering University of Minnesota Multiple Autonomous Robotic Systems Laboratory, TR-2005-002, Rev. 57
March 2005
1
Elemen Elements ts of Quater Quaternio nion n Algebr Algebra a
1.1
Quaternio Quaternion n Definition Definitionss
The quaternion is generally defined as
q¯ = q4 + q1 i + q2 j + q3 k
(1)
where i, j, and k are hyperimaginary numbers satisfying
i2 =
2
2
−1 , j = −1 , k = −1 , − jk = kj = i , − ki = ik = j
−ij = ji = k ,
(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 1.4 and [1, [1, p. 473]). This is in accordance accordance with the JPL Proposed Standard Conventions [2 [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¯ = If the quantities q and q4 fulfill
q
= q1
q4
q2
q3
kx sin(θ/ sin(θ/2) 2) ˆ sin(θ/ sin(θ/2) 2) = k q = ky sin(θ/ sin(θ/2) 2),, kz sin(θ/ sin(θ/2) 2)
q4
T
q4 = cos(θ/ cos(θ/2) 2)
(3)
(4)
ˆ the elements q1 , . . . , q4 are called “quaternion of rotation” or “Euler symmetric parameters” [ 1]. 1]. In this notati notation, on, k describes the unit vector along the axis and θ the angle of rotation rotation.. The quaternion quaternion of rotation rotation is a unit quaterni quaternion, on, satisfying
|q¯| =
| |
q 2 + q42 = 1
q¯T q¯ =
(5)
Henceforth, we will use the term “quaternion” to refer to a quaternion of rotation. The quaternion q¯ and the quaternion q¯ describe describe a rotation rotation to the same final coordinat coordinatee system position, position, i. e. the angle–axis representation is not unique [ 1, 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]..
−
2
1.2
Quaternio Quaternion n Multipli Multiplicati cation on
The quaternion multiplication is defined as
q¯ p¯ = (q4 + q1 i + q2 j + q3 k) ( p4 + p1 i + p2 j + p3 k) = q4 p4 q1 p1 q2 p2 q3 p3 + (q (q4 p1 + q1 p4 q2 p3 + q3 p2 ) i + (q4 p2 + q2 p4 q3 p1 + q1 p3 ) j + (q (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 matrixnotation for the cross-product using the skew-symmetric matrix operator q , defined as
×
q × = The cross product can then be written as
q
×p =
i
j
k
q1 p1
q2 p2
q3 p3
q2 p3 = q3 p1 q1 p2
− − −
− q3 p2 − q1 p3 − q2 p1
0 q3 q2
q3 0 q1
−
=
0 q3 q2
−q3
q2 q1 0
0 q1
The quaternion multiplication can now be rewritten in matrix form as
q¯ p¯ =
⊗
q¯ p¯ =
⊗
q¯ p¯ =
⊗
= =
q¯ p¯ =
⊗
q¯ p¯ =
⊗
q¯ p¯ =
⊗
(¯q) p¯ q4 q3 q2 q1
L − −
q3 q4 q1 q2
−q2
− − − q4 I3×3 − q × −qT
q
q4
p4
− ×
p4 q + pq4 + p p4 q4 pT q
−
− −
×q
p4 I3×3 + p
× T
p4 p3 p2 p1 ( p)¯ p¯)¯q
R
−p − p3 p4 p1 p2
−
p
p4
p2 p1 p4 p3
− −
p1 p2 p3 p4
p
q4 p + p4 q q p q4 p4 qT p
−
− q2 q1 0
q1 q2 q3 q4
q1 q4 q3
(6)
p1 p2 = q p3
×p
(7)
(8)
(9)
p1 p2 p3 p4
q
q4
q1 q2 q3 q4
(10)
Quaternions also have an neutral element with respect to multiplication, which is defined as
q¯I = 0 0 0 1 q¯ TR-2005-002, Rev. 57
⊗ q¯
I
= q¯I
T
⊗ q¯ = q¯
(11) (12) 3
The inverse rotation is described by the inverse or complex conjugate quaternion, denoted as −1
q¯
=
ˆ sin(θ/ k sin(θ/2) 2)
− − q
q4
=
q¯
cos(θ/ cos(θ/2) 2)
I
⊗
1.3.1
− cos(−θ/2) θ/2)
⊗ q¯−1 = q¯−1 ⊗ q¯ = q¯
(¯ q p) p¯)−1 = p¯−1
1.3
=
ˆ sin( θ/2) k θ/ 2)
⊗ q¯−1
(13) (14)
(15)
Useful Useful Identi Identitie tiess Properties of the quaternion multiplication multiplication matrices matrices
L= R=
L and R
Ψ(¯ q)
q¯
(16)
Ξ( p) p¯) p¯
(17) (18)
with matrices Ψ and Ξ defined as
Ψ= Ξ=
q4 I3×3
×
−Tq ×
−q
p4 I3×3 + p
−pT
ΨT Ψ = ΞT Ξ = I3×3
L(¯q−1) = LT(¯q) R(¯ p−1) = RT( p) p¯) LT(¯q)L(¯q) = L(¯q)LT(¯q) = I4×4 RT(¯q)R(¯q) = R(¯q)RT(¯q) = I4×4 L(¯q)R(¯r) = R(¯r)L(¯q) L(¯q)RT(¯r) = RT(¯r)L(¯q)
(19) (20) (21)
(22) (23) (24) (25) (26) (27)
Multiple Products
p¯ q¯ = (¯ p) p) = (¯ p) p) = (¯r) = (¯r)
⊗ ⊗ r¯ L L(¯q)¯r L R(¯r)¯q R L( p)¯ p¯)¯ q R R(¯q)¯ p
(28) (29) (30) (31)
Scalar Product
p¯T r¯ = p¯T
LT(¯q)L(¯q)¯r = ( q¯ ⊗ p) p¯)T (¯ q ⊗ r¯) T T T = p¯ R (¯q)R(¯ q )¯ r = ( p¯ ⊗ q¯) (¯r ⊗ q¯) TR-2005-002, Rev. 57
(32) (33)
4
( p¯
⊗ q¯)T r¯ = p¯T RT(¯q)¯r = p¯T(¯r ⊗ q¯−1)
(34)
p¯T (¯ q
⊗ r¯ ⊗ q¯−1) = p¯T RT (¯q)L(¯ q )¯ r =q¯T LT ( p) p¯)R(¯ r)¯q =q¯−T RT ( p) p¯)L(¯r)¯q−1
(35) (36) (37)
If two rotations, CA and CB are related, such that CA CX = CX CB (hand-eye calibration), then the related matrix (¯ qA ) (¯qB ) is skew-symmetric, and of rank 2. In particular, q¯A4 = q¯B4 and qA = qB .
L
1.3.2
−R
|| || || ||
Properties of the the cr cross oss product product skew-symmetr skew-symmetric ic matrix matrix
Anti-Commutativity
ω × = −ω ×T
(38)
a ×b = −b ×a ⇔ a b × = −bTa ×
(39)
a × + b × = a + b ×
(41)
T
(40)
Distributivity Distributivity over Addition
Scalar Multiplication Multiplication
c
· ω × = c ω ×
(42)
Cross Product of Parallel Vectors Vectors ω
× (c · ω) = c · ω ×ω = −c ·
Lagrange’s Formula
ω
T
ω × T = 03×1
(43)
a ×b × = baT − aT b I3×3 ⇔ a × (b × c) = b(aTc) − c(aT b)
(44)
a ×b × + abT = b ×a × + baT
(46)
(a × b) × = baT − abT(= ( a × b) × c)
(47)
a ×b ×c + b ×c ×a + c ×a ×b = 0
(48)
Ca × = Ca ×CT
(49)
(45)
Jacobi Identity
Rotations
C(a
TR-2005-002, Rev. 57
× b) = (Ca) × (Cb)
(50)
5
Cross product of vectors in quaternion notation
a ¯=
a
0
If we define the quaternions
b
¯b =
,
0
,
c¯ =
× c
0
=
a
b
0
(51)
we can show that
1 ¯ b a ¯ + ¯a ¯b−1 2 1 = (¯b) + T (¯b) a ¯ 2 1 b b b b = T T b + b 0 2 1 03×1 a 2 b = 01×3 0 0 2
c¯ =
= Powers of
⊗ ⊗ L R − ×− × − − −× − × × b
a
=
0
a
(52) (53)
a
(54)
0
(55)
b
(56)
0
ω × ω ×2
=
ωω
ω ×3
=
− |ω|2 · I3×3 ω × T 2 ωω ω ×−|ω| · ω × T 2 ω (−ω ×ω) − |ω | · ω × −ω (ω × ω)T − |ω|2 · ω × −|ω|2 · ω ×
= = = =
T
ωω
− |ω|2 · I3×3
T
(57)
ω ×4
= =
ω ×3 · ω × −|ω|2 · ω ×2
ω ×5
= =
ω ×3 · ω ×2 −|ω|2 · ω × ωω T − |ω|2 · I3×3 +|ω|4 · ω ×
=
ω ×6
= ω = +ω
×5 · ω × | |4 · ω ×2
ω ×7
= =
ω ×5 · ω ×2 −|ω|6 · ω ×
(58)
(59)
(60)
(61)
(62)
and so on.
TR-2005-002, Rev. 57
6
1.3.3 1.3.3
Properti Properties es of the matrix 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:
Ω(ω )
=
=
2
Ω(ω )
= = =
− −−
0 ωz ωy ωx
ωz 0 ωx ωy
− − ω × −ωT 2
−ω
ωx 0 ωz
ω
0
−
× − −| | · × ω
ω
T
ω
2
ωx ωy ωz 0
y
ωω
T
01×3
−|ω|2 · I4×4
(63)
(64)
−ω × ω T −ω ω
ω
I3×3
03×1
−|ω|2
(65)
Ω(ω )3
=
−|ω|2 · Ω(ω)
(66)
Ω(ω )4
=
|ω|4 · I4×4
(67)
Ω(ω )5
=
|ω|4 · Ω(ω)
(68)
Ω(ω )6
=
−|ω|6 · I4×4
(69)
and so on. 1.3.4 1.3.4
Properti Properties es of the matrix 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 (¯ 1.2). It is defined as q) and ( p) p¯) (cf. section 1.2).
L
Ξ(¯ q) =
− −
q4 q3 q2 q1
−q3 q4 q1 q2
−
and it can be shown that
− −
q2 q1 q I + q = 4 3x3 T q4 q q3
·
×
−
R
,
ΞT (¯ q) =
−
ΞT (¯ q)Ξ(¯q)
= I3×3 Ξ(¯ q )Ξ (¯q) = I4×4 ΞT (¯ q )¯ q = 03×1 T
q4 q3 q2
q3 q4 q1
−
−q2 −q1 q1 −q2 q4 −q3
(70)
(71) T
− q¯ q¯
(72) (73)
The relationship between Ξ and Ω is given by [3, [3, eq. (60)]
Ω(a)¯ q = Ξ(¯q)a
1.4
(74)
Relationsh Relationship ip betwee between n Quaterni Quaternion on and Rotation Rotational al Matrix Matrix
Given a vector p we define the corresponding quaternion as
p¯ = TR-2005-002, Rev. 57
p
0
(75)
7
We will be using the following two relations between vectors expressed in different coordinate frames L
p=
L q )G p G C(¯
(76)
L L q¯ and G C(¯ q ) is the (3 3) rotational matrix that expresses the (global) frame G with respect to the where q¯ = G (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.
×
{}
L
p¯ = = = = = =
L ¯ Gq
{ }
L −1 ¯ Gq
G
p¯
⊗ −⊗ × ⊗ −− × ⊗ − −− × × − × × − × − −− − × − − − − × × q4 I3×3
q
T
q
q4 p
q
p
q4
0
q p q p
G −1 ¯ Lq
q
T
q4
q42 p
q4 q p + qqT p + q4 p q (q p) +q4 qT p q4 qT p qT (q p)
q42 p
2q4 q
2q42
(77)
p + qqT p (1 q42 I3×3 ) qT q p
1 I3×3
+ 2qqT
2q4 q 0
q
qqT p
G
p
0
This gives us the relationship between a quaternion and its corresponding rotational matrix. L q) G C(¯
which can also be written as
= 2q42
−1
L q) G C(¯
I3×3
− 2q4q × + 2qqT
= ΞT (¯q)Ψ(¯ q)
(78)
(79)
using the definitions of Ξ and Ψ from Section 1.3.1. A similar form can be derived for the triple product of quaternions, although without an obvious physical interpretation.
⊗ ⊗ q¯−1 =L(¯q)RT (¯ q ) p¯ q¯ p¯
= =
(80) (81)
C(¯ q) 0
0
p
1
p4
C(¯ q )p
p4
(82) (83)
In case of only a very small rotation δ q¯, we can use the small angle approximation to simplify the above expression. We can write the quaternion describing a small rotation as
≈
δq¯ = =
δq δq 4
ˆ sin(δθ/ k sin(δθ/2) 2)
cos(δθ/ cos(δθ/2) 2)
1 2 δθ
1
(84) (85) (86)
leading to the following expression for the corresponding rotational matrix L ¯) G C(δ q
TR-2005-002, Rev. 57
≈ I3×3 − δθ ×
(87) 8
The result from eq. (78) (78) could have also been found by rewriting Euler’s formula which relates the rotational matrix and the angle–axis representation [ 2] L GC
ˆ × + (1 − cos(θ ˆk ˆT − sin(θ sin(θ )k cos(θ)) k ˆ × + 2 sin ˆk ˆT 2cos2 (θ/2) θ/2) − 1 · I3×3 − 2cos(θ/ 2cos(θ/2) 2) sin( sin(θ/2) θ/2)k sin2 (θ/2) θ/2)k
= cos(θ ) I3×3 =
·
(88) (89)
Substituting the appropriate quaternion components according to eq. ( 4) readily yields equation (78) ( 78).. The latte latterr can also be expressed in terms of quaternion components as L q) G C(¯
=
q12 q22 q32 + q42 2 (q1 q2 q3 q4 ) 2 (q1 q3 + q2 q4 )
− − −
−
2 (q1 q2 + q3 q4 ) q12 + q22 q32 + q42 2 (q2 q3 q1 q4 )
− −
−
−
−
1 2q22 2q32 2 (q1 q2 + q3 q4 ) = 2 (q1 q2 q3 q4 ) 1 2q12 2q32 2 (q1 q3 + q2 q4 ) 2 (q2 q3 q1 q4 )
−
− −
−
−
− − 2 (q1 q3 − q2 q4 ) 2 (q2 q3 + q1 q4 ) 2q32 + 2q 2q42 − 1 2 (q1 q3 − q2 q4 ) 2 (q2 q3 + q1 q4 ) 1 − 2q12 − 2q22
2q12 + 2q 2q42 1 2 (q1 q2 + q3 q4 ) 2q42 1 = 2 (q1 q2 q3 q4 ) 2q22 + 2q 2 (q1 q3 + q2 q4 ) 2 (q2 q3 q1 q4 )
−
2 (q1 q3 q2 q4 ) 2 (q2 q3 + q1 q4 ) q12 q22 + q32 + q42
− −
(90)
(91)
(92)
Another alternative form of eq. ( 78) 78) arises after exploiting the relationship between qqT and q L q) G C(¯
= I3×3
− 2q4q × + 2q ×2
×2 (cf. eq. (57 (57)) )) (93)
Note that, due to the convention chosen for the quaternion multiplication (cf. eq. ( 2)), the product of two rotational matrices will correspond to the product of two quaternions in the same order [ 2, 1]. Thus, L1 L1 ¯) L2 C(L2 q
Finally, we can interpret
L1 ¯ L2 q
=
·
ˆ sin(θ/ k sin(θ/2) 2)
cos(θ/ cos(θ/2) 2)
(94)
as the quaternion describing the rotation of coordinate frame
{L2}
L2 L2 ¯) G C(G q
L1 G C
=
L1 ¯ L2 q
L2 ¯ G q
⊗
ˆ expressed in L1 (cf. figure 1). to coordinate frame L1 , with the axis of rotation k 1). This can also be expressed as the matrix exponential
{ }
{ }
L q) G C(¯
= exp
− × ˆ k
θ
c11 c12 c13 c21 c22 c23 c31 c32 c33
The inverse problem is to determine q¯ as a function of C =
(95)
. The following solution is taken from [2]: [2]:
T = trace(C) = c 11 + c22 + c33 =
√ √
(q12
−
+
q22
+
q32
−
3q42 ) =
√
(96)
1 + 4q 4 q42
−
(97)
(c12 + c21 )/(4q (4q2 ) 1 + 2c 2 c11 T /2 (c12 + c21 )/(4q (4q1 ) 1 + 2c 2 c22 T /2 q= = (c13 + c31 )/(4q (4q1 ) (c23 + c32 )/(4q (4q2 ) (c23 c32 )/(4q (4q1 ) (c31 c13 )/(4q (4q2 )
−
−
(c13 + c31 )/(4q (4q3 ) (c23 + c32 )/(4q (4q3 ) = = 1 + 2c 2 c33 T /2 (c12 c21 )/(4q (4q3 )
−
−
−
(98)
− (c23 − c32 )/(4q (4q4 ) (c31 − c13 )/(4q (4q4 ) (c12√− c21 )/(4q (4q4 ) 1 + T /2
(99)
Each of these four solutions will be singular when the pivotal element is zero, but at least one will not be (since otherwise q could not have unit norm) norm )1 . For maximum numerical accuracy, the form with the largest pivotal element T ). should be used. The maximum of ( q1 , q2 , q3 , q4 ) corresponds to the most positive of (c11 , c22 , c33 , T )
| || || || |
1
√
Note further, that T = 1 + 4 cos only case where where q4 = 1 + T /2 is zero is for cos2 (θ/2) θ/2) = 1 + 2cos 2cos θ and hence T m in = 1. The only orthonormality of C, the diagonal elements are bounded 1 T = 1. Due to orthonormality cii 1. For q4 and all other pivotal elements to be zero, we would require cii = 1, but this contradicts T = 1. Hence, at least one pivotal element will be non-zero.
−
−
−
TR-2005-002, Rev. 57
−
− − ≤ ≤
9
fLg
fGg
L G
2 cos(©) C = 4¡ sin(©) 0
3 5
sin(©) 0 cos(©) 0 0 1
2 0 3 6 0 77 q q¹ = 6 4 sin(©= sin(©=2) 5 cos(©= cos(©=2)
+©
Figure 1: Relationship between quaternion and rotational matrix. Note that the chosen quaternion convention results in a “left-hand rule”-type rotational matrix.
1.5
Quaternio Quaternion n Time Time Derivati Derivative ve
When the local coordinate frame L is moving with respect to the global reference frame G , we can compute the rate of change or the derivative derivative of the corresponding quaternion describing their relationship. We do that by computing the limit of the difference quotient
{}
{ }
L(t) ¯˙(t) G q L(t+∆t)
The quaternion G
1 = lim lim ∆t→0 ∆t
L(t+∆t) q¯ G
−
L(t) ¯ G q
(100)
q¯ can be expressed as the product of two quaternions L(t+∆t) L(t+∆t) q¯ = L(t) q¯ G
where
L(t+∆t) q¯ = L(t) L(t+∆t)
Note, that the quaternion L(t)
⊗
L(t) ¯ G q
(101)
ˆ sin(θ/ k sin(θ/2) 2)
cos(θ/ cos(θ/2) 2)
(102)
q¯ describes the rotation of reference frame L(t) to reference frame L(t + ∆t)
{
}
{
}
ˆ (the latter being expressed in L(t + ∆t in terms of the rotation angle θ and the axis of rotation k ∆t) ). In the limit, as ∆t 0, the angle of rotation will become very small, so that we can approximate the sin and cos-functions by their first-order Taylor expansion
→
L(t+∆t) q¯ = L(t)
ˆ sin(θ/ k sin(θ/2) 2)
ˆ θ/2 k θ/2
cos(θ/ cos(θ/2) 2)
1
1 2
≈ · · =
δθ 1
{
}
(103)
The vector δ θ has the direction of the axis of rotation and the magnitude magnitude of the angle of the rotation. Dividing this vector by ∆t will yield, in the limit, the rotational velocity ω
TR-2005-002, Rev. 57
δθ ∆t→0 ∆t
= lim lim
(104)
10
We are now ready to derive the quaternion derivative as L(t) ¯˙(t) G q
1 ∆t→0 ∆t 1 = l im ∆t→0 ∆t 1 lim ∆t→0 ∆t 1 ω = 2 0
=
l im
≈
= = =
− ⊗ − · − ⊗ L(t+∆t) q¯ G
L(t) ¯ G q
L(t+∆t) q¯ L(t)
L(t) ¯ G q
1 2
δθ 1
⊗ − × −
Ω(ω ) =
and (cf. sections 1.3.3 and 1.3.4) 1.3.4)
− −
0 ωz ωy ωx
L(t)
Ξ(G q¯) =
1
⊗
L(t) ¯ G q
L(t) ¯ G q
1 ω ω T ω 0 2 1 L(t) Ω(ω ) G q¯ 2 1 L(t) Ξ( q¯)ω 2 G
with
0
q¯I
L(t) ¯ G q
(105) L(t) ¯ G q
(106) (107)
ωz 0 ωx ωy
−ω
ωx 0 ωz
− −
− −
q4 q3 q2 q1
ωx ωy ωz 0
y
−
−q3 q4 q1 q2
−
− − q2 q1 q4 q3
(108)
(109)
Note that ω = L(t) ω , i. e. the turn rate is expressed in the local, not the inertial coordinate coordinate frame [1, [1, p. 482].
1.6
Quaternio Quaternion n Integrati Integration on
Integrating a quaternion is equivalent to solving the first order differential equation (cf. eq. (106)) (106)) L ˙ ¯(t) Gq
=
1 L Ω(ω ) G q¯(t) 2
(110)
where we have dropped the time index from the leading superscript and instead denoted the time variability of the quaternion by writing q¯ = q¯(t) for greater notational clarity. It should be clear that the leading superscript L refers to the local frame L at time instant t. The solution to this differential equation has the general form [ 4, p. 40]
{}
L ¯(t) Gq
L = Θ(t, tk ) G q¯(tk )
(111)
The governing equation for Θ(t, tk ) is found by differentiation and substitution
˙ (t, tk ) L q¯(tk ) =Θ G
(112)
1 L ˙ (t, tk ) L q¯(tk ) Ω(ω) G q¯(t) = Θ G 2
(113)
L ˙ ¯(t) Gq
1 L ˙ (t, tk ) L q¯(tk ) Ω(ω) Θ(t, tk ) G q¯(tk ) = Θ G 2 ˙ (t, tk ) = 1 Ω(ω (t))Θ(t, tk ) Θ 2
⇔
(114) (115)
and has the initial condition
Θ(tk , tk ) = I4×4 TR-2005-002, Rev. 57
(116) 11
Under certain assumptions we can obtain closed form solutions for this equation. The simplest assumption is that ω is constant over the integration period ∆t = tk+1 tk , thus making the differential equation linear time invariant. This assumption leads to the zeroth order quaternion integrator. The next more accurate approximation is to assume a linear evolution of ω during ∆t. We will term the resulting formula the first order quaternion integrator.
−
1.6.1 1.6.1
Zeroth Zer oth Order Order Quaterni Quaternion on Integra Integrator tor
If ω(t) = ω is constant over the integration period ∆t = tk+1 Θ(tk+1 , tk ) can be expressed as [4, [ 4, eq. (2-58a)]
Θ(tk+1 , tk ) = Θ(∆t (∆t) = exp
− t , the matrix Ω does not depend on time, and k
1 Ω(ω)∆t )∆t 2
(117)
We can rewrite this matrix exponential using its Taylor series expansion
1 1 Θ(∆t (∆t) = I4×4 + Ω(ω)∆t )∆t + 2 2!
1 Ω(ω )∆t )∆t 2
2
1 + 3!
1 Ω(ω)∆t )∆t 2
3
+ ...
(118)
Using the properties of the matrix Ω described in section 1.3.3, this transforms into
Θ(∆t (∆t)
=
1 I4×4 + ∆t Ω(ω) 2
−
1 3!
+
1 5!
Reordering and expanding with
Θ(∆t (∆t)
= +
1 ∆t 2
3
1 ∆t 2
5
1 2!
−
2
||· ||· − | |· 1 ∆t 2
2
ω
I4×4
ω
1 Ω(ω) + 4!
1 ∆t 2
4
2
4
Ω(ω)
1 6!
1 ∆t 2
6
ω
||· ||·
ω
4
I4×4
ω
6
I4×4
− ...
(119)
|ω| yields
− | | | |− | | − | | | | − 1
1
|ω |
1 2! 1 2
1 ∆t 2
ω
2
2
ω
1 3!
∆t
+
1 4!
1 ∆t 2 3
1 2
∆t
ω
4
ω
1 + 5!
4
1 2
. . . I4×4 5
ω
∆t
. . . Ω(ω)
(120)
After close inspection, we recognize the Taylor series expansions of the sin and cos functions
Θ(∆t (∆t) = cos
| | · ω
∆t
2
I4×4 +
1
|ω|
sin
| | · ω
2
∆t
Ω(ω )
(121)
We can finally write the zero-th order quaternion integrator as L ¯(tk+1 ) Gq
=
L Θ(tk+1 , tk ) G q¯(tk )
=
| | · cos
ω
2
∆t
I4×4 +
1
|ω |
sin
| | · ω
2
∆t
Ω(ω )
L ¯(tk ) Gq
(122)
A close look reveals that Θ(tk+1 , tk ) is nothing but the multiplication multiplication matrix associated with a specific quaternion, so that we can rewrite the zero-th order quaternion integration as a quaternion product
· ⊗ ω
L ¯(tk+1 ) Gq
=
|ω |
|ω | 2 ∆t |ω | 2 ∆t
sin
cos
L ¯(tk ) Gq
This quaternion product corresponds to rotating the original frame around the rotation axis defined by angle ω ∆t, which corresponds exactly to the assumption of a constant ω .
(123)
ω
through the
| |
TR-2005-002, Rev. 57
12
| |
The above expression will cause numerical instability for very small ω, due to ω appearing in the denominator. We will therefore compute the limit of the above equation as ω goes towards zero, making multiple use of L’H opital’s oˆ pital’s rule.
| |
lim Θ(∆t (∆t) =
|ω|→0 |→0
li m
|ω|→0 |→0
| | | | | | | | ω
cos
2
1
= I4×4 + lim lim
|ω|
|ω|→0 |→0
= I4×4 + 1.6.2 1.6.2
∆t
I4×4 + ω
sin
2
1
ω
sin
ω
∆t Ω(ω)
2
∆t Ω(ω)
∆t Ω(ω ) 2
(124)
First First Order Order Quate Quaternio rnion n Integra Integrator tor
The first order quaternion integrator makes the assumption of a linear evolution of ω during the integration interval ∆t. In this case, we have to modify the matrix Θ(tk+1 , tk ) from eq. (117 (117). ). For that purpose, we introduce the average ¯ , defined as turn rate ω
¯ ω
=
ω(tk+1 ) + ω (tk )
(125)
2
˙ and the associated matrix Ω(ω˙ ), which, in the linear case, is We can also define the derivative of the turn rate ω constant ˙)= Ω Ω(ω
ω(tk+1 )
− ω (t ) k
∆t
(126)
Note that higher order derivatives of Ω(ω) are zero. Following Wertz [5, [5, p. 565], in order to compute the quaternion at time instant tk+1 , we write its Taylor series expansion around time instant tk L ¯(tk+1 ) Gq
L L ˙ =G q¯(tk ) +G q¯(tk )∆t )∆t +
1 2
L¨ ¯(tk )∆t )∆t2 Gq
+ ...
(127)
Repeatedly applying the definition of the quaternion time derivative (eq. ( 106)) 106)) yields L ¯(tk+1 ) Gq
=
1 1 I4×4 + Ω(ω (tk ))∆t ))∆t + 2 2!
1 L + ∆t2 Ω(ω˙ (tk ))G q¯(tk ) + 4
1 Ω(ω(tk ))∆t ))∆t 2
2
1 + 3!
1 Ω(ω(tk ))∆t ))∆t 2
3
+ ...
L ¯(tk ) Gq
1 1 L ˙ (tk ))Ω(ω(tk )) + Ω(ω(tk ))Ω(ω˙ (tk )) ∆t3 G Ω(ω q¯(tk ) + . . . (128) 12 24
¯ ) as If we write the average Ω(ω tk+1
1 ¯) = Ω(ω ∆t
Ω(ω(τ )) τ )) dτ = Ω(ω (tk )) +
tk
1 ˙ (tk ))∆t Ω(ω ))∆t 2
(129)
we can reorder the terms in eq. (128) (128) to form L ¯(tk+1 ) Gq
=
1 1 I4×4 + Ω(ω ¯ )∆t )∆t + 2 2! +
1 Ω(ω ¯ )∆t )∆t 2
1 Ω(ω ˙ (tk ))Ω(ω(tk )) 48
2
1 + 3!
∆t3
− Ω(ω(t ))Ω(ω˙ (t )) k
k
3
1 Ω(ω ¯ )∆t )∆t 2
+ ...
L ¯(tk ) Gq
(130)
˙ ) with Recognizing the first term as the Taylor series expansion of the matrix exponential, and after replacing Ω(ω its definition (eq. (126 ( 126)), )), we obtain the final formula L ¯(tk+1 ) Gq
=
exp
TR-2005-002, Rev. 57
1 1 ¯ )∆t Ω(ω Ω )∆t + 2 48
−
ω(tk+1 )
Ω
ω(tk )
Ω
ω(tk )
Ω
ω (tk+1 )
∆t2
L ¯(tk ) Gq
(131)
13
2
Attit Attitude ude Propa Propagat gation ion
The Kalman Filter Filter consists consists of two stages stages to determin determinee an estimate estimate of the current attitude. attitude. In the first stage, the propagation, the filter produces a prediction of the attitude based on the last estimate and some proprioceptive measurements. This estimate is then corrected in the update stage, where new absolute orientation measurements are taken into account. One way to predict the system state is to feed the control commands given to the system into a system model and thus to predict the behavior of the system. An alternative alternative way to estimate position and orientation is to use data from an inertial measurement measurement unit (IMU) as dynamic model replacement. The IMU provides measurements of the translational accelerations and rotational velocities acting on the system. In this paper, we will pursue the latter approach. Before discussing the state equation and the error propagation, we will describe the model for the gyros that will provide measurements of the rotational velocity.
2.1
Gyro Gyro Noise Noise Model Model
As part of the IMU, a three-axis gyro provides measurements of the rotational velocity. Gyros are known to be subject to different error terms, such as a rate noise error and a bias. In accordance with the literature [ 3, 6], we use a simple model that relates the measured turn rate ω m to the real angular velocity ω as ωm
= ω + b + nr
(132)
In this equation, b denotes the gyro bias and nr the rate noise, assumed to be Gaussian white noise with characteristics
E [ E [nr ] = 03×1 E nr (t + τ ) τ )nr T (t) = Nr δ(τ ) τ )
(133)
(134)
b˙ = nw
(135)
E [ E [nw ] = 03×1 E nw (t + τ ) τ )nw T (t) = Nw δ(τ ) τ )
(136)
The gyro bias is non-static and simulated as a random walk process
with characteristics characteristics
(137)
The bias is therefore a random quantity and needs to be estimated along with the quaternion. For simplification we will assume that the noise is equal in all three spatial directions, i. e.
Nr
= σr2
Nw
2
c
= σw
c
· I3×3 · I3×3
(138) (139)
The subscript c indicates, that these are the noise covariances for the continuous-time system, distinguishing them from the noise covariances in the discretized system used later. In order to determine the units of the covariances [ 6] we consider the scalar case for illustration purposes, which extends directly to the vector case. For compatibility, nr has the same units as ω, therefore
E [ E [nr (t + τ ) τ )nr (t)] = σr2 δ (τ ) τ ) =
But the unit of δ (τ ) [4, p. 221] τ ) is defined as [4,
[δ (τ )] τ )] =
c
1 sec
rad2 sec2
(140)
(141)
Therefore
σr2
c
= = =
TR-2005-002, Rev. 57
rad2 sec sec2 2 rad 1 1 sec sec
·
· · rad sec
2
1 Hz
(142) (143) (144)
14
and
· √ rad sec
[σr ] = c
Analogously, we obtain
2
σw
=
c
1 rad = sec Hz
(145)
sec
(146)
2
·
rad /sec sec
rad2 sec3 rad = sec
√
=
(147) 2
· ·√
and
rad sec
[σw ] = c
Hz
Hz =
(148)
√rad 3 sec
(149)
In the discrete case, nr d and nw d will have to have the same units as the turn rate (cf. also the state equation in section 2.2), 2.2), namely rad sec . In order to preserve equivalence of the noise strength, we have to incorporate the sampling frequency when discretizing, yielding
σr
√σ∆t √ σ · ∆t rc
=
d
σw
=
d
(150) (151)
wc
1 where ∆t is the inverse of the sampling frequency, or f sample discrete variance variancess will be used in the sample = ∆t . The discrete simulation of noisy real-world data. Further explanation for conversion between continuous and discrete noise variance is provided by Simon [ 7, pp. 230-233]. 230-233]. To make the analogy analogy to the derivat derivation ion in the book, the bias driving driving noise, nw , corresponds to the process noise, whereas the rate noise, nr , corresponds to the measurement noise. A detailed explanation of this gyro noise model and its relation to the various specifications provided by manufacturers can be found in the appendix of [ 8]. 8].
2.2
The State State Equati Equation on
In direct consequence of the previous analysis, we define a seven-element state vector consisting of the quaternion and the gyro-bias
x(t) =
q¯(t)
(152)
b(t)
Using the definition of the quaternion derivative (eq. ( 106)) 106)) and the error model (eqs. (132 ( 132)) and (135) (135)), ), we find the following system of differential equations governing the state L ˙ ¯(t) Gq
=
1 Ω(ω m 2
−b−n ) r
b˙ = nw
L ¯(t) Gq
(153) (154)
Taking the expectation of the above yields the prediction equations (cf. [3, [3, p. 422]) for the state within the EKFframework Lˆ ¯˙(t) Gq
=
1 Lˆ Ω(ω ˆ) G q¯(t) 2
˙
ˆ = 03×1 b with
ˆ ω
= ωm
(155) (156)
− bˆ
(157)
Since the bias is constant over the integration interval, we may integrate the quaternion using the zeroth order (cf. ˆ instead of ω. section 1.6.1) 1.6.1) or first order (cf. section 1.6.2) 1.6.2) integrator, using ω TR-2005-002, Rev. 57
15
2.3
Error Err or and Covarianc Covariancee Repr Represen esentati tation on
Usually, the error vector and its covariance is expressed in terms of the arithmetic difference between the state vector and its estimate. In the problem at hand, however, this representation is problematic, due to the presence of constraints in the system. system. The fact that the quaterni quaternion on is enforced enforced to be of unit length length (cf. eq. (5)) (5)) makes the corresponding covariance matrix singular [ 3, p. 423], which which is difficul difficultt to maintain maintain numerical numerically ly.. For stability stability reasons, reasons, we will therefore use a different, six-dimensional representation of the error vector. Instead of using the arithmetic difference between quaternion and quaternion estimate to define the error, we will employ the error quaternion δ qˆ; a small rotation between the estimated and the true orientation of the local frame of reference. Instead of a difference, this error is defined as a multiplication.
⊗ ˆ qˆ¯ ˆ q¯ ⊗ qˆ¯−1
L ¯= L ¯ ˆ δq Gq L L L ¯= G ˆ δq L
L G
(158)
L G
(159)
Since the rotation associated with the error quaternion δ q¯ can be assumed to be very small, we can employ the small angle approximation (as seen in section 1.4) 1.4) and define the attitude error angle vector δ θ as follows
≈ δq δq 4
δq¯ =
(160)
ˆ sin(δθ/ k sin(δθ/2) 2)
=
(161)
cos(δθ/ cos(δθ/2) 2)
1 2 δθ
(162)
1
This error angle vector δ θ is of dimension 3 The bias error is defined as
× 1 and will be used together with the bias error in the error state vector. ˆ ∆b = b − b (163)
We can now define the error vector as
˜= x
δθ ∆b
(164)
In the next section we will develop the continuous time first order state equation for the error vector.
2.4
Continuous Continuous Time Time Erro Errorr State State Equations Equations
In order to derive the continuous time linear state equations for the error vector, we will start with the definition of the error quaternion (eq. (158)) (158))
| ddt
⊗ qˆ¯ q¯˙ = δ˙q¯ ⊗ qˆ¯ + δq¯ ⊗ qˆ¯˙ q¯ = δ q¯
(165) (166)
Substituting the definitions for q¯˙ (eq. (105) (105))) and qˆ (155)) )) leads to ¯˙ (eq. (155
⊗
1 ω 2 0
δ˙q¯
⊗ ⊗ ⊗ ⊗ − ⊗ ⊗ ⊗ − ⊗
q¯ = δ˙q¯
⊗ qˆ¯ = 12 1 δ˙q¯ = 2
Combining the gyro model for
ω
q¯ˆ + δq¯
ω
0
ω
0
q¯
δq¯
1 ω ˆ ( 2 0 δq¯
δ q¯
ˆ ω 0
|−
| ⊗ q¯ˆ−1,
q¯ˆ
⊗ ⊗ ˆ ω 0
q¯ˆ)
eq. (159) (159)
ˆ ω
(167) (168) (169)
0
ˆ (cf. eq. (157)) (cf. eq. (132 (132)) )) and the definition of ω (157)) yields ω
TR-2005-002, Rev. 57
q¯ˆ)
1 (δq¯ 2
ˆ =ω
− ∆b − n
r
(170) 16
which, upon substitution into the above leads to
δ˙q¯ = = = = =
1 2 1 2 1 2 1 2 1 2
⊗ − ⊗ − ⊗ − × · − × · − ⊗ − − × · − − ⊗ − × · − − · × − × · − − − | || | | || | ˆ ω
δq¯
0
ˆ ω
ˆ ω
ˆ ω
δ q¯
ˆ ω
T
δq¯
0
ˆ 2 ω
0
03×1
0T 3×1
03×1
0T 3×1
δ q¯
0
ˆ 2 ω
03×1
0T 3×1
δ q¯
0
δ q¯
1 ∆b + nr 0 2
δq¯
1 ∆b + nr δ q¯ 0 2 1 (∆b + nr ) (∆b + nr ) δq T (∆ b + n ) 0 1 2 r 1 (∆b + nr ) O( ∆b δ q , nr δ q ) 0 2
δ q¯
0
ˆ 2 ω
1 ∆b + nr 0 2 ˆ ˆ + ω ω δq¯ T ˆ ω 0
(171) (172) (173) (174) (175)
Neglecting the second order terms, we can write
− 1˙ 2 δθ
δ˙q δ˙q¯ = ˙ = δq 4
ˆ ω
=
1˙
× δq − 12 (∆b + n ) r
0
(176)
or finally
δ˙θ =
−ωˆ × δθ − ∆b − n
(177)
r
The governing equation for the bias error is easily computed from eqs. ( 154) 154) and (156 (156)) as
∆˙b = b˙
− bˆ˙ = n
(178)
w
Combining these results, we may write the error state equations as
δ˙θ = ∆˙b
−
−
ˆ ω
× −I3×3 · 03×3 03×3
or
δθ + ∆b
I3×3 03×3
03×3 I3×3
· nr nw
(179)
˜˙ = Fc x ˜ + Gc n x
·
·
(180)
where
Fc =
−
ˆ ω
× −I3×3
03×3
03×3
,
Gc =
−
I3×3 03×3
03×3 I3×3
(181)
are the system matrix and the noise matrix, and
δθ ˜= x ∆b
,
n=
nr nw
(182)
denote the error state and the noise vector, respectively. As discussed in section 2.1, we assume nr and nw to be white and independent, so that the continuous time system noise covariance matrix is specified by
Nr Qc = E n(t + τ ) τ )n (t) = 03×3
2.5
T
03×3 σ 2 I3×3 = r Nw 03×3
c
·
03×3 2 σw I3×3 c
·
(183)
Discre Discrete te Time Time Err Error or State State Equations Equations
For implementation implementation of the discrete time Kalman Filter equations, we will need to discretize the above error propagation model. In particular, we will have to find the state transition matrix Φ and the system noise covariance Qd [4, Chapter 4.9]. TR-2005-002, Rev. 57
17
2.5.1 2.5.1
The State Transition ransition Matrix Matrix
Since the continuous time system matrix Fc is constant over the integration time step, we may write the state transition matrix as [4, [ 4, eq. (2-58a)]
Φ(t + ∆t, ∆t, t) = exp(Fc ∆t)
(184)
= I6×6 + Fc ∆t +
1 2 2 F ∆t + . . . 2! c
(185)
Straightforward calculation produces the powers of Fc as
Fc = 3
Fc =
− −
ˆ ω
× −I3×3
03×3
ˆ ω
03×3
3
× −ωˆ
03×3
, Fc2 = 2
×
03×3
4
, Fc =
ˆ ω
×2 ωˆ ×
03×3
ˆ ω
×
03×3
4
× ωˆ
03×3
(186)
3
03×3
In keeping with the notation of Lefferts et al. [3], 3], we see that the transition matrix has the following block structure
Θ Φ(t + ∆t, ∆t, t) = 03×3
Ψ I3×3
(187)
The matrix Θ can be written as
− ωˆ ×∆t + 2!1 ωˆ ×2∆t2 − 3!1 ωˆ ×3∆t3 + . . . ˆ × (cf. section 1.3.2) and reordering the terms yields Using the properties of the skew-symmetric matrix ω 1 1 2 1 ˆ |2 ∆t3 − . . . ω ˆ × + ˆ |2 ∆t4 + . . . ω ˆ ×2 Θ = I3×3 + −∆t + |ω ∆t − |ω 3! 2! 4! Θ = I3×3
= I3×3
1 ˆ ω
−| |
| | ˆ ω
∆t
1 ˆ 3 ∆t3 + . . . ω 3!
− | |
ˆ ω
1 + ˆ2 ω
× | |
− − 1
1
1 1 ˆ 2 ∆t2 + ˆ 4 ∆t4 ω ω 2! 4!
| |
| |
− ...
(188)
(189)
ˆ ω
×2
(190)
− |ω1ˆ | sin(|ωˆ |∆t) ωˆ × + |ωˆ1|2 (1 − cos(|ωˆ |∆t)) ωˆ ×2 ˆ × gives Further expansion of ω ˆ ˆ ω ˆ T ω ω ˆ |∆t) · I3×3 − sin(|ω ˆ |∆t) · ˆ |∆t)) · × Θ = cos cos (|ω + (1 − cos(|ω |ωˆ | |ωˆ | |ωˆ | = I3×3
(191)
(192)
ˆ ∆t where comparison with section 1.4 reveals that Θ is in fact a rotational matrix with ω as the axis of rotation and ω the corresponding angle. ˆ , either of the above expressions will lead to numerical instability. By taking the limit and For small values of ω applying L’Hopital’s oˆ pital’s rule, we arrive at
| |
| |
2
lim Θ = I3×3
|ω|→0 |→0
− ∆tωˆ × + ∆2t ωˆ ×2
(193)
Proceeding in similar fashion with the matrix Ψ, we find
−I3×3∆t + 2!1 ∆t2ωˆ × − 3!1 ∆t3ωˆ ×2 + . . . 1 2 1 1 1 ˆ |2 ∆t4 + . . . ω ˆ × + − ∆t3 + |ω ˆ |2 ∆t5 − . . . ω ˆ ×2 = −I3×3 ∆t + ∆t − |ω 2! 4! 3! 5!
Ψ=
=
=
− − −| | | | | | −
1 I3×3 ∆t + ˆ 2 ω
1 1 ˆ 2 ∆t2 ˆ 4 ∆t4 + . . . ˆ ω ω ω 1 1 2! 4! 1 3 1 2 ˆ ∆t + ω ˆ ∆t ˆ 2 ∆t5 . . . ˆ + ∆t + ω ω ω 3! 5! 1 1 I3×3 ∆t + ˆ ∆t)) ω ˆ ˆ ∆t sin( ω ˆ ∆t)) ω ˆ (1 cos( ω (ω 2 ˆ ˆ 3 ω ω
− −
TR-2005-002, Rev. 57
| |
−
| |
| |
− | |
| |
−
× − | | | | −
×
(194) (195)
(196)
×
| |
×2
(197)
18
ˆ , we can again take the limit and apply L’H opital’s For small values of ω oˆ pital’s rule and obtain
| |
ˆ |∆t) ∆t ˆ |)) ∆t sin(|ω (1 − cos(|ω ˆ × − lim ˆ ×2 −I3×3∆t + | ˆlim lim ω ω 2 ˆ| ˆ| 2|ω 3| ω |→0 |→0 | ˆ |→0 |→0 2 ˆ |∆t) ∆t ˆ |) ∆t2 cos(|ω sin(|ω ˆ × − lim ωˆ ×2 = −I3×3 ∆t + lim lim ω ˆ| 2 6|ω | ˆ |→0 |→0 | ˆ |→0 |→0 ˆ |) ∆t3 ∆t2 cos (|ω ωˆ ×2 ˆ × − lim ω = −I3×3 ∆t + 2 6 | ˆ |→0 |→0
lim Ψ =
ˆ |→0 |ω |→0
ω
(198)
ω
ω
(199)
ω
(200)
ω
2
3
−I3×3∆t + ∆2t ωˆ × − ∆6t ωˆ ×2
=
(201)
A note regarding the small ω approximation The error committed by using the small angle approximation is ˆ 2 , if the non-approximated term is divided by ω ˆ n. roughly in the order of ∆tn+2 ω ˆ thresh 2 , where Therefore, at a sampling rate of 100 Hz, the error for Θ above would be in the order of 10−6 ω ˆ thresh denotes the threshold for the small ω approximation. ω
| |
| |
|
|
2.5.2 2.5.2
The Noise Noise Cova Covarianc riancee Matri Matrix x Qd
·|
|
The covariance of the noise in the discrete time system can be computed according to [ 4, p. 171] tk+1
Qd =
− tk tk+1
=
Φ(tk+1 , τ ) τ )Gc (τ ) τ )Qc GcT (τ ) τ )ΦT (tk+1 , τ ) τ ) dτ Θ 03×3
tk tk+1
=
Θ
03×3
tk
Ψ I3×3 Ψ I3×3
− −
I3×3 03×3
−
03×3 Qc I3×3
ΘT ΨT
Qc
(202)
I3×3 03×3
03×3 I3×3
ΘT ΨT
03×3 dτ I3×3
(203)
03×3 dτ I3×3
(204) (205)
Assuming the noise to be white and independent (cf. eq. ( 183)), 183)), we can further expand to tk+1
Qd =
− ·
Θ
03×3
tk tk+1
=
Ψ I3×3
σr2
c
· I3×3
03×3
2 ΨΨT σr2 I3×3 + σw 2 T σw Ψ
tk
·
·
03×3 2 σw I3×3 c
·
ΘT ΨT
−
2 Ψ σw dτ 2 σw I3×3
·
·
03×3 dτ I3×3
(206) (207)
where the last step follows from the fact, that Θ is a rotational matrix, and therefore ΘΘT gives the identity matrix. The resulting matrix Qd has the following structure
Q11 Qd = T Q12
Q12 Q22
(208)
and the elements follow after considerable algebra as 2 Q11 = σr2 ∆t I3×3 + σw
·
Q12 =
−σ2 · w
∆t2 I3×3 2
2 Q22 = σw ∆t I3×3
·
TR-2005-002, Rev. 57
·
∆t3 I3×3 + 3
ˆ |∆t)3 (|ω 3
ˆ ∆t) + 2 sin( sin( ω ˆ5 ω
| | | |
− 2|ωˆ |∆t · ωˆ ×2 2
(| ˆ |∆ ) ˆ |∆t − sin(|ω ˆ |∆t) | ω 2 ˆ − · ω × + 3 |ωˆ | ω
t
ˆ ∆t) + cos( ω ˆ 4 ω
| |
| |
− 1 · ωˆ ×2
(209)
(210) (211)
19
Similar to the transition matrix, we can derive the form for small rule 2
2
lim Q11 = σr ∆t I3×3 + σw
·
ˆ |→0 |ω |→0
lim Q12 =
ˆ |→0 |ω |→0
2.6
−σ
2
w
·
∆t2 I3×3 2
|ωˆ | by taking the limit and applying L’H opital’s oˆ pital’s
∆t3 2∆t5 2∆t 2 I3×3 ˆ ω + 3 5! 3 ∆t ∆t4 ˆ ˆ + ω ω 3! 4!
·
−
· ×
×
·
× 2
(212) (213)
Propa Propagat gation ion Equati Equations ons
Having defined the propagation of the quaternion (cf. section 1.6), 1.6), and the discrete time state transition- and noise covariance matrices (cf. section 2.5), 2.5), we can now write the Kalman Filter propagation equations. Assume we receive gyro measurements ω mk and ω mk+1 , and we have an estimate of the quaternion qˆ ¯k|k and the
ˆ k|k at timestep k , together with the corresponding covariance matrix Pk|k . From this, using eq. (157 bias b ( 157), ), we also ˆ k|k . have an estimate of ω We now proceed as follows: 1. We propagate the bias using the discretized discretized form of eq. (156) ( 156) as
ˆ k+1| ˆ b +1|k = bk|k
(214)
ˆ k+1| 2. Using Using the measureme measurement nt ω mk+1 and b +1|k , we form the estimate of the new turn rate according to eq. ( 157) as ˆ k+1| ω +1|k
= ωm
k+1
− bˆ +1| +1| k
k
(215)
ˆ k|k and ω ˆ k+1| 3. We propagate propagate the quaternion quaternion using a first order integrato integratorr (cf. section section 1.6.2) 1.6.2) with ω +1|k to obtain ˆ q¯k+1| +1|k . 4. From the formulas formulas in sections sections 2.5.1 and 2.5.2 we compute the state transition matrix Φ and the discrete time noise covariance matrix Qd . 5. We compute the state covariance matrix according according to the Extended Kalman Filter equation T Pk+1| +1|k = ΦPk|k Φ + Qd
TR-2005-002, Rev. 57
(216)
20
3
Update
In the update update phase phase of the Kalman Kalman filter filter,, we will will use an exter exteroce ocepti ptive ve sensor sensor to measur measuree orient orientati ation. on. Common Common examp examples les are star- and sunsensors. We will now consider a simplified model of a sunsensor, a more detailed version of which is presented in [9] [ 9]..
3.1
Measur Measureme ement nt Model Modelss
In order to provide exterocep exteroceptiv tivee informati information on to the attitude attitude filter, filter, we can envisag envisagee a multitud multitudee of sensors. Each is characterized by its particular sensor model. In the following, we will present a selection of sensors models, followed by the general procedure to fuse these measurements with the filter estimate. 3.1.1 3.1.1
Sun Sensor Sensor
In this model, we assume that we are able to measure a projection of the vector from the sensor to the sun with respect to sensor frame S r , whose representation in the global coordinate frame G r is known. The relationship between the two is given by the rotation S
The rotational matrix
S GC
r =
S G G C r
(217)
can be decomposed as S GC
L C = SL CG
(218)
where the transformation S L C between sensor frame S and spacecraft frame L is known and fixed, and the transL C is a function of the attitude quaternion. formation G The actual measurement z will be a projection of the vector r , corrupted by zero-mean, white, Gaussian noise nm L G z = ΠS (219) L CG C r + nm
{}
{}
where Π is the projection matrix. The the measurement noise is characterized by
E [ E [nm ] = 0
(220)
E nm nm T = R
(221)
˜ to the state vector. For the update phase of the Kalman filter, we need to relate the measurement error z ˜=z z
− zˆ =
ΠS LC
L q) G C(¯
−
ˆ L ˆ¯) G C(q
·
G
r + nm
(222)
From the error model (cf. section 2.3) and the properties of the rotational matrix (cf. section 1.4) 1.4) we recall the following expressions
q¯ = δq¯ L q) G C(¯
L ¯) ˆ C(δ q L
TR-2005-002, Rev. 57
⊗ q¯ˆ
=
L ¯ G C(δ q
=
L L
(223)
⊗ q¯ˆ) ˆ ¯) · C(q¯ˆ) ˆ C(δ q L G
(224)
≈ I − δθ ×
(225)
21
and hence 2 L q) G C(¯
−
ˆ L ˆ¯) G C(q
=
We can now write
= ΠSL C
˜ z
− · −
L ¯) ˆ C(δ q L
L ¯) ˆ C(δ q L
ˆ L ˆ¯) G C(q
ˆ L ˆ¯) G C(q
I
·
−δθ × · ˆ C(qˆ¯) L G
=
G
r + nm
ΠS LC (
=
S L
L G
L G
S L
G
L G
(229)
m
G
(230)
m
G
(227)
(228)
−δθ ×) ˆ C(qˆ¯) · r + n ˆ Π C C(qˆ ¯) r × · δθ + n δθ ˆ Π C C(qˆ ¯) r × 0 · ˜ + n b
≈
=
I3×3
(231)
m
so that the measurement matrix H corresponds to
H=
3.2
ΠS LC
ˆ L ˆ¯)G r G C(q
×
(232)
0
Kalman Kalman Filte Filterr Update Update
ˆ +1|k , as well as their covariance matrix Pk+1| ¯k+1| Given the propagated state estimates qˆ +1|k and bk+1| +1|k , the current measurement z(k + 1), and the measurement matrix H, we can update our estimate in the following way: 1. Compute the measurement matrix H(k) according to eq. (232) (232) 2. Compute Compute residua residuall r according to
r=z
− zˆ
(233) (234)
3. Compute the covariance covariance of the the residual S as
S = HPHT + R
(235)
K = PHT S−1
(236)
ˆ (+) ˆ (+) 2 δq δθ ˆ (+) = ∆x = = Kr ˆ ˆ (+) ∆b(+) ∆b
(237)
4. Compute the the Kalman gain K
ˆ (+) 5. Compute the correction correction ∆x
·
ˆ (+) δq ˆ (+)T δ q ˆ (+) δq
6. Update the quaternion quaternion according according to
δ qˆ¯ = ˆ (+)T δq ˆ (+) > 1, using or, if δ q δ qˆ¯ =
1
1 ˆ (+)T δ q ˆ (+) 1 + δq
ˆ¯ qˆ¯k+1| +1|k+1 = δ q 2
−
(238)
· ˆ (+) δq 1
(239)
⊗ qˆ¯ +1| +1| k
(240)
k
ˆ + ∆¯ Note here the difference to the expression obtained if we were using an additive instead of multiplicative error model, where q¯ = q¯ q with
∆¯ q = ∆qT
∆q4
T
:
L G
ˆ + ∆q¯) C(q¯
TR-2005-002, Rev. 57
−
ˆ L G
ˆ) = 4 qˆ4 ∆q4 I3×3 C(q¯
T
ˆ × + 2∆qˆ 2∆q q q − 2qˆ ∆q × − 2∆q 4
4
+ 2 q∆q q ˆ∆qT
(226)
22
7. Update Update the bias bias
ˆ k+1| ˆ +1|k + ∆b ˆ (+) b +1|k+1 = bk+1|
(241)
8. Update the estimated turn turn rate using the new estimate for the bias
ˆ k+1| ω +1|k+1
= ωm
k+1
− bˆ +1| +1| +1 k
k
(242)
9. Compute the new new updated Covariance Covariance matrix
Pk+1| +1|k+1 = (I6×6
TR-2005-002, Rev. 57
T T − KH)P +1| +1| (I6×6 − KH) + KRK k
k
(243)
23
References [1] M. D. Shuster, Shuster, “A survey of attitude representations, representations,”” Journal of the Astronautical Sciences , vol. 41, no. 4, pp. 439–517, October–December 1993. [2] W. G. Breckenridge, “Quaternions “Quaternions - Proposed Standard Conventions,” Conventions,” JPL, Tech. Rep. INTEROFFICE MEMORANDUM IOM 343-79-1199, 1999. [3] E. J. Lefferts, F. F. L. Markley, Markley, and M. D. Shuster, “Kalman filtering for spacecraft attitude estimation,” estimation,” Journal of Guidance, Control, and Dynamics , vol. 5, no. 5, pp. 417–429, Sept.-Oct. 1982. [4] P. S. Maybeck, Maybeck, Stochastic Models, Estimation and Control .
New York: Academi Academicc Press, Press, 1979, 1979, vol. vol. 1. 1.
[5] J. R. Wertz Wertz,, Ed., Spacecraft Attitude Determination and Control .
Dordrecht Dordrecht;; Boston: Boston: Kluwer Kluwer Academic, Academic, 1978.
[6] R. O. Allen Allen and D. H. Chang, Chang, “Perform “Performance ance Testin Testing g of the Systron Systron Donner Quartz Gyro (QRS11-100-42 (QRS11-100-420); 0); Sn’s 3332, 3347 and 3544,” JPL, Tech. Rep. ENGINEERING MEMORANDUM EM #343-1297, 1993. [7] D. Simon, Simon, Optimal State Estimation, Kalman, H ∞ , and Nonlinear Approaches. Sons, Inc., 2006.
Hobok Hoboken, en, NJ: John John Wile Wiley y &
[8] J. Crassidi Crassidis, s, “Sigma-po “Sigma-point int Kalman filtering filtering for integrat integrated ed GPS and inertial navigatio navigation, n,”” in Proc. Proc. of AIAA AIAA Guidance, Navigation, and Control Conference, Paper #2005-6052 , San Francisco, CA, Aug. 2005. [Online]. Available: http://www.acsu.buffalo.edu/ ∼ johnc/gpsins gnc05.pdf [9] N. Trawny, Trawny, “Sun sensor model,” University University of Minnesota, Minnesota, Dept. of Comp. Sci. & Eng., Tech. Tech. Rep. 2005-001, Jan. 2005.
TR-2005-002, Rev. 57
24
