Get Roll/Pitch/Yaw from IMU not correct

asked 2019-05-21 15:34:58 -0500

AutoCar gravatar image

Hi, I have 2D robot with IMU. I am trying to extract the roll/pitch/yaw data from the IMU message but found the results were not what I expected.

At the beginning, the IMU orientation (x, y, z, w) is (-0.08443, -0.5308, -0.0665, 0.01739), the correspondent yaw is about 64 degree. Then the car rotate about 90 degree, the IMU reported orientation is (0.8799, 0.4707, 0.0645, -0.0027) and the yaw is about 54 degree. The difference between 64 and 54 is far from 90 degree. Why?

edit retag flag offensive close merge delete

Comments

How are you performing the conversion from quaternion values to the yaw value?

kosmastsk gravatar image kosmastsk  ( 2019-06-06 04:49:50 -0500 )edit

This is what I used:

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
    // roll (x-axis rotation)
    double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    roll = atan2(sinr_cosp, cosr_cosp);

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

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());  
    yaw = atan2(siny_cosp, cosy_cosp);
}
AutoCar gravatar image AutoCar  ( 2019-06-20 10:18:50 -0500 )edit

I am thinking may be the IMU orientation is related to the robot the IMU device attached to? Even the car rotated 90 degree, since the IMU is fixed on the car, yaw did not change much...

AutoCar gravatar image AutoCar  ( 2019-06-20 15:24:01 -0500 )edit
add a comment