Module 1 1

1. Coordinate Systems
2. Rotation Matrix
3. Direction Cosine Matrix
4. Euler Angles
5. Quaternions
6. Kinematics Basics

2
1.1
▪ The Earth’s coordinate system (or the world CS) is
fixed in inertial space. The origin of this coordinate
system is denoted 𝐸 𝑶1 .

▪ The Body-fixed coordinate system is rigidly

attached to the object whose attitude we
would like to describe. The origin of this CS is
denoted 𝐸 𝑶2 .

▪ If 𝐸 𝑷 is a point in the Earth CS, then 𝐵𝑷 is the

same point described in the Body-fixed CS.

▪ Note: 𝐸 𝑶1 and 𝐵 𝑶2 are usually both zero,

but 𝐵𝑶1 and 𝐸 𝑶2 are generally not.

4
▪ There can be different orientations of Sensor CS, Body CS and the Earth’s CS!

▪ First of all, it is necessary to know how to transfer measurements from one CS to another.

5
Check Point

The AscTec “Firefly” hex-rotor helicopter CSs (left) and

the relations of CSs used while capturing the datasets (right)
6
1.2
▪ The pose of a rigid body is the position
and attitude of that body.
▪ The rigid body position is most naturally
encoded by 𝐸 𝑶2 , the position of the origin of
the body-fixed coordinate system (CS) as
expressed in the world (or the Earth’s) CS.

▪ It is equally valid to store 𝐵 𝑶1 , the

position of the origin of the Earth’s CS as
expressed in the body-fixed CS.

▪ Our main goal is to understand how to

define attitude of the Body CS.

8
▪ A coordinate rotation is a rotation about a
single coordinate axis.
▪ Then coordinate rotations around
corresponding axis can be described by
one of the Rotation Matrices:
1 0 0
𝑹𝑋 𝛼 = 0 𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼
0 −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼
𝑐𝑜𝑠 𝛼 0 −𝑠𝑖𝑛 𝛼
𝑹𝑌 𝛼 = 0 1 0
𝑠𝑖𝑛 𝛼 0 𝑐𝑜𝑠 𝛼
𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
An example of coordinate rotation about
𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0 the Z-axis by an angle 𝛼
0 0 1 9
Check Point
▪ Let’s prove the view of coordinate
Rotation Matrix around Z axis (𝑹𝑍 ). Why
does it look like this?

𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
0 0 1

10
▪ Rotation Matrix (RM) is a matrix whose multiplication with a vector rotates the
vector while preserving its length.
▪ The special orthogonal group of all 3x3 RMs is denoted by SO(3). Thus, if R є SO(3),
then
det 𝑹 = ±1 𝑹−1 = 𝑹𝑇
𝑟11 𝑟12 𝑟13
𝑹 = 𝑟Ԧ1 𝑟Ԧ2 𝑟Ԧ3 = 𝑟21 𝑟22 𝑟23
𝑟31 𝑟32 𝑟33

▪ Some authors prefer to write the matrix that maps from the Body-fixed coordinates
to the Earth’s coordinates; others prefer the matrix that maps from the Earth’s
coordinates to the Body-fixed coordinates.
11
▪ We define the RM that encodes the attitude of a rigid body to be the matrix that
when pre-multiplied by a vector, expressed in the Earth’s CS, yields the same vector,
expressed in the Body-fixed CS (i.e. from E to B):

𝐵𝑧Ԧ = 𝑹 ∙ 𝐸𝑧Ԧ 𝐸 𝑧Ԧ = 𝑹𝑇 ∙ 𝐵𝑧Ԧ

𝐸 𝑧Ԧ ∈ ℝ𝟑 is a vector in the Earth’s CS and

∈ ℝ𝟑 is the same vector expressed in the
𝐵𝑧Ԧ
Body-fixed CS.

12
Check Point
▪ Let’s apply the RM 𝑹𝑍 for transforming the vector
= 3 2 0 𝑇 to the Body CS if angle 𝛼 between
𝐸 𝑟Ԧ
CSs is equal to 60 deg.

𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
0 0 1

𝐵𝑟Ԧ = 𝑹 𝐸 𝑟Ԧ

▪ Let’s make the reverse transformation. We know the

vector 𝐵𝑟Ԧ = 4 1 0 𝑇 and want to convert it to
the Earth’s CS. Relation between CSs is the same.

13
If 𝐸 𝑷 ∈ ℝ𝟑 is a point in the Earth’s CS and 𝐵𝑷 ∈ ℝ𝟑 is
the same point expressed in the Body-fixed CS, then:

𝐵𝑷 =𝑹 𝐸 𝑷 − 𝐸 𝑶2 = 𝑹 𝐸𝑷 + 𝐵𝑶1

𝐸𝑷 = 𝑹𝑇 𝐵𝑷 − 𝐵𝑶1 = 𝑹𝑇 𝐵𝑷 + 𝐸𝑶2

Substituting 𝐸𝑷 = 0 and 𝐵𝑷 = 0 to the equations

yields:

𝐵 𝑶1 = −𝑹 𝐸𝑶2

