orientation.rst

From reference points to an orientation matrix

In geometry the orientation ( also: angular position, or attitude ) of an object is part of the description of how it is placed in the space it is in. Namely, it is the imaginary rotation that is needed to move the object from a reference placement to its current placement. For example for the head of an animal it is the description in which direction the animal is looking (foward), the direction from left to right (sideward), and the direction from top to bottom (downward). Each direction is mathematically represented by a unit vector. Furthormore those vector are orthogonal to each other, and formed a directly oriented base. To get an intuitive idea, use your right hand, thumbs toward a wall, the index parallel to the floor, and the major pointing downward. Your right hand form a directly orientaed orthogonal base :) .

The orientation of an agent is described by three vectors. Those vector can be obtained, for example, from the apexes of a triangle. Let's take an equilateral triangle for simplicity, but any triangle could work. All vectors will originate from the same points, so let choose one apex to be the origin, and mark it with a black point. The forward vector points toward the edge facing the origin, and is along the mediatrix, i.e. it crosses the the middle of the facing edge. The sideward is parrallel to the facing edge. The last vector is simply the cross product of the two other vectors.

This example illustrated how one can define the orientation of an agent from a triangle. A triangle being described by only three points in space. To know the orinentation of the head of an animal, one only need three points on its head, neat :).

For an aircraft, the forward, sideward, and downward vectors are nammed roll axis, pitch axis, and yaw axis, respectively. We will from now on stick to this convention.

Isocel triangle

Let's assume that we have one marker (mark0) centered on the center of mass of the agent. The two others (mark1 and mark2) are placed such that

• the markers form an isosceles triangle, here the two equal sides are

mark0-mark1 and mark0-mark2, * the median of the triangle, i.e. the vector going from mark0 to the middle of the segment between mark1 and mark2, is along the the roll axis

The yaw,pitch,roll axis are then calculated as:

• The roll_axis is along the median between the 2nd and 3rd apexes ( mark1 and mark2)
• The yaw_axis is the cross-product between the vector 1st appex to 2nd and 1st appex to 3rd apexe.
• The pitch_axis is the cross-product between the roll_axis and the opposite

of the yaw_axis

Pitch align triangle

However the markers may not be always correctly placed, therefore the local reference frames may differ a bit from the axis convention for aircraft. To compensate this error, one may calculate the rotation between the correct local reference frame and the estimated local reference frame by identifying other reference points, when the agent is at the null orientation. This procedure is sadly rarely plausible with insects. It is rather difficult to define unambigeously the local reference frame at null orientation. The experimentalist may trust more one of the axis of the triangle. For example less error can be done while placing markers aligned with the pitch axis on the head, because the pitch axis is aligned with axis connecting the two eyes.

When the pitch axis can be trusted, the yaw,pitch,roll axis are calculated as:

• The pitch_axis is the vector between the 2nd and the 3rd apexe
• The yaw_axis is the cross-product between the vector 1st appex to 2nd and 1st appex to 3rd apexe.
• The roll_axis is the cross-product between the pitch_axis and yaw_axis

Yaw, pitch, and roll rotations

A rotation is a circular movement of an object around a center (or point) of rotation . A three-dimensional object always rotates around an imaginary line called a rotation axis. In three dimensions, the orientation of an object is given by three rotations, i.e. by three rotation axis. Several conventions exist for defining the rotations. Here we will talk only about the ZYX convention, also known as yaw, pitch, and roll rotations.

