1
$\begingroup$

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

$\endgroup$
2
$\begingroup$

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;
}
$\endgroup$
2
  • $\begingroup$ 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. $\endgroup$
    – Pioneer83
    Aug 23 at 23:37
  • $\begingroup$ Another though maybe to compute the yaw rate to detect if the vehicle is turning left or right, or just driving straight. $\endgroup$
    – Pioneer83
    Aug 23 at 23:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.