𝐸 𝑶2 = −𝑹𝑇 𝐵𝑶1

14
1.3
▪ A RM may also be referred to as a Direction Cosine Matrix (DCM), because the
elements of this matrix are the cosines of the unsigned angles between the Body-
fixed axes and the Earth’s axes.

▪ Denote the Earth’s CS axes by 𝑥, 𝑦, 𝑧 and the Body-fixed CS axes by 𝑥 ′ , 𝑦 ′ , 𝑧 ′ .

▪ Let 𝜃𝑥 ′ ,𝑦 be, for example, the unsigned angle between the 𝑥 ′ -axis and the 𝑦-axis.
Then the RM can be written in a following way:

𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑧

𝑅𝑋 𝛼 = 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑧
𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑧

16
▪ Consider the situation where 𝜃𝑥 ′ ,𝑥 = 𝜃𝑦 ′ ,𝑦 = 𝛼.
𝜋 𝜋
▪ Then 𝜃𝑥 ′ ,𝑦 = − 𝛼 and 𝜃𝑦 ′ ,𝑥 = + 𝛼; 𝜃𝑧 ′ ,𝑧 = 0.
2 2
𝜋
▪ 𝜃𝑧 ′ , 𝑥,𝑦 = 𝜃 𝑥 ′ ,𝑦 ′ ,𝑧 = . Then RM is written as:
2

𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑧

𝑅𝑍 𝛼 = 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑧 =
𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑧

𝜋
𝑐𝑜𝑠 𝛼 𝑐𝑜𝑠 −𝛼 0
2 𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
= 𝜋 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
𝑐𝑜𝑠 +𝛼 𝑐𝑜𝑠 𝛼 0
2 0 0 1
0 0 1
17
▪ The rows of the RM are the basis vectors of the Body-
fixed CS expressed in the Earth’s CS;

▪ The columns are the basis vectors of the Earth’s CS

expressed in the Body-fixed CS.

𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑥 ′ ,𝑧

𝑅= 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑦 ′ ,𝑧
𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑥 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑦 𝑐𝑜𝑠 𝜃𝑧 ′ ,𝑧

18
▪ The multiplication of two rotation matrices yields another RM whose application to a
point effects the same rotation as the sequential application of the two original RMs.
▪ Suppose, at first, we perform rotation of arbitrary vector 𝑧Ԧ from the initial (Earth’s)
CS to CS named A:
𝐴𝑧Ԧ = 𝑹𝐴 𝐸𝑧Ԧ
▪ Then, at the second step, we perform one more rotation of vector 𝑧Ԧ from A CS to
CS named B:

𝐵𝑧Ԧ = 𝑹𝐵/𝐴 𝐴𝑧Ԧ = 𝑹𝐵/𝐴 𝑹𝐴 𝐸𝑧Ԧ = 𝑹𝐵 𝐸𝑧Ԧ where 𝑹𝐵 = 𝑹𝐵/𝐴 𝑹𝐴

▪ The resulting rotation of 𝑧Ԧ from initial CS to B CS can be represented with a single RM!

▪ Note that the rotations are applied in the reverse order, from right to left!
19
Check Point
▪ It is known that the Body CS can be obtain from the
Earth’s CS by, at first, the rotation about Z axis on
45 deg., and then by the rotation about the X axis
on 30 deg.

▪ Let’s transform the vector 𝐸 𝑟Ԧ = 3 2 0 𝑇 to the

Body CS.
▪ Remember that 𝑹𝐵 = 𝑹𝐵/𝐴 𝑹𝐴 and

1 0 0 𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
𝑹𝑋 𝛼 = 0 𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
0 −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0 0 1

20
1.4
▪ 3 coordinate rotations in sequence can describe any rotation:

▪ The first rotation is an angle 𝜓 about the k-axis;

▪ The second rotation is an angle 𝜃 about the j-axis;
▪ The third rotation is an angle 𝜑 about the i-axis.

▪ These angles are usually arranged in a 3D-vector called the Euler angle vector:

𝑢 = 𝜑, 𝜃, 𝜓 𝑇

▪ The function that maps an Euler angle vector to its corresponding RM, 𝑹𝑖𝑗𝑘 𝑢 , is:

𝑹𝑖𝑗𝑘 𝑢 = 𝑹𝑖 𝜑 ∙ 𝑹𝑗 𝜃 ∙ 𝑹𝑘 𝜓

22
Check Point

▪ Let’s find the RM that corresponds to the Euler Angles vector 𝑢 = 300 , 600 , 450 𝑇 .

▪ Remember that 𝑢 = 𝜑, 𝜃, 𝜓 𝑇 and 𝑹𝑖𝑗𝑘 𝑢 = 𝑹𝑖 𝜑 ∙ 𝑹𝑗 𝜃 ∙ 𝑹𝑘 𝜓 .

23
▪ For an arbitrary vector the following relations hold:

𝑇
𝐵𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐸𝑧,
Ԧ 𝐸 𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐵𝑧Ԧ

