Obtaining Heading vector from IMU

Question

First time here.

I'm an electronics engineer who has been tasked with this problem: Given the 4D quaternion representing a vehicle orientation, how do you get the heading vector?

The vehicle orientation quaterions was obtained through the transformation of the IMU reference frame to the vehicles reference frame.

Now, I'm not extremely expert in quaterions, though I have been reading considerably about them recently.

Thank you

Answer 1

Probably the easiest way to do this would be to convert from Quaternion to Roll-Pitch-Yaw rotations, and then your heading is the Yaw angle.

I'll note that the Yaw angle is not fixed/correct unless your IMU has a magnetometer. Accelerometers can fix roll/pitch by detecting the gravity/down vector, but North only comes from the magnetometer.

You can get more information from the Wikipedia article here, but I'll reproduce the Quaternion to Euler RPY code here for posterity:

#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}