Rotations, Orientations, and Quaternions for Automated Driving

A quaternion is a four-part hypercomplex number used to describe three-dimensional rotations and orientations. Quaternions have applications in many fields, including aerospace, computer graphics, and virtual reality. In automated driving, sensors such as inertial measurement units (IMUs) report orientation readings as quaternions. To use this data for localization, you can capture it using a quaternion object, perform mathematical operations on it, or convert it to other rotation formats, such as Euler angles and rotation matrices.

You can use quaternions to perform 3-D point and frame rotations.

• With point rotations, you rotate points in a static frame of reference.

• With frame rotations, you rotate the frame of reference around a static point to convert the frame into the coordinate system relative to the point.

You can define these rotations by using an axis of rotation and an angle of rotation about that axis. Quaternions encapsulate the axis and angle of rotation and have an algebra for manipulating these rotations. The quaternion object uses the "right-hand rule" convention to define rotations. That is, positive rotations are clockwise around the axis of rotation when viewed from the origin.

Quaternion Format

A quaternion number is represented in this form:

$$a+b\text{i}+c\text{j}+d\text{k}$$

a, b, c, and d are real numbers. These coefficients are known as the parts of the quaternion.

i, j, and k are the complex elements of a quaternion. These elements satisfy the equation i² = j² = k² = ijk = −1.

The quaternion parts a, b, c, and d specify the axis and angle of rotation. For a rotation of ɑ radians about a rotation axis represented by the unit vector [x, y, z], the quaternion describing the rotation is given by this equation:

$$\cos \left(\frac{\alpha }{2}\right)+\sin \left(\frac{\alpha }{2}\right)\left(xi+yj+zk\right)$$

Quaternion Creation

You can create quaternions in multiple ways. For example, create a quaternion by specifying its parts.

q = quaternion(1,2,3,4)

q = 

  quaternion


     1 + 2i + 3j + 4k

You can create arrays of quaternions in the same way. For example, create a 2-by-2 quaternion array by specifying four 2-by-2 matrices.

qArray = quaternion([1 10; -1 1], [2 20; -2 2], [3 30; -3 3], [4 40; -4 4])

qArray = 

  2x2 quaternion array


      1 +  2i +  3j +  4k     10 + 20i + 30j + 40k
     -1 -  2i -  3j -  4k      1 +  2i +  3j +  4k

You can also use four-column arrays to construct quaternions, where each column represents a quaternion part. For example, create an array of quaternions that represent random rotations.

qRandom = randrot(4,1)

qRandom = 

  4x1 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k
      0.21908 -  0.89875i -    0.298j +  0.23548k
       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.69704 - 0.060589i +  0.68679j -  0.19695k

Index and manipulate quaternions just like any other array. For example, index a quaternion from the qRandom quaternion array.

qRandom(3)

ans = 

  quaternion


      0.6375 + 0.49338i - 0.24049j + 0.54068k

Reshape the quaternion array.

reshape(qRandom,2,2)

ans = 

  2x2 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.21908 -  0.89875i -    0.298j +  0.23548k      0.69704 - 0.060589i +  0.68679j -  0.19695k

Concatenate the quaternion array with the first quaternion that you created.

[qRandom; q]

ans = 

  5x1 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k
      0.21908 -  0.89875i -    0.298j +  0.23548k
       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.69704 - 0.060589i +  0.68679j -  0.19695k
            1 +        2i +        3j +        4k

Quaternion Math

Quaternions have well-defined arithmetic operations. To apply these operations, first define two quaternions by specifying their real-number parts.

q1 = quaternion(1,2,3,4)

q1 = quaternion
     1 + 2i + 3j + 4k

q2 = quaternion(-5,6,-7,8)

q2 = quaternion
    -5 + 6i - 7j + 8k

Addition of quaternions is similar to complex numbers, where parts are added independently.

q1 + q2

ans = quaternion
     -4 +  8i -  4j + 12k

Subtraction of quaternions works similar to addition of quaternions.

q1 - q2

ans = quaternion
      6 -  4i + 10j -  4k