▪ In fact, there are the 27 possible rotation sequences (that follows from the possible
combinations of three integer numbers {1; 2; 3}); but there are only 12 that satisfy
the constraint that no two consecutive numbers in a valid sequence may be equal:

𝑖, 𝑗, 𝑘 ∈ ሼ 𝑋, 𝑌, 𝑋 , 𝑿, 𝒀, 𝒁 , 𝑋, 𝑍, 𝑋 , 𝑋, 𝑍, 𝑌 ,

𝑌, 𝑋, 𝑌 , 𝑌, 𝑋, 𝑍 , 𝑌, 𝑍, 𝑋 , 𝑌, 𝑍, 𝑌 ,

𝑍, 𝑋, 𝑌 , 𝒁, 𝑿, 𝒁 , 𝑍, 𝑌, 𝑋 , 𝒁, 𝒀, 𝒁 ሽ

24
1.4

Rotation Sequence ZXZ (313)

▪ The most common sequence associated with
the name Euler angles.

▪ It is also known as the X-convention.

▪ In the study of the gyroscopic motion of a

spinning rigid body, the Euler angles, 𝜑, 𝜃 and
𝜓, are known respectively as Spin, Nutation
and Precession.

26
27
▪ The function that maps a vector of Euler angles to its RM:

𝑐𝜑 𝑐𝜓 − 𝑠𝜑 𝑐𝜃 𝑠𝜓 𝑐𝜑 𝑠𝜓 + 𝑠𝜑 𝑐𝜃 𝑐𝜓 𝑠𝜑 𝑠𝜃
𝑹𝑍𝑋𝑍 𝑢 = 𝑹𝑍 𝜑 ∙ 𝑹𝑋 𝜃 ∙ 𝑹𝑍 𝜓 = −𝑠𝜑 𝑐𝜓 − 𝑐𝜑 𝑐𝜃 𝑠𝜓 −𝑠𝜑 𝑠𝜓 + 𝑐𝜑 𝑐𝜃 𝑐𝜓 𝑐𝜑 𝑠𝜃
𝑠𝜃 𝑠𝜓 −𝑠𝜃 𝑐𝜓 𝑐𝜃

▪ The inverse mapping gives

𝜑𝑍𝑋𝑍 𝑹𝑍𝑋𝑍 𝑎𝑡𝑎𝑛2 𝑟13 , 𝑟23

𝑢 𝑹𝑍𝑋𝑍 = 𝜃𝑍𝑋𝑍 𝑹𝑍𝑋𝑍 = 𝑎𝑐𝑜𝑠 𝑟33
𝜓𝑍𝑋𝑍 𝑹𝑍𝑋𝑍 𝑎𝑡𝑎𝑛2 𝑟31 , −𝑟32

28
▪ The function that maps a vector of Euler angles to rotation quaternion:

𝑐𝜑/2 𝑐𝜃/2 𝑐𝜓/2 − 𝑠𝜑/2 𝑐𝜃/2 𝑠𝜓/2

𝑐𝜑/2 𝑠𝜃/2 𝑐𝜓/2 + 𝑠𝜑/2 𝑠𝜃/2 𝑠𝜓/2
𝒒𝑍𝑋𝑍 𝑢 = 𝑐𝜑/2 𝑠𝜃/2 𝑠𝜓/2 − 𝑠𝜑/2 𝑠𝜃/2 𝑐𝜓/2
𝑐𝜑/2 𝑐𝜃/2 𝑠𝜓/2 + 𝑠𝜑/2 𝑐𝜃/2 𝑐𝜓/2

▪ The inverse mapping, from rotation quaternion to Euler angles, gives:

𝑎𝑡𝑎𝑛2 2𝑞1 𝑞3 − 2𝑞0 𝑞2 , 2𝑞2 𝑞3 + 2𝑞0 𝑞1

𝑢𝑍𝑋𝑍 𝑹 𝒒 = 𝑎𝑐𝑜𝑠 𝑞32 − 𝑞22 −𝑞12 +𝑞02
𝑎𝑡𝑎𝑛2 2𝑞1 𝑞3 + 2𝑞0 𝑞2 , −2𝑞2 𝑞3 + 2𝑞0 𝑞1

29
▪ (from "2-argument arctangent") is defined as the angle in the Euclidean plane, given
in radians, between the positive X-axis and the ray to the point (x, y) ≠ (0,0):

𝑎𝑡𝑎𝑛 𝑦/𝑥 𝑖𝑓 𝑥 > 0

𝑎𝑡𝑎𝑛2 𝑦, 𝑥 = ൞𝑎𝑡𝑎𝑛 𝑦/𝑥 − 𝜋 𝑖𝑓 𝑥 < 0 𝑎𝑛𝑑 𝑦 < 0
𝑎𝑡𝑎𝑛 𝑦/𝑥 + 𝜋 𝑖𝑓 𝑥 < 0 𝑎𝑛𝑑 𝑦 > 0

▪ The derivatives:

𝜕𝑎𝑡𝑎𝑛2 𝑦, 𝑥 𝑥 𝜕𝑎𝑠𝑖𝑛 𝑥 1
= 2 =
𝜕𝑦 𝑥 + 𝑦2 𝜕𝑥 1 − 𝑥2

