Attitude estimation using IMU and quaternions

 

Understanding the relationship between IMU data and quaternions is vital for designing unmanned aerial vehicles (UAVs) to compute orientation in 3D space. The idea is to apply Inertial Measurement Unit (IMU) sensors (accelerometer, magnetometer, and gyroscope) to calculate the attitude of an object based on the quaternions. This article introduces the concept of quaternions and shows how a gyroscope can help compute an object's orientation using them. I expect you know the basic principles of attitude estimation, including inertial and body frame. Also, you need at least a basic knowledge of linear algebra. So, let us get started. IMU and Quaternions: Introduction Quaternions are numbers with three imaginary terms (i,j,k). At first glance, the quaternions resemble complex numbers, which is not coincidental. Complex numbers are used in 2D space, whereas quaternions are necessary for 3D space rotations. To better grasp quaternions, we will first consider complex numbers. Unlike in quaternions, we have only a single imaginary part in complex numbers:

 

z = a + bi, i2 = -1,

 

Where 'i' is an abstract notion whose square equals minus one. The true power of complex numbers lies in their representation in polar form. Based on the real and imaginary parts, we can define a point in 2d space, as shown in the figure below.

Using the polar form, we define two terms: magnitude (norm) and phase of complex numbers. 

 

|z| = sqrt(a² + b²)

θ = atan2(b, a) 

Using the norm and phase, we can represent a complex number differently:

z = a + bi = |z|(cos(θ) + sin(θ)i)

 

Here is the exciting point. Let us multiply two complex numbers with arbitrary magnitudes and phases:

z₁ * z₂ = |z₁|(cos(θ₁) + sin(θ₁)i) * |z₂|(cos(θ₂) + sin(θ₂)i) = 

= |z₁||z₂|((cos(θ₁)cos(θ₂) - sin(θ₁)sin(θ₂)) + i(cos(θ₁)sin(θ₂) + sin(θ₁)cos(θ₂))) = 

= |z₁||z₂|(cos(θ₁+θ₂) + sin(θ₁ + θ₂)i)

 

Using basic trigonometry, we can see that multiplying complex numbers is equivalent to multiplying magnitudes and adding phases. It means that the product of the complex numbers involves a rotation counterclockwise and multiplication of the magnitudes. If we assume the magnitude is unity, the product of the complex numbers reduces to a rotation operation.  

 

 

Using this feature of the complex numbers, we can solve our first rotation problem:

 

Problem 1. Given a point in 2D space with coordinates (2, 1). Find its position after a counterclockwise rotation of 30 degrees. 

Solution. First, we define a complex number: 2 + 1i. Then, we have another complex number with a magnitude equal to one and a 30-degree phase:

 

z = cos(30) + sin(30)i

 

Then we multiply these complex numbers to obtain the point's position (1.232, 1.866) after the rotation.

 

z_new  = (cos(30) + sin(30)i) * (2 + 1i) = 1.232 + 1.866i

 

 

As we see, the complex numbers allow us to deal with rotations in 2D space effortlessly. The next step is to do 3D space rotations using quaternions. 

---

Quaternions

 

First, let me present some algebraic definitions of quaternions. Like in complex numbers, there is a real and complex part in quaternions. The complex part consists of three imaginary terms. 

q = s + xi +yj +zk,  

s,x,y, and z are real numbers.

i² = j² = k² = -1

ij = k, jk = i, ki = j, ji = -k, kj = -i, ik = -j  

It is also helpful to denote the imaginary part as a vector with three terms:

We can do elementary addition and subtraction operations. Also, we can multiply a quaternion by a scalar. A pure quaternion is when the real part of the quaternion equals zero:

 

q_a = [s_a, a], q_b = [s_b, b]

q_a +q_b = [s_a+s_b, a + b]

q_a -q_b = [s_a+s_b, a - b]

 

For quaternions, we can also compute the norm(magnitude), the square root of the sum of squares of all terms.

q = s + xi +yj +zk

|q|=sqrt(s² + x² + y² + z²)

Unit norm: |q| = 1

 

As with complex numbers, we can have a conjugate, inverting the sign of the imaginary part. Using the conjugate, we can efficiently compute the inverse of the quaternion.

Conjugate:  q^*=[s, -v]

Inverse q^-1=q^*/|q|² 

 

Lastly, we can multiply two quaternions, as shown below. We used the aforementioned algebraic definitions to deal with the product of imaginary terms.  

q_a = s_a + x_ai +y_aj +z_ak

q_b = s_b + x_bi +y_bj +z_bk 

q_aq_b = (s_a + x_ai +y_aj+z_ak)(s_b + x_bi +y_bj +z_bk) = 

(s_as_b - x_ax_b - y_ay_b - z_az_b)+(s_ax_b+x_as_b+y_az_b-z_ay_b)i +

(s_ay_b - x_az_b + y_as_b + z_ax_b)j + (s_az_b + x_ay_b - y_ax_b + z_as_b)k

We can also represent multiplication in matrix form. There are two options: we can either keep the order of quaternions or reverse them. When holding the order, we use the matrix L; changing the order requires the matrix R.

 

 

 

