Professional Documents
Culture Documents
Multiple Autonomous
Robotic Systems Laboratory
Technical Report
Number 2005-002
March 2005
q̄ = q4 + q1 i + q2 j + q3 k (1)
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 1.4 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
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
1.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)
= 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̄
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)
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 ||.
Quaternions also have an neutral element with respect to multiplication, which is defined as
£ ¤T
q̄0 = 0 0 0 1 (19)
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)
ba ×c + bb ×c = ba + b ×c (27)
Scalar Multiplication
c · bω ×c = bc ω ×c (28)
TR-2005-002 4
Lagrange’s Formula
¡ ¢
ba ×cbb ×c = baT − aT b I3×3 (30)
T T
⇔ a × (b × c) = b(a c) − c(a b) (31)
Jacobi Identity
Rotations
TR-2005-002 5
Powers of bω ×c
¡ ¢
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ω ×c6 = bω ×c5 · bω ×c
= +|ω|4 · bω ×c2 (45)
and so on.
TR-2005-002 6
1.3.2 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)
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.
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)
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.
We can write the quaternion describing a small rotation as
· ¸
δq
δ q̄ = (68)
δq4
· ¸
k̂ sin(δθ/2)
= (69)
cos(δθ/2)
·1 ¸
δθ
≈ 2 (70)
1
TR-2005-002 8
The result from eq. (62) could have also been found by rewriting Euler’s formula which relates the rotational
matrix and the angle–axis representation [2]
L
GC = cos(θ) · I3×3 − sin(θ)bk̂ ×c + (1 − cos(θ)) k̂k̂T (72)
¡ ¢
= 2 cos2 (θ/2) − 1 · I3×3 − 2 cos(θ/2) sin(θ/2)bk̂ ×c + 2 sin2 (θ/2)k̂k̂T (73)
Substituting the appropriate quaternion components according to eq. (4) readily yields equation (62). The latter
can also be expressed in terms of quaternion components as
2
q1 − q22 − q32 + q42 2 (q1 q2 + q3 q4 ) 2 (q1 q3 − q2 q4 )
L
G C(q̄) =
2 (q1 q2 − q3 q4 ) −q12 + q22 − q32 + q42 2 (q2 q3 + q1 q4 ) (74)
2 (q1 q3 + q2 q4 ) 2 (q2 q3 − q1 q4 ) −q12 − q22 + q32 + q42
2
2q1 + 2q42 − 1 2 (q1 q2 + q3 q4 ) 2 (q1 q3 − q2 q4 )
= 2 (q1 q2 − q3 q4 ) 2q22 + 2q42 − 1 2 (q2 q3 + q1 q4 ) (75)
2 (q1 q3 + q2 q4 ) 2 (q2 q3 − q1 q4 ) 2q32 + 2q42 − 1
1 − 2q22 − 2q32 2 (q1 q2 + q3 q4 ) 2 (q1 q3 − q2 q4 )
= 2 (q1 q2 − q3 q4 ) 1 − 2q12 − 2q32 2 (q2 q3 + q1 q4 ) (76)
2 (q1 q3 + q2 q4 ) 2 (q2 q3 − q1 q4 ) 1 − 2q12 − 2q22
Another alternative form of eq. (62) arises after exploiting the relationship between qqT and bq ×c2 (cf. eq. (41))
L
G C(q̄) = I3×3 − 2q4 bq ×c + 2bq ×c2 (77)
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, 5]. Thus,
µ ¶
L1 L1 L2 L2 L1 L1 L2
L2 C(L2 q̄) · G C(G q̄) = G C L2 q̄ ⊗ G q̄ (78)
· ¸
L1 k̂ sin(θ/2)
Finally, we can interpret L2 q̄
= as the quaternion describing the rotation of coordinate frame {L2 }
cos(θ/2)
to coordinate frame {L1 }, with the axis of rotation k̂ expressed in {L1 } (cf. figure 1). This can also be expressed as
the matrix exponential ³ ´
L
G C(q̄) = exp −bk̂ ×cθ (79)
L(t+∆t)
The quaternion G q̄ can be expressed as the product of two quaternions
L(t+∆t) L(t+∆t) L(t)
G q̄ = L(t) q̄ ⊗ G q̄ (81)
where · ¸
L(t+∆t) k̂ sin(θ/2)
L(t) q̄ = (82)
cos(θ/2)
L(t+∆t)
Note, that the quaternion L(t) q̄ describes the rotation of reference frame {L(t)} to reference frame {L(t+∆t)}
in terms of the rotation angle θ and the axis of rotation k̂ (the latter being expressed in {L(t + ∆t)}).
TR-2005-002 9
Figure 1: Relationship between global and local frame
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
· ¸ · ¸ ·1 ¸
L(t+∆t) k̂ sin(θ/2) k̂ · θ/2 2 · δθ
q̄ = ≈ = (83)
L(t) cos(θ/2) 1 1
The vector δθ has the direction of the axis of rotation and the magnitude of the angle of the rotation. Dividing this
vector by ∆t will yield, in the limit, the rotational velocity
δθ
ω = lim (84)
∆t→0 ∆t
TR-2005-002 10
and (cf. sections 1.3.2 and 1.3.3)
q4 −q3 q2
L(t) q3 q4 −q1
Ξ(G q̄) =
−q2
(89)
q1 q4
−q1 −q2 −q3
Note that ω = L(t) ω, i. e. the turn rate is expressed in the local, not the inertial coordinate frame [5, p. 482].
L ˙ 1
G q̄(t) = Ω(ω) L
G q̄(t) (90)
2
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
G q̄(t) = Θ(t, tk ) L
G q̄(tk ) (91)
Differentiating and reordering the terms yields the governing equation for Θ(t, tk ) as
L ˙ L −1
Θ̇(t, tk ) = G q̄(t) G q̄ (tk ) (92)
1
= Ω(ω(t)) L L −1
G q̄(t) G q̄ (tk ) (93)
2
1
= Ω(ω(t))Θ(t, tk ) (94)
2
with the initial condition
Θ(tk , tk ) = I4×4 (95)
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.
TR-2005-002 11
Reordering and expanding with |ω| yields
à µ ¶2 µ ¶4 !
1 1 1 1
Θ(∆t) = 1− ∆t |ω|2 + ∆t |ω|4 − . . . I4×4
2! 2 4! 2
à µ ¶3 µ ¶5 !
1 1 1 1 1 1
+ |ω|∆t − |ω|∆t + |ω|∆t − . . . Ω(ω) (99)
|ω| 2 3! 2 5! 2
After close inspection, we recognize the Taylor series expansions of the sin and cos functions
µ ¶ µ ¶
|ω| 1 |ω|
Θ(∆t) = cos ∆t · I4×4 + sin ∆t · Ω(ω) (100)
2 |ω| 2
A close look reveals that Θ(tk+1 , tk ) is nothing but the multiplication matrix associated with a specific quaternion,
so that we can rewrite the zero-th order quaternion integration as a quaternion product
³ ´
ω |ω|
· sin ∆t
L
G q̄(tk+1 ) =
|ω| ³ 2 ´ ⊗ L G q̄(tk ) (102)
cos |ω|2 ∆t
This quaternion product corresponds to rotating the original frame around the rotation axis defined by ω through the
angle |ω|∆t, which corresponds exactly to the assumption of a constant ω.
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ôpital’s
rule.
µ µ ¶ µ ¶ ¶
|ω| 1 |ω|
lim Θ(∆t) = lim cos ∆t I4×4 + sin ∆t Ω(ω)
|ω|→0 |ω|→0 2 |ω| 2
µ µ ¶ ¶
1 |ω|
= I4×4 + lim sin ∆t Ω(ω)
|ω|→0 |ω| 2
∆t
= I4×4 + Ω(ω) (103)
2
L 1
G q̄(tk+1 ) =L L ˙
G q̄(tk ) +G q̄(tk )∆t +
L¨
G q̄ (tk )∆t
2
+ ... (106)
2
TR-2005-002 12
Repeatedly applying the definition of the quaternion time derivative (eq. (86)) yields
à µ ¶2 µ ¶3 !
L 1 1 1 1 1
G q̄(tk+1 ) = I4×4 + Ω(ω(tk ))∆t + Ω(ω(tk ))∆t + Ω(ω(tk ))∆t + . . . L G q̄(tk )
2 2! 2 3! 2
µ ¶
1 1 1
+ ∆t2 Ω(ω̇(tk ))LG q̄(t k ) + Ω(ω̇(tk ))Ω(ω(tk )) + Ω(ω(t k ))Ω( ω̇(t k )) ∆t3L
G q̄(tk ) + . . . (107)
4 12 24
Recognizing the first term as the Taylor series expansion of the matrix exponential, and after replacing Ω(ω̇) with
its definition (eq. (105)), we obtain the final formula
µ µ ¶ ¶
L 1 1³ ¡ ¢ ¡ ¢ ¡ ¢ ¡ ¢´ 2 L
G q̄(tk+1 ) = exp Ω( ω̄)∆t + Ω ω(t k+1 ) Ω ω(t k ) − Ω ω(t k ) Ω ω(tk+1 ) ∆t G q̄(tk ) (110)
2 48
TR-2005-002 13
2 Attitude Propagation
The Kalman Filter consists of two stages to determine an estimate of the current attitude. In the first stage, the
propagation, the filter produces a prediction of the attitude based on the last estimate and some proprioceptive mea-
surements. 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 way to estimate position and orientation is to use data from an
inertial 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.
TR-2005-002 14
and µ ¶
rad 1 rad
[σrc ] = ·√ =√ (124)
sec Hz sec
Analogously, we obtain
µ ¶2
£ 2 ¤ rad
σwc = /sec · sec (125)
sec
rad2
= (126)
sec3
µ ¶2
rad
= · Hz (127)
sec
and µ ¶
rad rad√
[σwc ] = Hz = √ · (128)
sec sec3
In the discrete case, nrd and nwd will have to have the same units as the turn rate (cf. also the state equation in
section 2.2), namely rad
sec . In order to preserve equivalence of the noise strength, we have to incorporate the sampling
frequency when discretizing, yielding
σ
σrd = √ rc (129)
∆t
√
σwd = σwc · ∆t (130)
1
where ∆t is the inverse of the sampling frequency, or fsample = ∆t . The discrete variances will be used in the
simulation of noisy real-world data.
Further explanation for conversion between continuous and discrete noise variance is provided by Simon [6,
pp. 230-233]. To make the analogy to the derivation in the book, the bias driving noise, nw , corresponds to the
process noise, whereas the rate noise, nr , corresponds to the measurement noise.
L ˙ 1
G q̄(t) = Ω(ω m − b − nr ) L
G q̄(t) (132)
2
ḃ = nw (133)
Taking the expectation of the above yields the prediction equations (cf. [3, p. 422]) for the state within the EKF-
framework
Lˆ ˙ 1
G q̄ (t) = Ω(ω̂) L ˆ
G q̄ (t) (134)
2
˙
b̂ = 03×1 (135)
with
ω̂ = ω m − b̂ (136)
Since the bias is constant over the integration interval, we may integrate the quaternion using the zeroth order (cf.
section 1.6.1) or first order (cf. section 1.6.2) integrator, using ω̂ instead of ω.
TR-2005-002 15
2.3 Error and Covariance Representation
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. The fact that the quaternion is enforced to be of unit length (cf. eq. (5)) makes the corresponding
covariance matrix singular [3, p. 423], which is difficult to maintain numerically. For stability 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.
L
G q̄ =L
L̂
δ q̄ ⊗ L̂ ˆ
G q̄ (137)
L L L̂ ˆ−1
L̂
δ q̄ = G q̄ ⊗ G q̄ (138)
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) and define the attitude error angle vector δθ as follows
· ¸
δq
δ q̄ = (139)
δq4
· ¸
k̂ sin(δθ/2)
= (140)
cos(δθ/2)
·1 ¸
δθ
≈ 2 (141)
1
This error angle vector δθ is of dimension 3 × 1 and will be used together with the bias error in the error state vector.
The bias error is defined as
∆b = b − b̂ (142)
We can now define the error vector as · ¸
δθ
x̃ = (143)
∆b
In the next section we will develop the continuous time first order state equation for the error vector.
Substituting the definitions for q̄˙ (eq. (85)) and q̄ˆ˙ (eq. (134)) leads to
· ¸ · ¸ · ¸
1 ω ˙ 1 ω̂ 1 ω̂
⊗ q̄ = δ q̄ ⊗ q̄ˆ + δ q̄ ⊗ ( ⊗ q̄ˆ) | − (δ q̄ ⊗ ⊗ q̄ˆ) (146)
2 0 2 0 2 0
µ· ¸ · ¸ ¶
˙ 1 ω ω̂
δ q̄ ⊗ q̄ˆ = ⊗ q̄ − δ q̄ ⊗ ⊗ q̄ˆ | ⊗ q̄ˆ−1 , eq. (138) (147)
2 0 0
µ· ¸ · ¸¶
1 ω ω̂
δ˙q̄ = ⊗ δ q̄ − δ q̄ ⊗ (148)
2 0 0
Combining the gyro model for ω (cf. eq. (111)) and the definition of ω̂ (cf. eq. (136)) yields
ω = ω̂ − ∆b − nr (149)
TR-2005-002 16
which, upon substitution into the above leads to
µ· ¸ · ¸¶ · ¸
˙ 1 ω̂ ω̂ 1 ∆b + nr
δ q̄ = ⊗ δ q̄ − δ q̄ ⊗ − ⊗ δ q̄ (150)
2 0 0 2 0
µ· ¸ · ¸ ¶ · ¸
1 −bω̂ ×c ω̂ +bω̂ ×c ω̂ 1 ∆b + nr
= T · δ q̄ − T · δ q̄ − ⊗ δ q̄ (151)
2 −ω̂ 0 −ω̂ 0 2 0
· ¸ · ¸
1 −2bω̂ ×c 03×1 1 ∆b + nr
= · δ q̄ − ⊗ δ q̄ (152)
2 0T 3×1 0 2 0
· ¸ · ¸ · ¸
1 −2bω̂ ×c 03×1 1 −b(∆b + nr ) ×c (∆b + nr ) δq
= · δ q̄ − · (153)
2 0T 3×1 0 2 −(∆b + nr )T 0 1
· ¸ · ¸
1 −2bω̂ ×c 03×1 1 (∆b + nr )
= · δ q̄ − − O(|∆b||δq|, |nr ||δq|) (154)
2 0T 3×1 0 2 0
or finally
˙ = −ω̂ × δθ − ∆b − nr
δθ (156)
The governing equation for the bias error is easily computed from eqs. (133) and (135) as
˙ = ḃ − b̂˙ = nw
∆b (157)
or
x̃˙ = Fc · x̃ + Gc · n (159)
where
· ¸ · ¸
−bω̂ ×c −I3×3 −I3×3 03×3
Fc = , Gc = (160)
03×3 03×3 03×3 I3×3
TR-2005-002 17
2.5.1 The State Transition Matrix
Since the continuous time system matrix Fc is constant over the integration time step, we may write the state transition
matrix as [4, eq. (2-58a)]
Φ(t + ∆t, t) = exp (Fc ∆t) (163)
1 2 2
= I6×6 + Fc ∆t + F ∆t + . . . (164)
2! c
Straightforward calculation produces the powers of Fc as
· ¸ · ¸
−bω̂ ×c −I3×3 bω̂ ×c2 bω̂ ×c
Fc = , F2c =
03×3 03×3 03×3 03×3
· 3 2
¸ · ¸ (165)
−bω̂ ×c −bω̂ ×c bω̂ ×c4 bω̂ ×c3
F3c = , F4c =
03×3 03×3 03×3 03×3
In keeping with the notation of Lefferts et al. [3], we see that the transition matrix has the following block structure
· ¸
Θ Ψ
Φ(t + ∆t, t) = (166)
03×3 I3×3
The matrix Θ can be written as
1 1
Θ = I3×3 − bω̂ ×c∆t + bω̂ ×c2 ∆t2 − bω̂ ×c3 ∆t3 + . . . (167)
2! 3!
Using the properties of the skew-symmetric matrix bω̂ ×c (cf. section 1.3.1) and reordering the terms yields
µ ¶ µ ¶
1 2 3 1 2 1
Θ = I3×3 + −∆t + |ω̂| ∆t − . . . bω̂ ×c + ∆t − |ω̂| ∆t + . . . bω̂ ×c2
2 4
(168)
3! 2! 4!
µ ¶ µ µ ¶¶
1 1 1 1 1
= I3×3 − |ω̂|∆t − |ω̂|3 ∆t3 + . . . bω̂ ×c + 2
1 − 1 − |ω̂|2 ∆t2 + |ω̂|4 ∆t4 − . . . bω̂ ×c2
|ω̂| 3! |ω̂| 2! 4!
(169)
1 1
= I3×3 − sin (|ω̂|∆t) bω̂ ×c + (1 − cos(|ω̂|∆t)) bω̂ ×c2 (170)
|ω̂| |ω̂|2
Further expansion of bω̂ ×c gives
ω̂ ω̂ ω̂ T
Θ = cos (|ω̂|∆t) · I3×3 − sin (|ω̂|∆t) · b ×c + (1 − cos(|ω̂|∆t)) · (171)
|ω̂| |ω̂| |ω̂|
where comparison with section 1.4 reveals that Θ is in fact a rotational matrix with ω as the axis of rotation and |ω̂|∆t
the corresponding angle.
For small values of |ω̂|, either of the above expressions will lead to numerical instability. By taking the limit and
applying L’Hôpital’s rule, we arrive at
∆t2
lim Θ = I3×3 − ∆tbω̂ ×c + bω̂ ×c2 (172)
|ω|→0 2
Proceeding in similar fashion with the matrix Ψ, we find
1 2 1
Ψ = −I3×3 ∆t + ∆t bω̂ ×c − ∆t3 bω̂ ×c2 + . . . (173)
2!
µ 3! ¶ µ ¶
1 2 1 1 3 1
= −I3×3 ∆t + ∆t − |ω̂| ∆t + . . . bω̂ ×c + − ∆t + |ω̂| ∆t − . . . bω̂ ×c2
2 4 2 5
(174)
2! 4! 3! 5!
µ µ ¶¶
1 1 1
= −I3×3 ∆t + 1 − 1 − |ω̂|2 ∆t2 − |ω̂|4 ∆t4 + . . . bω̂ ×c
|ω̂|2 2! 4!
µ µ ¶¶ (175)
1 1
+ −|ω̂|∆t + |ω̂|∆t − ∆t3 + |ω̂|2 ∆t5 − . . . bω̂ ×c2
3! 5!
1 1
= −I3×3 ∆t + (1 − cos(|ω̂|∆t)) bω̂ ×c − (|ω̂|∆t − sin(|ω̂|∆t)) bω̂ ×c2 (176)
|ω̂|2 |ω̂|3
TR-2005-002 18
For small values of |ω̂|, we can again take the limit and apply L’Hôpital’s rule and obtain
A note regarding the small ω approximation The error committed by using the small angle approximation is
roughly in the order of ∆tn+2 |ω̂|2 , if the non-approximated term is divided by |ω̂|n .
Therefore, at a sampling rate of 100 Hz, the error for Θ above would be in the order of 10−6 · |ω̂ thresh |2 , where
|ω̂ thresh | denotes the threshold for the small ω approximation.
Assuming the noise to be white and independent (cf. eq. (162)), we can further expand to
Z tk+1 · ¸· 2 ¸· ¸
−Θ Ψ σrc · I3×3 03×3 −ΘT 03×3
Qd = 2 dτ (185)
tk 03×3 I3×3 03×3 σw c
· I3×3 ΨT I3×3
Z tk+1 · 2 2
¸
σr · I3×3 + σw · ΨΨT σw2
·Ψ
= 2 dτ (186)
tk σw · ΨT 2
σw · I3×3
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 Q12
Qd = (187)
QT12 Q22
TR-2005-002 19
Similar to the transition matrix, we can derive the form for small |ω̂| by taking the limit and applying L’Hôpital’s
rule
µ ¶
2 2 ∆t3 2∆t5 2
lim Q11 = σr ∆t · I3×3 + σw I3×3 + · bω̂ ×c (191)
|ω̂|→0 3 5!
µ ¶
2 ∆t2 ∆t3 ∆t4
lim Q12 = −σw · I3×3 − · bω̂ ×c + · bω̂ ×c2 (192)
|ω̂|→0 2 3! 4!
2. Using the measurement ω mk+1 and b̂k+1|k , we form the estimate of the new turn rate according to eq. (136) as
3. We propagate the quaternion using a first order integrator (cf. section 1.6.2) with ω̂ k|k and ω̂ k+1|k to obtain
q̄ˆk+1|k .
4. From the formulas in 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 to the Extended Kalman Filter equation
TR-2005-002 20
3 Update
In the update phase of the Kalman filter, we will use an exteroceptive sensor to measure orientation. Common examples
are star- and sunsensors. We will now consider a simplified model of a sunsensor, a more detailed version of which is
presented in [7].
where the transformation SL C between sensor frame {S} and spacecraft frame {L} is known and fixed, and the trans-
formation L
G C is a function of the attitude quaternion.
The actual measurement z will be a projection of the vector r¯ , corrupted by zero-mean, white, Gaussian noise
nm
z = ΠSL CL G
G C r¯ + nm (198)
where Π is the projection matrix.
The the measurement noise is characterized by
E [nm ] = 0 (199)
£ ¤
E nm nm T = R (200)
For the update phase of the Kalman filter, we need to relate the measurement error z̃ to the state vector.
³ ´
z̃ = z − ẑ = ΠSL C L G C(q̄) − L̂
G C( ˆ
q̄ ) · G r¯ + n m (201)
From the error model (cf. section 2.3) and the properties of the rotational matrix (cf. section 1.4) we recall the
following expressions
q̄ = δ q̄ ⊗ q̄ˆ (202)
L L
G C(q̄) = G C(δ q̄ ⊗ q̄ˆ)
L
= L̂
C(δ q̄) · L̂ ˆ
G C(q̄ ) (203)
L
L̂
C(δ q̄) ≈ I − bδθ ×c (204)
TR-2005-002 21
and hence 1 ³ ´
L
G C(q̄) − L̂ ˆ
G C(q̄ ) =
L
L̂
C(δ q̄) − I3×3 · L̂ ˆ L̂ ˆ
G C(q̄ ) = −bδθ ×c · G C(q̄ ) (206)
We can now write
¡L ¢
z̃ = ΠSL C L̂
C(δ q̄) − I L̂ ˆ
G C(q̄ ) · G r¯ + nm (207)
≈ ΠSL C (−bδθ ×c) L̂ ˆ
G C(q̄ ) · G r¯ + nm (208)
= ΠSL C bL̂ ˆG
G C(q̄ ) r¯ ×c · δθ + nm (209)
h i ·δθ ¸
= ΠSL C bL̂
G C( ˆ
q̄ ) G
r¯ ×c 0 · + nm (210)
b̃
so that the measurement matrix H corresponds to
h i
H = ΠSL C bL̂ ˆG
G C(q̄ ) r¯ ×c 0
(211)
r = z − ẑ (212)
(213)
S = HPHT + R (214)
TR-2005-002 22
7. Update the bias
b̂k+1|k+1 = b̂k+1|k + ∆b̂(+) (220)
8. Update the estimated turn rate using the new estimate for the bias
TR-2005-002 23
References
[1] R. O. Allen and D. H. Chang. Performance Testing of the Systron Donner Quartz Gyro (QRS11-100-420);
Sn’s 3332, 3347 and 3544. Technical Report ENGINEERING MEMORANDUM EM #343-1297, JPL, 1993.
[2] W. G. Breckenridge. Quaternions - Proposed Standard Conventions. Technical Report INTEROFFICE MEMO-
RANDUM IOM 343-79-1199, JPL, 1999.
[3] E. J. Lefferts, F. L. Markley, and M. D. Shuster. Kalman filtering for spacecraft attitude estimation. Journal of
Guidance, Control, and Dynamics, 5(5):417–429, Sept.-Oct. 1982.
[4] P. S. Maybeck. Stochastic Models, Estimation and Control, volume 1. Academic Press, New York, 1979.
[5] M. D. Shuster. A survey of attitude representations. Journal of the Astronautical Sciences, 41(4):439–517,
October–December 1993.
[6] D. Simon. Optimal State Estimation, Kalman, H∞ , and Nonlinear Approaches. John Wiley & Sons, Inc., Hobo-
ken, NJ, 2006.
[7] N. Trawny. Sun sensor model. Technical Report 2005-001, University of Minnesota, Dept. of Comp. Sci. & Eng.,
Jan. 2005.
[8] J. R. Wertz, editor. Spacecraft Attitude Determination and Control. Kluwer Academic, Dordrecht; Boston, 1978.
TR-2005-002 24