𝜕𝑎𝑡𝑎𝑛2 𝑦, 𝑥 −𝑦 𝜕𝑎𝑐𝑜𝑠 𝑥 −1
= 2 =
𝜕𝑥 𝑥 + 𝑦2 𝜕𝑥 1 − 𝑥2
30
1.4

Rotation Sequence XYZ (123)

▪ The angles associated with the sequence XYZ are sometimes called:
▪ Cardan angles, for Gerolamo Cardano, an Italian Renaissance mathematician;

▪ Tait-Bryan angles, for Peter Guthrie Tait, a 19th-century Scottish mathematical

physicist;
▪ or nautical angles;

▪ simply as Euler angles in the aeronautics field (despite the lack of consensus on
the issue).

▪ They are commonly used in aerospace engineering and computer graphics.

32
▪ These angles are also commonly referred to
simply as Euler angles in the aeronautics
field;

▪ Angles are commonly known respectively as

Roll, Pitch, and Yaw, or, equivalently, Bank,
Attitude, and Heading.

33
34
▪ ECEF states for Earth-Centered, Earth-Fixed CS – geographical and Cartesian CS.
35
▪ The function that maps a vector of Euler angles to its RM is:

𝑐𝜃 𝑐𝜓 𝑐𝜃 𝑠𝜓 −𝑠𝜃
𝑹𝑋𝑌𝑍 𝑢 = 𝑹𝑋 𝜑 ∙ 𝑹𝑌 𝜃 ∙ 𝑹𝑍 𝜓 = 𝑠𝜑 𝑠𝜃 𝑐𝜓 − 𝑐𝜑 𝑠𝜓 𝑠𝜑 𝑠𝜃 𝑠𝜓 + 𝑐𝜑 𝑐𝜓 𝑠𝜑 𝑐𝜃
𝑐𝜑 𝑠𝜃 𝑐𝜓 + 𝑠𝜑 𝑠𝜓 𝑐𝜑 𝑠𝜃 𝑠𝜓 − 𝑠𝜑 𝑐𝜓 𝑐𝜑 𝑐𝜃

▪ The inverse mapping gives

𝜑𝑋𝑌𝑍 𝑹𝑋𝑌𝑍 𝑎𝑡𝑎𝑛2 𝑟23 , 𝑟33

𝑢 𝑹𝑋𝑌𝑍 = 𝜃𝑋𝑌𝑍 𝑹𝑋𝑌𝑍 = −𝑎𝑠𝑖𝑛 𝑟13
𝜓𝑋𝑌𝑍 𝑹𝑋𝑌𝑍 𝑎𝑡𝑎𝑛2 𝑟12 , 𝑟11

36
Check Point
▪ Let’s find the XYZ Euler Angles vector that corresponds
to the following RM:

0.5 −0.1464 0.8536

𝑹𝑋𝑌𝑍 = 0.5 0.8536 −0.1464
−0.7071 0.5 0.5

▪ Remember that

𝑎𝑡𝑎𝑛 𝑦/𝑥 𝑖𝑓 𝑥 > 0

𝑎𝑡𝑎𝑛2 𝑦, 𝑥 = ൞𝑎𝑡𝑎𝑛 𝑦/𝑥 − 𝜋 𝑖𝑓 𝑥 < 0 𝑎𝑛𝑑 𝑦 < 0
𝑎𝑡𝑎𝑛 𝑦/𝑥 + 𝜋 𝑖𝑓 𝑥 < 0 𝑎𝑛𝑑 𝑦 > 0

37
▪ The function that maps a vector of Euler angles to rotation quaternion:

𝑐𝜑/2 𝑐𝜃/2 𝑐𝜓/2 + 𝑠𝜑/2 𝑠𝜃/2 𝑠𝜓/2

−𝑐𝜑/2 𝑠𝜃/2 𝑠𝜓/2 + 𝑠𝜑/2 𝑐𝜃/2 𝑐𝜓/2
𝒒𝑋𝑌𝑍 𝑢 =
𝑐𝜑/2 𝑠𝜃/2 𝑐𝜓/2 + 𝑠𝜑/2 𝑐𝜃/2 𝑠𝜓/2
𝑐𝜑/2 𝑐𝜃/2 𝑠𝜓/2 − 𝑠𝜑/2 𝑠𝜃/2 𝑐𝜓/2

▪ The inverse mapping, from rotation quaternion to Euler angles, gives:

𝑎𝑡𝑎𝑛2 2𝑞2 𝑞3 + 2𝑞0 𝑞1 , 𝑞32 −𝑞22 −𝑞12 +𝑞02

𝑢𝑋𝑌𝑍 𝑹 𝒒 = −𝑎𝑠𝑖𝑛 2𝑞1 𝑞3 − 2𝑞0 𝑞2
𝑎𝑡𝑎𝑛2 2𝑞1 𝑞2 + 2𝑞0 𝑞3 , 𝑞12 +𝑞02 − 𝑞22 −𝑞32

38
1.4

