# Obtaining Heading vector from IMU

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

## 1 Answer

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;
}

• Thanks for the answer. This is helpful but I was hoping to be able to get the heading vector not only the angle. Apparently, after some reading this is not possible with Accel./gyro IMU. Aug 23 at 23:37
• Another though maybe to compute the yaw rate to detect if the vehicle is turning left or right, or just driving straight. Aug 23 at 23:39