Because the complex elements of quaternions must satisfy the equation

$${\mathit{i}}^2={\mathit{j}}^2={\mathit{k}}^2=\mathit{ijk}=-1$$,

multiplication of quaternions is more complex than addition and subtraction. Given this requirement, multiplication of quaternions is not commutative. That is, when multiplying quaternions, reversing the order of the quaternions changes the result of their product.

q1 * q2

ans = quaternion
    -28 + 48i - 14j - 44k

q2 * q1

ans = quaternion
    -28 - 56i - 30j + 20k

However, every quaternion has a multiplicative inverse, so you can divide quaternions. Right division of q1 by q2 is equivalent to $$q1(q{2}^{-1})$$.

q1 ./ q2

ans = quaternion
      0.10345 -   0.3908i - 0.091954j + 0.022989k

Left division of q1 by q2 is equivalent to $$(q{2}^{-1})q1$$.

q1 .\ q2

ans = quaternion
     0.6 - 1.2i +   0j +   2k

The conjugate of a quaternion is formed by negating each of the complex parts, similar to conjugate of a complex number.

conj(q1)

ans = quaternion
     1 - 2i - 3j - 4k

To describe a rotation using a quaternion, the quaternion must be a unit quaternion. A unit quaternion has a norm of 1, where the norm is defined as

$$\mathrm{norm}\left(\mathit{q}\right)=\sqrt{{\mathit{a}}^2+{\mathit{b}}^2+{\mathit{c}}^2+{\mathit{d}}^2}$$.

Normalize a quaternion.

qNormalized = normalize(q1)

qNormalized = quaternion
     0.18257 + 0.36515i + 0.54772j +  0.7303k

Verify that this normalized unit quaternion has a norm of 1.

norm(qNormalized)

ans = 
1.0000

The rotation matrix for the conjugate of a normalized quaternion is equal to the inverse of the rotation matrix for that normalized quaternion.

rotmat(conj(qNormalized),'point')

ans = 3×3

   -0.6667    0.6667    0.3333
    0.1333   -0.3333    0.9333
    0.7333    0.6667    0.1333

inv(rotmat(qNormalized,'point'))

ans = 3×3

   -0.6667    0.6667    0.3333
    0.1333   -0.3333    0.9333
    0.7333    0.6667    0.1333

Extract Quaternions from Transformation Matrix

If you have a 3-D transformation matrix created using functions such as rigidtform3d, simtform3d or affinetform3d, you can extract the rotation matrix from it and represent it in the form of a quaternion.

Create a 3-D rigid geometric transformation object from the rotation angles in degrees and a translation vector.

eulerAngles = [0 0 30];
translation = [0 0 0];

tform = rigidtform3d(eulerAngles,translation)

tform = 
  rigidtform3d with properties:

    Dimensionality: 3
       Translation: [0 0 0]
                 R: [3x3 double]

                 A: [0.8660   -0.5000         0         0
                     0.5000    0.8660         0         0
                          0         0    1.0000         0
                          0         0         0    1.0000]

Create a quaternion from the rotation matrix. Specify that the rotation matrix is configured for point rotations.

rotationMatrix = tform.R;
q = quaternion(rotationMatrix,"rotmat","point")

q = quaternion
     0.96593 +       0i +       0j + 0.25882k

To verify that the quaternion and the rotation matrix produce the same results, rotate a sample point using both approaches.

point = [1 2 3];
rotatedPointQuaternion = rotatepoint(q,point)

rotatedPointQuaternion = 1×3

   -0.1340    2.2321    3.0000

rotatedPointRotationMatrix = transformPointsForward(tform,point)

rotatedPointRotationMatrix = 1×3

   -0.1340    2.2321    3.0000

To convert back to the original transformation object, extract a rotation matrix from the quaternion. Then, create a rigidtform3d object.

R = rotmat(q,"point");
recoveredTform = rigidtform3d(R,translation);
recoveredTform.R

ans = 3×3

    0.8660   -0.5000         0
    0.5000    0.8660         0
         0         0    1.0000