Gimbal lock
▪ This parametrization has singularities at Pitch
𝜋
values of 𝜃 = + 𝑛𝜋 for 𝑛 ∈ ℤ. At these points,
2
changes in Roll and Yaw constitute the same
motion.

▪ Thus, it is only suitable for describing vehicles

that do not perform vertical or inverted
maneuvers, such as land vehicles, boats and
ships, and transport aircrafts.

▪ Mathematically it can be seen by 1/cos(𝜃)

coefficient in atan2-functions:
𝑐𝜃 𝑐𝜓 𝑐𝜃 𝑠𝜓 −𝑠𝜃 𝑎𝑡𝑎𝑛2 𝑟23 , 𝑟33
𝑹𝑋𝑌𝑍 𝑢 = 𝑠𝜑 𝑠𝜃 𝑐𝜓 − 𝑐𝜑 𝑠𝜓 𝑠𝜑 𝑠𝜃 𝑠𝜓 + 𝑐𝜑 𝑐𝜓 𝑠𝜑 𝑐𝜃 𝑢 𝑹𝑋𝑌𝑍 = −𝑎𝑠𝑖𝑛 𝑟13
𝑐𝜑 𝑠𝜃 𝑐𝜓 + 𝑠𝜑 𝑠𝜓 𝑐𝜑 𝑠𝜃 𝑠𝜓 − 𝑠𝜑 𝑐𝜓 𝑐𝜑 𝑐𝜃 𝑎𝑡𝑎𝑛2 𝑟12 , 𝑟11
40
▪ This rotation scheme has singularities at Nutation values of
𝜃 = 𝑛𝜋 for 𝑛 ∈ ℤ. At these points, changes in Spin and
Precession constitute the same motion.

▪ Thus, there exists a singularity at the home position, when

𝜑, 𝜃, 𝜓 = 0, 0, 0 .

▪ Mathematically it can be seen by 1/sin(𝜃) coefficient in

atan2-functions:
𝑐𝜑 𝑐𝜓 − 𝑠𝜑 𝑐𝜃 𝑠𝜓 𝑐𝜑 𝑠𝜓 + 𝑠𝜑 𝑐𝜃 𝑐𝜓 𝑠𝜑 𝑠𝜃 𝑎𝑡𝑎𝑛2 𝑟13 , 𝑟23
𝑹𝑍𝑋𝑍 𝑢 = −𝑠𝜑 𝑐𝜓 − 𝑐𝜑 𝑐𝜃 𝑠𝜓 −𝑠𝜑 𝑠𝜓 + 𝑐𝜑 𝑐𝜃 𝑐𝜓 𝑐𝜑 𝑠𝜃 𝑢 𝑹𝑍𝑋𝑍 = 𝑎𝑐𝑜𝑠 𝑟33
𝑠𝜃 𝑠𝜓 −𝑠𝜃 𝑐𝜓 𝑐𝜃 𝑎𝑡𝑎𝑛2 𝑟31 , −𝑟32
41
42
▪ Obtaining a set of ZXZ (313) angles from XYZ (123) Euler angles:

𝜑𝑍𝑋𝑍 𝑹𝑋𝑌𝑍 𝑎𝑡𝑎𝑛2 −𝑠𝜃 , 𝑠𝜑 𝑐𝜃

𝑢𝑍𝑋𝑍 𝑹𝑋𝑌𝑍 = 𝜃𝑍𝑋𝑍 𝑹𝑋𝑌𝑍 = 𝑎𝑐𝑜𝑠 𝑐𝜑 𝑐𝜃
𝜓𝑍𝑋𝑍 𝑹𝑋𝑌𝑍 𝑎𝑡𝑎𝑛2 𝑐𝜑 𝑠𝜃 𝑐𝜓 + 𝑠𝜑 𝑠𝜓 , −𝑐𝜑 𝑠𝜃 𝑠𝜓 + 𝑠𝜑 𝑐𝜓

▪ Obtaining a set of XYZ (123) angles from ZXZ (313) Euler angles:

𝜑𝑋𝑌𝑍 𝑹𝑍𝑋𝑍 𝑎𝑡𝑎𝑛2 𝑐𝜑 𝑠𝜃 , 𝑐𝜃

𝑢𝑋𝑌𝑍 𝑹𝑍𝑋𝑍 = 𝜃𝑋𝑌𝑍 𝑹𝑍𝑋𝑍 = −𝑎𝑠𝑖𝑛 𝑠𝜑 𝑠𝜃
𝜓𝑋𝑌𝑍 𝑹𝑍𝑋𝑍 𝑎𝑡𝑎𝑛2 𝑐𝜑 𝑠𝜓 + 𝑠𝜑 𝑐𝜃 𝑐𝜓 , 𝑐𝜑 𝑐𝜓 − 𝑠𝜑 𝑐𝜃 𝑠𝜓

43
1.5
▪ Quaternions were first devised by William Rowan
Hamilton, a 19th-century Irish mathematician.

▪ Complex numbers have the form a+ib where a and b

are real numbers; i2 = -1 (or ‘j’).

