Wednesday, October 19, 2016

BNO055 Quaternion to Euler

BNO055 Quaternion to Euler

In the previous blog, I mentioned getting quaternion output from BMO055 and this time I just confirmed that the quaternion to euler (Pitch, Roll, Yaw) conversion is same as the euler output from BNO055.
The quaternion to euler conversion has several sequences depending on how you rotate the axis. For example, X axis - Y axis - Z axis, Y axis - X axis - Z axis, Z axis - X axis- Y axis, and so on.

This time, I just wanted to compare the conversion with the euler output from BNO055, so I simply calculated the euler from quaternion and compared with pre-calculated euler output.


                         


Equation

So here is the equation.
The sequence used here is Y - X - Z (2 - 1 - 3) and part of my processing code is listed below.
euler[0] = -atan2(2 * q[1] * q[3] + 2 * q[2] * q[0], 1 - 2 * q[1]*q[1] - 2 * q[2] * q[2])*180/PI;
euler[1] = asin(2 * q[2] * q[3] - 2 * q[1] * q[0])*180/PI; 
euler[2] = -atan2(2 * q[1] * q[2] + 2 * q[3] * q[0], 1 - 2 * q[2] * q[2] - 2 * q[3] * q[3])*180/PI;
Here, euler[0] is pitch, euler[1] is roll, and euler[2] is yaw angle. Also, q[0] is w, q[1] is x, q[2] is y, and q[3] is z.
You may notice the yaw angle is different around the +180 degree and -180 degree since the euler output from BNO055 is actually 0 degree to 360 degree.
To make it exactly same, you would need to generate case statement to convert the range from +/-180 degree to 0-360 degree.
With this equation, I was able to obtain the same result with the pre-calculated euler angle from BNO055. So my code is basically just getting quaternion from BNO055 and convert it to euler if needed. Quaternion is actually better to handle graphic rotations, and next time I will implement something that I can rotate the graphical object using processing.


1 comment:

  1. Do you think that with the sensor, it is possible to calculate the distance between a measure in moment x and the meaure in moment x+1:

    pose1 = (x,y,z) = bno055.measure1
    pose2 = (x,y,z) = bno055.measure2

    Many thanks in advance

    ReplyDelete