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 i + c j + d 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 (\frac{α}{2}) + \sin (\frac{α}{2}) (x i + y j + z k)$

Quaternion Creation

Open Script

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

Open Live Script

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

$i^{2} = j^{2} = k^{2} = 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 $q 1 (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}) q 1$ .

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

$norm (q) = \sqrt{a^{2} + b^{2} + c^{2} + 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

This example uses:

Open Live Script

If you have a 3-D transformation matrix created using functions such as rigid3d (Image Processing Toolbox) or affine3d (Image Processing Toolbox), you can extract the rotation matrix from it and represent it in the form of a quaternion. However, before performing this conversion, you must first convert the rotation matrix from the postmultiply format to the premultiply format expected by quaternions.

Postmultiply Format

To perform rotations using the rotation matrix part of a transformation matrix, multiply an (x, y, z) point by this rotation matrix.

In point rotations, this point is rotated within a frame of reference.
In frame rotations, the frame of reference is rotated around this point.

Transformation matrices represented by rigid3d or affine3d objects use a postmultiply format. In this format, the point is multiplied by the rotation matrix, in that order. To satisfy the matrix multiplication, the point and its corresponding translation vector must be row vectors.

This equation shows the postmultiply format for a rotation matrix, R, and a translation vector, t.

$[\begin{array}{c} x' & y^{'} & z^{'} \end{array}] = [\begin{array}{c} x & y & z \end{array}] [\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] + [\begin{array}{c} t_{x} & t_{y} & t_{z} \end{array}]$

This format also applies when R and t are combined into a homogeneous transformation matrix. In this matrix, the 1 is used to satisfy the matrix multiplication and can be ignored.

$[\begin{array}{c} x^{'} & y^{'} & z^{'} & 1 \end{array}] = [\begin{array}{c} x & y & z & 1 \end{array}] [\begin{array}{c} R_{11} & R_{12} & R_{13} & 0 \\ R_{21} & R_{22} & R_{23} & 0 \\ R_{31} & R_{32} & R_{33} & 0 \\ t_{x} & t_{y} & t_{z} & 1 \end{array}]$

Premultiply Format

In the premultiply format, the rotation matrix is multiplied by the point, in that order. To satisfy the matrix multiplication, the point and its corresponding translation vector must be column vectors.

This equation shows the premultiply format, where R is the rotation matrix and t is the translation vector.

$[\begin{array}{c} x' \\ y^{'} \\ z^{'} \end{array}] = [\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] [\begin{array}{c} x \\ y \\ z \end{array}] + [\begin{array}{c} t_{x} \\ t_{y} \\ t_{z} \end{array}]$

As with the postmultiply case, this format also applies when R and t are combined into a homogeneous transformation matrix.

$[\begin{array}{c} x^{'} \\ y^{'} \\ z^{'} \\ 1 \end{array}] = [\begin{array}{c} R_{11} & R_{12} & R_{13} & t_{x} \\ R_{21} & R_{22} & R_{23} & t_{y} \\ R_{31} & R_{32} & R_{33} & t_{z} \\ 0 & 0 & 0 & 1 \end{array}] [\begin{array}{c} x \\ y \\ z \\ 1 \end{array}]$

Convert from Postmultiply to Premultiply Format

To convert a rotation matrix to the premultiply format expected by quaternions, take the transpose of the rotation matrix.

Create a 3-D rigid geometric transformation object from a rotation matrix and translation vector. The angle of rotation, $θ$ , is in degrees.

theta = 30;
rot = [ cosd(theta) sind(theta) 0; ...
       -sind(theta) cosd(theta) 0; ...
                  0           0 1];
trans = [2 3 4];

tform = rigid3d(rot,trans)

tform = 
  rigid3d with properties:

       Rotation: [3x3 double]
    Translation: [2 3 4]

The elements of the rotation matrix are ordered for rotating points using the postmultiply format. Convert the matrix to the premultiply format by taking its transpose. Notice that the second element of the first row and first column swap locations.

rotPost = tform.Rotation

rotPost = 3×3

    0.8660    0.5000         0
   -0.5000    0.8660         0
         0         0    1.0000

rotPre = rotPost.'

rotPre = 3×3

    0.8660   -0.5000         0
    0.5000    0.8660         0
         0         0    1.0000

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

q = quaternion(rotPre,'rotmat','point')

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

To verify that the premultiplied quaternion and the postmultiplied 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 = point * rotPost

rotatedPointRotationMatrix = 1×3

   -0.1340    2.2321    3.0000

To convert back to the original rotation matrix, extract a rotation matrix from the quaternion. Then, create a rigid3d object by using the transpose of this rotation matrix.

R = rotmat(q,'point');
T = rigid3d(R',trans);
T.Rotation

ans = 3×3

    0.8660    0.5000         0
   -0.5000    0.8660         0
         0         0    1.0000

Documentation