▪ Complex numbers define a plane – the complex plane

– where one axis is real and the other imaginary.

45
▪ By definition quaternion is determined as:

𝐪 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3

where 𝑞0 is a quaternion scalar part, while 𝑞Ԧ1:3 =

𝑞1 𝑞2 𝑞3 𝑇 denotes the quaternion vector part.

𝑞0
𝐪 = 𝑞0 𝑞1 𝑞2 𝑞3 𝑇
=
𝑞Ԧ1:3
▪ Basis vectors and Imaginary units:

𝑖 2 = 𝑗 2 = 𝑘 2 = −1

▪ Cyclic permutations:

𝑖𝑗 = 𝑘 𝑗𝑖 = −𝑘 𝑖→𝑗→𝑘→𝑖
46
▪ Quaternion definition: 𝐪 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3

▪ Conjugate Quaternion: ഥ = 𝑞0 − 𝑖𝑞1 − 𝑗𝑞2 − 𝑘𝑞3

𝐪

▪ Quaternion norm: 𝐪 = 𝑞02 + 𝑞12 + 𝑞22 + 𝑞32

ഥ
𝐪
▪ Inverse quaternion: 𝐪−1 =
𝐪 2

▪ Unit quaternion: 𝐪 =1

▪ Inverse of Unit quaternion: 𝐪−1 = 𝐪

ഥ

47
Check Point
▪ Suppose we have two quaternions 𝐪𝐴 = 2 3 1 −1 𝑇 and 𝐪𝐵 = 4 5 −2 6 𝑇
.
Let’s write them in the form with basis vectors, i.e. 𝐪 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3 .

▪ Calculate the 𝐪𝐶 = 𝐪𝐴 + 𝐪𝐵 and 𝐪𝐷 = 𝐪𝐴 − 𝐪𝐵

▪ Evaluate the 𝐪
ഥ𝐴 , 𝐪
ഥ𝐵 , 𝐪𝐴 , 𝐪𝐵 , and 𝐪
ഥ𝐴 , 𝐪
ഥ𝐵 .

▪ Calculate the 𝐪𝐸 = 1/𝐪𝐴 and 𝐪𝐹 = 1/𝐪𝐵

48
▪ We can define rotation axis as a vector in 3D CS (called A)

𝐴𝑟Ԧ = 𝑟𝑋 , 𝑟𝑌 , 𝑟𝑍

and rotation angle as 𝜃 around this vector 𝐴𝑟.

Ԧ

▪ Then corresponding quaternion for rotation the CS A to CS

B is defined as:

𝐪 = cos 𝜃 Τ2 + 𝑖𝑟𝑋 sin 𝜃 Τ2 + 𝑗𝑟𝑌 sin 𝜃 Τ2 + 𝑘𝑟𝑍 sin 𝜃 Τ2 =

= 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3

𝑟𝑌 sin 𝜃 Τ2 𝑟𝑍 sin 𝜃 Τ2 𝑇
𝐪 = cos 𝜃 Τ2 𝑟𝑋 sin 𝜃 Τ2
49
Check Point
▪ In one of the first exercises we proved that
in order to rotate a vector 𝐸 𝑟Ԧ to the Body
CS according to the figure conditions
(about Z axis), we can use RM 𝑹𝑍 :

𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
0 0 1

▪ Let’s define a quaternion that corresponds to

the following rotation of the Earth’s CS to the
Body CS, if 𝛼 is equal to 60 deg.

50
▪ Suppose we have two quaternions:

𝐩 = 𝑝0 + 𝑖𝑝1 + 𝑗𝑝2 + 𝑘𝑝3 = 𝑝0 𝑝1 𝑝2 𝑝3 𝑇

𝐪 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3 = 𝑞0 𝑞1 𝑞2 𝑞3 𝑇

▪ Then their product is calculated as:

𝒑 × 𝒒 = 𝑝0 𝑝1 𝑝2 𝑝3 𝑇 × 𝑞0 𝑞1 𝑞2 𝑞3 𝑇 =

𝑝0 𝑞0 + 𝑝1 𝑞1 − 𝑝2 𝑞2 − 𝑝3 𝑞3
𝑝0 𝑞1 + 𝑝1 𝑞0 + 𝑝2 𝑞3 + 𝑝3 𝑞2
= 𝑝0 𝑞2 − 𝑝1 𝑞3 + 𝑝2 𝑞0 + 𝑝3 𝑞1
𝑝0 𝑞3 + 𝑝1 𝑞2 − 𝑝2 𝑞1 + 𝑝3 𝑞0

51
Check Point
▪ Let’s check the correctness of the formula for two quaternions multiplication. Does it
really look like this?

𝑝0 𝑞0 + 𝑝1 𝑞1 − 𝑝2 𝑞2 − 𝑝3 𝑞3
𝑝0 𝑞1 + 𝑝1 𝑞0 + 𝑝2 𝑞3 + 𝑝3 𝑞2
𝒑×𝒒= 𝑝0 𝑞2 − 𝑝1 𝑞3 + 𝑝2 𝑞0 + 𝑝3 𝑞1
𝑝0 𝑞3 + 𝑝1 𝑞2 − 𝑝2 𝑞1 + 𝑝3 𝑞0

