I am using an Oak-d on a robotic rover platform and would like to use the IMU to be able to get roll, pitch, and yaw/heading. I've read the documentation and saw the several different methods for getting different types of data from the IMU but these are typically conveyed in a quaternion. Is there a straightforward method for converting the quaternion into roll, pitch, and yaw? I'm not looking for somebody to write a full implementation for me, but some proper equations would be greatly appreciated.

Additionally, how would I go about correcting the quaternion for something like magnetic declination. The IMU will return a quaternion that is referenced to magnetic north (depending on which method is being used), but most of the time there is a small difference between magnetic north and geographic north. The system I'm using has an RTK on board. I am able to drive the rover in a straight line for a short distance and then use the positioning data to calculate what the true heading is in reference to geographic north. But how would I use that to correct the orientation data from the IMU?

  • jakaskerl replied to this.

  • silo
    Posting GPT answer as it is the most concise:

    Sure! Here's the modified version of my first answer with inline formulas:

    Given a quaternion ( q = (q_w, q_x, q_y, q_z) ), you can convert it to roll, pitch, and yaw (Euler angles) using the following equations:

    (can't render latex so I'll add python code)

    import math

def quaternion_to_euler(q_w, q_x, q_y, q_z):
    # Roll (x-axis rotation)
    roll = math.atan2(2 * (q_w * q_x + q_y * q_z), 1 - 2 * (q_x**2 + q_y**2))
    
    # Pitch (y-axis rotation)
    pitch = math.asin(2 * (q_w * q_y - q_z * q_x))
    
    # Yaw (z-axis rotation)
    yaw = math.atan2(2 * (q_w * q_z + q_x * q_y), 1 - 2 * (q_y**2 + q_z**2))
    
    return roll, pitch, yaw

    As for the declination issue; I assume you are operating in a small scale (relative to earth size) so you can use a constant declination theta which can be computed once from the difference between the RTK and the magnetometer (or by checking the declination online for your specific location). Then correct the heading by that angle theta.

    Thoughts?

    Thanks,
    Jaka

      silo
      Posting GPT answer as it is the most concise:

      Sure! Here's the modified version of my first answer with inline formulas:

      Given a quaternion ( q = (q_w, q_x, q_y, q_z) ), you can convert it to roll, pitch, and yaw (Euler angles) using the following equations:

      (can't render latex so I'll add python code)

      import math

def quaternion_to_euler(q_w, q_x, q_y, q_z):
    # Roll (x-axis rotation)
    roll = math.atan2(2 * (q_w * q_x + q_y * q_z), 1 - 2 * (q_x**2 + q_y**2))
    
    # Pitch (y-axis rotation)
    pitch = math.asin(2 * (q_w * q_y - q_z * q_x))
    
    # Yaw (z-axis rotation)
    yaw = math.atan2(2 * (q_w * q_z + q_x * q_y), 1 - 2 * (q_y**2 + q_z**2))
    
    return roll, pitch, yaw

      As for the declination issue; I assume you are operating in a small scale (relative to earth size) so you can use a constant declination theta which can be computed once from the difference between the RTK and the magnetometer (or by checking the declination online for your specific location). Then correct the heading by that angle theta.

      Thoughts?

      Thanks,
      Jaka

        Thank you this is greatly appreciated.

        I initially thought that correcting the heading would be as simple as just adding/subtracting the declination from the heading, but just wanted to have some reassurance because it seemed too easy.