Finally, let me show how to perform rotations in 3D space using quaternions. The quaternion allows us to rotate a point in 3D space around any vector by any angle. If we have a point p(xp, yp, zp) and if we decide to rotate around an arbitrary axis v(x,y,z) by  Θ angle, we follow these steps:

1. We define a pure quaternion for the point: p = [0, x_p, y_p, z_p]
2. We define another quaternion using the rotation axis and angle:  q = [cos(θ/2), sin(θ/2)x, sin(θ/2)y, sin(θ/2)z]
3. Finally, we will do this multiplication: p' = qpq^-1

After this multiplication, the imaginary part of p' will possess the position of the point after the rotation. So, we are ready to solve our second problem when we do the rotation in 3D space.

 

Problem 2. Rotate a point (0, 1, 1) around the y-axis to 90 degrees. 

Solution. First, we define a pure quaternion of the point to be rotated: q = [0, 0, 1, 1].

Second, we define another quaternion using the rotation vector and angle. In our case, the rotation axis is the y-axis (0,1,0), and the angle is 90 degrees.

q_rot = [cos(45), 0, sin(45), 0]

Finally, we will do these multiplications: p' = q_rotpq^-1_rot. And we use the matrices above to handle these multiplications. We multiply the first two terms qrotp = L(qrot)p = a. We keep the order of the quaternions, so I use the L matrix. The result is another quaternion a, which is equal to:

 

Then, we do the second part of the multiplication. We use the quaternion's conjugate to get the quaternion's inverse. And in this case, I decided to reverse the order using the matrix R: aq^-1_rot= R(q^-1_rot)a.  

 

Finally, the last three terms (1, 1, 0) of the result represent the position of the point after the rotation operation. We can verify it visually using the figure below.

 

---

Multiple rotations using quaternions

 

We can apply the quaternions to handle them without difficulty if we have sequential multiple rotations.

For example, if we have a single rotation, we can use this equation to compute the position of a point after the rotation:

q₁(p)q^-1₁

If we do another rotation, we need to add another quaternion:

 

q₂q₁(p)q^-1₁q^-1₂

Here we can use a trick to rearrange the order of inverse quaternions: q^-1₁q^-1₂ = (q₂q₁)^-1. Using this trick, let's rewrite the equation of multiple rotations:

 

q₂q₁(p)q^-1₁q^-1₂ = (q₂q₁)(p)(q₂q₁)^-1

If we denote (q₂q₁) = q_final:

(q₂q₁)(p)(q₂q₁)^-1 = (q_final)(p)(q_final)^-1 

This equation means that instead of recomputing the position each time a rotation occurs, we can keep track of the total rotation by multiplying the quaternions.

---

 

IMU and Quaternions: Frames of reference

At this point, we clearly understand how quaternions can be used to compute the position of a point after rotation around an axis by a given angle in 3D space. At the same time, we can define the relationship between the inertial frame and the body frame using quaternions because, ultimately, how the body frame is rotated determines its relation to the inertial frame. The quaternions are extremely helpful for representing rotations in 3D space. 

 

 

---

Gyroscope IMU and Quaternions

 

Finally, let me show how IMU sensors and quaternions are linked. First, I will show how to calculate quaternions using the Gyroscope. The gyroscope is a sensor that measures the angular velocity in three axes. In other words, it measures the projection of the rotation onto the body frame axes. We can convert the gyroscope readings to a quaternion:

 

Gyroscope readings: [w_x, w_y, w_z], converted into radians

We compute the rotation angle(w) and the axes of the rotation(x_g, y_g, z_g):

w = sqrt(w²_x + w²_y + w²_z)

x_g = ω_x/ω, y_g = ω_y/ω, z_g = ω_z/ω

Finally, we have a quaternion:

 

q_gyro = [cos(w/2), x_gsin(w/2), y_gsin(w/2), z_gsin(w/2)]

 

We can further simplify this equation by exploiting the fact that the gyroscope is sampled at a high frequency (~1kHz), so the rotation angle is tiny. Then we can employ the approximations cos(ω/2) = 1 and sin(ω/2) = ω/2. If we substitute these approximations, we obtain

q_gyro = [1, x_gw/2, y_gw/2, z_gw/2] 

Then, instead of x_g, y_g, z_g, we insert their real expressions, so w is canceled:

q_gyro = [1, (ω_x/ω)(w/2), (ω_x/ω)(w/2), (ω_x/ω)(w/2)]=

[1, ω_x/2, ω_y/2, ω_z/2] 

Finally, we can extract quaternions from the gyroscope readings without much computation.

 

If we sample the gyroscope every 1 ms, the quaternion computed by the gyroscope readings will represent a rotation that happened in this 1 ms time frame. As I showed before, we can multiply the quaternions of the successive rotations to obtain the quaternion representing the total rotation. In the case of using the gyroscope, We can do this operation to keep the quaternion that relates the body and inertial frame up-to-date:

 

q_new = q_old q_gyro

And we will use the R matrix to do this operation:

 

q_new = R(q_gyro)q_old 

 

Here is the final equation for updating the quaternion using the gyroscope:

 

 

If you want to get more detailed guidance about attitude estimation, please refer to my course:

STM32 attitude estimation course