52
▪ The product of two quaternions can be also written as:

ഥ 𝒒 ∙𝒑
𝒑×𝒒=𝑸 𝒑 ∙𝒒= 𝑸

where 𝑸 𝒑 denotes the quaternion matrix function defined as:

𝑝0 −𝑝1 −𝑝2 −𝑝3

−𝑝3 𝑝2 𝑇
𝑝1 𝑝0 𝑝0 −𝒑1:3
𝑸 𝒑 = 𝑝2 𝑝3 𝑝0 −𝑝1 =
𝒑1:3 𝑝0 𝐼3𝑥3 + 𝑪 𝒑1:3
𝑝3 −𝑝2 𝑝1 𝑝0

where 𝑪 𝑥Ԧ is the skew-symmetric cross product matrix function:

0 −𝑥2 𝑥1
𝑪 𝑥Ԧ = 𝑥2 0 −𝑥0
−𝑥1 𝑥0 0
53
▪ The product of two quaternions represents the composite rotation (remember the
product of two RMs):
𝒒⅀ = 𝒒2 × 𝒒1

▪ The rotations are performed from right to the left, i.e. at first, we perform the
rotation described by 𝒒1 quaternion, next the rotation described by 𝒒2 -quaternion.

𝒒1 𝒒2 𝒒3 = 𝒒1 𝒒2 𝒒3
𝒒1 + 𝒒2 𝒒3 = 𝒒1 𝒒3 + 𝒒2 𝒒3
𝒒1 𝒒2 + 𝒒3 = 𝒒1 𝒒2 + 𝒒1 𝒒3
𝛼 𝒒2 + 𝒒3 = 𝛼𝒒1 + 𝛼𝒒2
𝛼 is a scalar
𝛼𝒒1 𝒒2 = 𝛼 𝒒1 𝒒2 = 𝒒1 𝛼𝒒2
54
▪ 3D rotations are not commute!
𝒒⅀ = 𝒒2 × 𝒒1

𝒒1 × 𝒒2 ≠ 𝒒2 × 𝒒1

Roll, then Yaw Rotations Yaw, then Roll Rotations 55

Check Point
▪ Let’s define how should we set up CS rotations based on quaternions for combining
the Sensor CS and Body CS:

56
▪ Let’s take a vector: 𝐸 𝑟Ԧ = 𝑟𝑋 , 𝑟𝑌 , 𝑟𝑍 𝑇

▪ We want to rotate it and the rotation is described by a unit quaternion:

𝒒 =1 or 𝒒ഥ
𝒒=1

▪ In order to calculate the rotated vector, let’s create a quaternion from 𝐸 𝑟:

Ԧ
𝑻 𝑇
𝐸 𝒓 = 0, 𝐸 𝑟Ԧ = 0, 𝑟𝑋 , 𝑟𝑌 , 𝑟𝑍

▪ Then the rotation can be made as (where 𝐵𝒓 denotes the rotated vector quaternion):

𝑻 1 𝟎𝑻 0
𝐵𝒓 = 𝒒 ∙ 𝐸 𝒓 ∙ 𝒒−𝟏 = 0, 𝐵𝑟Ԧ =
𝟎 𝑹 𝒒 𝐸 𝑟Ԧ

57
Check Point

▪ And now let’s transform the vector 𝐸 𝑟Ԧ =

3 2 0 𝑇 to the Body CS if angle 𝛼 between
CSs is equal to 60 deg., but using quaternions
and after that compare the resulting vector
component values with that one obtained by
application of RM.

58
▪ When we have a vector 𝐸 𝑧Ԧ and want to convert it to the body-fixed CS, we can
apply corresponding RM:
𝑇
𝐵𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐸𝑧,
Ԧ 𝐸 𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐸𝑧Ԧ

▪ But what if we don’t have RM but have the quaternion which describes the relation
between the Earth’s CS and the Body-fixed CS. Then we can get RM from quaternion:

2 𝑞02 + 𝑞12 − 1 2 𝑞1 𝑞2 + 𝑞0 𝑞3 2 𝑞1 𝑞3 − 𝑞0 𝑞2
𝑹𝑖𝑗𝑘 𝒒 = 2 𝑞1 𝑞2 − 𝑞0 𝑞3 2 𝑞02 + 𝑞22 − 1 2 𝑞2 𝑞3 + 𝑞0 𝑞1
2 𝑞1 𝑞3 + 𝑞0 𝑞2 2 𝑞1 𝑞3 − 𝑞0 𝑞2 2 𝑞02 + 𝑞32 − 1
▪ Then apply:
𝑇
𝐵𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐸𝑧,
Ԧ 𝐸 𝑧Ԧ = 𝑹𝑖𝑗𝑘 𝑢 ∙ 𝐸𝑧Ԧ
59
Check Point
▪ Let’s check the quaternion that we have
recently obtained while solving the task.
Recalculate it to the RM and compare its
values with the initial RM 𝑹𝑍 :

