Calculating oriantation using IMU

This document describes how the orientation of the IMU MPU-9250 can be calculated.

There are basically two ways to calculate the orientation of the IMU.

• First we can use the result provided in the sensor fusion algorithm on the chip. This is done in a Digital Motion Processor (DMP) on the sensor chip and send as a quaternion.

• Second, we can use raw data and do the filtering ourself.
  

1 Using the DMP quaternion

The result of the calculations in the Digital Motion Processor (DMP) done in the IMU are reported as quaternions (basically a short coding of the rotation between the IMU and the fixed laboratory system). Technically they are send to the Aduino (as a byte vector the fifoBuffer). On the Aduino the following steps the result of the FIFO Buffer is parsed and written in a Quaternion with components \(q=(q_w, q_x, q_y, q_z)\) this is done in `dmpGetQuaternion`

1.1 Getting the quaternion q

In the first line the quaternion send from the DSP is read from the fifoBuffer and parsed into the variable q. The quaternion q contains all information about a single IMU. This is done with the following code:

Code

uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet == 0) packet = dmpPacketBuffer;
    data[0] = ((packet[0] << 8) | packet[1]);
    data[1] = ((packet[4] << 8) | packet[5]);
    data[2] = ((packet[8] << 8) | packet[9]);
    data[3] = ((packet[12] << 8) | packet[13]);
    return 0;
}

uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4];
    uint8_t status = dmpGetQuaternion(qI, packet);
    if (status == 0) {
        q -> w = (float)qI[0] / 16384.0f;
        q -> x = (float)qI[1] / 16384.0f;
        q -> y = (float)qI[2] / 16384.0f;
        q -> z = (float)qI[3] / 16384.0f;
        return 0;
    }
    return status; // int16 return value, indicates error if this line is reached
}

1.2 Understanding the quaternion

A good source is shown in the following youtube video.

The quaternion send from the DMP is a rotation quaternion. A rotation quarternion describes a rotation of \(\theta\) around a vector \(\vec{r}\) as

\[ q(\theta, v) = \left(\cos(\frac{\theta}{2}),\; \sin(\frac{\theta}{2}) r_1, \;\sin(\frac{\theta} {2}) r_2, \;\sin(\frac{\theta}{2}) r_3 \right) \]

• Step 1: Align the lab coordinate system and the coordinate system of the IMU. In this case we have the identity quaternion \(q=(1,0,0,0)\)

  • Put the sensor on the table and then rotate until you find the identity quaternion.

• Step 2: Turn around x-axis in positive direction. If you found the right axis you have \(q(\theta, v) = \left(\cos(\theta/2),\; \sin(\theta /2), \;0, \;0\right)\) and a rotation around 90 degrees in positive direction you have $q=(0,1,0,0)$ Rotate in the other direction you should get \(q=(0,-1,0,0)\)

  Repeat step 2 for the remaining axis. If all goes as expected you have a right handed coordinate system with a body-to-lab (a.k.a. frame) based quaternion.

1.2.1 Calculation of yaw, pitch, roll from quarternion

The code used on the Arduino has been wrong ypr_calculations.html see also Bugreport but it can be directly calculated from the quaternion via.

\[ \begin{align*} \text{roll} &= \text{atan2}(2 \cdot (w \cdot x + y \cdot z), 1 - 2 \cdot (x^2 + y^2)) \\ \text{pitch} &= \text{asin}(2 \cdot (w \cdot y - z \cdot x)) \\ \text{yaw} &= \text{atan2}(2 \cdot (w \cdot z + x \cdot y), 1 - 2 \cdot (y^2 + z^2)) \end{align*} \]

It is implemented in quaternion_utils.py where you also find testing code.