## 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 i2 = j2 = k2 = 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:

`$\mathrm{cos}\left(\frac{\alpha }{2}\right)+\mathrm{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\left(q{2}^{-1}\right)$.

`q1 ./ q2`
```ans = quaternion 0.10345 - 0.3908i - 0.091954j + 0.022989k ```

Left division of `q1` by `q2` is equivalent to $\left(q{2}^{-1}\right)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 R: [3x3 double] Translation: [0 0 0] A: [4x4 double] ```

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 ```