𝑐𝑜𝑠 𝛼 𝑠𝑖𝑛 𝛼 0
𝑹𝑍 𝛼 = −𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛼 0
0 0 1

if 𝛼 is equal to 60 deg.

60
1.6
▪ If we have two vectors:
𝑇 𝑇
𝜔 = 𝜔𝑋 , 𝜔𝑌 , 𝜔𝑍 𝑟Ԧ = 𝑟𝑋 , 𝑟𝑌 , 𝑟𝑍

then the vector product can be written as :

0 −𝜔𝑍 𝜔𝑌
𝜔 × 𝑟Ԧ = 𝑪 𝜔 𝑟Ԧ = 𝜔𝑍 0 −𝜔𝑋 𝑟Ԧ
−𝜔𝑌 𝜔𝑋 0

where 𝑪 𝜔 denotes a skew-symmetric matrix.

62
▪ There is a relation of quaternion rates and angular velocity:

𝜔 = 𝜔𝑋 , 𝜔𝑌 , 𝜔𝑍 𝑇 𝒒 = 𝑞0 , 𝑞1 , 𝑞2 , 𝑞3 𝑇

1 0 1 0 1
𝒒ሶ 𝜔 = 𝒒× = 𝑸 𝒒 = 𝑾 𝒒 𝑇𝜔
2 𝜔 2 𝜔 2

where 𝑾 𝒒 denotes a quaternion rates matrix:

−𝑞1 𝑞0 −𝑞3 𝑞2
𝑾 𝒒 = −𝑞2 𝑞3 𝑞0 −𝑞1
−𝑞3 −𝑞2 𝑞1 𝑞0

63
▪ There is a reversed relation of angular velocity and quaternion rates:

𝜔 = 𝜔𝑋 , 𝜔𝑌 , 𝜔𝑍 𝑇 𝒒 = 𝑞0 , 𝑞1 , 𝑞2 , 𝑞3 𝑇

0
𝒒 × 𝒒ሶ 𝜔 = 2𝑸 𝒒 𝑇 𝒒ሶ 𝜔
= 2ഥ
𝜔

▪ Or more compact:

𝜔 = 2𝑾 𝒒 𝒒ሶ 𝜔

64
▪ If Point p is rigidly attached to the Body, b, i.e. 𝑥Ԧሶ 𝑝/𝑏 = 𝑥Ԧሷ 𝑝/𝑏 = 0, then the velocity of
the Point is calculated as:

ሶ = 𝑥Ԧሶ + 𝜔 × 𝑥Ԧ Ԧሶ 𝑏 + 𝑪( 𝐸𝜔) ∙ 𝐸𝑥Ԧ𝑝/𝑏

𝐸𝑥
Ԧ𝑝 𝐸 𝑏 𝐸 𝐸 𝑝/𝑏 = 𝐸 𝑥

ሶ = 𝑥Ԧሶ + 𝑹𝑇 × 𝐵𝑥Ԧ𝑝/𝑏 = 𝐸𝑥Ԧሶ 𝑏 + 𝑹𝑇 ∙ 𝑪( 𝐵𝜔) ∙ 𝐵𝑥Ԧ𝑝/𝑏

𝐸𝑥
Ԧ𝑝 𝐸 𝑏 𝐵𝜔

where 𝐸 𝜔 and 𝐵𝜔 denotes the angular velocity of Body,

𝐸𝑥
Ԧ𝑝/𝑏 and 𝐵𝑥
Ԧ𝑝/𝑏 are the position of Point relative to Body.

65
▪ If Point p is NOT rigidly attached to the Body, b, but moving relative to it i.e.
𝑥Ԧሶ 𝑝/𝑏 ≠ 𝑥Ԧሷ 𝑝/𝑏 ≠ 0, then the velocity of the Point is calculated as:

ሶ = 𝑥Ԧሶ + 𝑥Ԧሶ Ԧ𝑝/𝑏 = 𝐸𝑥Ԧሶ 𝑏 + 𝑪( 𝐸𝜔) ∙ 𝐸𝑥Ԧ𝑝/𝑏

𝐸𝑥
Ԧ𝑝 𝐸 𝑏 𝐸 𝑝/𝑏 + 𝐸 𝜔 × 𝐸 𝑥

ሶ = 𝑥Ԧሶ + 𝑹𝑇 ሶ + 𝐵𝜔 × 𝐵𝑥Ԧ𝑝/𝑏 = 𝐸𝑥Ԧሶ 𝑏 + 𝑹𝑇 ሶ

𝐸𝑥
Ԧ𝑝 𝐸 𝑏 𝐵𝑥
Ԧ𝑝/𝑏 𝐵𝑥
Ԧ𝑝/𝑏 + 𝑪( 𝐵𝜔) ∙ 𝐵𝑥Ԧ𝑝/𝑏

where 𝐸 𝜔 and 𝐵𝜔 denotes the angular velocity of Body,

𝐸𝑥
Ԧ𝑝/𝑏 and 𝐵𝑥
Ԧ𝑝/𝑏 are the position of Point relative to Body.

66