(from: http://planning.cs.uiuc.edu/node102.html)

A 3D body can be rotated about three orthogonal axes, as shown in Figure 3.8. Borrowing aviation terminology, these rotations will be referred to as yaw, pitch, and roll:

A yaw is a counterclockwise rotation of \alpha about the z-axis. The rotation matrix is given by

R_z(\alpha) = \begin{pmatrix}\cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1  \end{pmatrix}

Note that the upper left entries of R_z(\alpha) form a 2D rotation applied to the x and y coordinates, whereas the z coordinate remains constant.

A pitch is a counterclockwise rotation of \beta about the y-axis. The rotation matrix is given by

R_y(\beta) = \begin{pmatrix}\cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta  \end{pmatrix}

A roll is a counterclockwise rotation of \gamma about the x-axis. The rotation matrix is given by

R_x(\gamma) = \begin{pmatrix}1 & 0 & 0 \\ 0 & \cos\gamma & -\sin\gamma \\ 0 & \sin\gamma & \cos\gamma  \end{pmatrix}

Each rotation matrix is a simple extension of the 2D rotation matrix, (3.31). For example, the yaw matrix, R_z(\alpha), essentially performs a 2D rotation with respect to the x and y coordinates while leaving the z coordinate unchanged. Thus, the third row and third column of R_z(\alpha) look like part of the identity matrix, while the upper right portion of R_z(\alpha) looks like the 2D rotation matrix. The yaw, pitch, and roll rotations can be used to place a 3D body in any orientation. A single rotation matrix can be formed by multiplying the yaw, pitch, and roll rotation matrices to obtain

\begin{split}
R(\alpha,& \beta,\gamma) = R_z(\alpha) \, R_y(\beta) \, R_x(\gamma) = \\
& \begin{pmatrix}
\cos\alpha \cos\beta &
\cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma &
\cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma \\
\sin\alpha \cos\beta &
\sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma &
\sin\alpha \sin\beta \cos\gamma - \cos\alpha \sin\gamma \\
-\sin\beta & \cos\beta \sin\gamma & \cos\beta \cos\gamma \\
\end{pmatrix}
\end{split}

It is important to note that R(\alpha,\beta,\gamma) performs the roll first, then the pitch, and finally the yaw. If the order of these operations is changed, a different rotation matrix would result. Be careful when interpreting the rotations. Consider the final rotation, a yaw by \alpha. Imagine sitting inside of a robot {\cal A} that looks like an aircraft. If \beta = \gamma = 0, then the yaw turns the plane in a way that feels like turning a car to the left. However, for arbitrary values of \beta and \gamma, the final rotation axis will not be vertically aligned with the aircraft because the aircraft is left in an unusual orientation before \alpha is applied. The yaw rotation occurs about the z-axis of the world frame, not the body frame of {\cal A}. Each time a new rotation matrix is introduced from the left, it has no concern for original body frame of {\cal A}. It simply rotates every point in {\mathbb{R}}^3 in terms of the world frame. Note that 3D rotations depend on three parameters, \alpha, \beta, and \gamma, whereas 2D rotations depend only on a single parameter, \theta `. The primitives of the model can be transformed using :math:`R(\alpha,\beta,\gamma), resulting in {\cal A}(\alpha,\beta,\gamma).

Orientation matrix

When introducing the body frame, we talked about an imaginary rotation of the reference frame to the animal's head frame. This is equivalent to finding the matrix R so that:

R.F^\text{ref} = F^\text{bee} \\
F^\text{ref} = \begin{pmatrix}
roll^\text{ref}_x & pitch^\text{ref}_x & yaw^\text{ref}_z \\
roll^\text{ref}_y & pitch^\text{ref}_y & yaw^\text{ref}_y \\
roll^\text{ref}_z & pitch^\text{ref}_z & yaw^\text{ref}_z \\
\end{pmatrix}

The linear algebra tells us that if the inverse of F^\text{ref} exists; the rotation matrix R`is equal to :math:`F^\text{bee}.\left(F^\text{ref}\right)^{-1}

The rotation matrix R has nine values, but we know that only three angles are necessary to know the orientation of the rigid body. So how can we have the yaw, pitch, and roll angles from the rotation matrix R?

Determining yaw, pitch, and roll from a rotation matrix

(adapted from http://planning.cs.uiuc.edu/node103.html)

It is often convenient to determine the \alpha, \beta, and \gamma parameters directly from a given rotation matrix. Suppose an arbitrary rotation matrix

\begin{pmatrix}r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\  r_{31} & r_{32} & r_{33}  \end{pmatrix}

is given. By setting each entry equal to its corresponding entry in (3.42), equations are obtained that must be solved for \alpha, \beta, and \gamma. Note that r_{21}/r_{11} = \tan\alpha and r_{32}/r_{33} = \tan \gamma. Also, r_{31} = - \sin\beta and \pm\sqrt{r^2_{32}+r^2_{33}} = \cos\beta. Solving for each angle yields

\displaystyle \alpha = \tan^{-1} (\pm r_{21}/\pm r_{11})

\beta = \tan^{-1} \Big(-r_{31} \big/ \pm\sqrt{r^2_{32}+r^2_{33}}\Big)

\gamma = \tan^{-1} (\pm r_{32}/\pm r_{33})

Note that the ambiguity on \pm`come from the sign of :math:cosbeta`, which is a priori unknown.

There is a choice of four quadrants for the inverse tangent functions. How can the correct quadrant be determined? Each quadrant should be chosen by using the signs of the numerator and denominator of the argument. The numerator sign selects whether the direction will be above or below the x -axis, and the denominator selects whether the direction will be to the left or right of the y -axis. This is the same as the \arctan_2 function in the C programming language, which nicely expands the range of the arctangent to [0,2 \pi). This can be applied to express (3.44), (3.45), and (3.46) as

\alpha = \arctan_2(\pm r_{21},\pm r_{11})

\beta = \arctan_2\Big(-r_{31},\pm\sqrt{r^2_{32}+r^2_{33}}\Big)

\gamma = \arctan_2(\pm  r_{32},\pm  r_{33})

Note that this method assumes r_{11} \not = 0 and r_{33} \not = 0.

Note that the choice of \pm can be determined by comparing the estimated orientation matrix from \alpha,:math:beta, and \gamma and the orientation matrix of the agent.

Axis convention for aircraft

The local reference frames is composed of three axis.

• X axis is the longitudinal axis pointing ahead
• Z axis is the vertical axis pointing downwards
• Y axis is the lateral one, pointing in such a way that the frame is right handed (from left to right when looking ahead)