matlabrotationquaternionseuler-angleskinematics

Translate quaternion rotation to correct frame


I place an IMU on my wrist and extend my arm as shown in the photo below. I spin in a circle once, while my arm remains in the fixed position. I calculate the euler pitch and quaternion angle. In the photo below, the euler pitch remains approximately constant ( my hand shakes a bit ), while the quaternion angle increases linearly it seems. My data is located here: (sample_data.csv)

enter image description here

Question:

What changes should I make to measure the angle from the quaternion? ( I suspect it is in form of RPR', adjust to world axis, but am unsure what P would be )

Matlab code:

clc;
clear;

table = readtable("sample_data.csv", 'Delimiter', ',');
euler_angles = zeros(length(table.w),1);
quaternion_angles = zeros(length(table.w),1);

for idx = 1:length(table.w)
    w = table.w(idx);
    x = table.x(idx);
    y = table.y(idx);
    z = table.z(idx);
    euler_angles(idx) = getEulerAngle(w,x,y,z);
    quaternion_angles(idx) = getQuaternionAngle(w,x,y,z);
end

figure(1);
clf;
hold on;
plot(euler_angles,'ro');
ylabel("Angle in deg");
xlabel("Sample");
plot(quaternion_angles, 'bo');
hold off;
legend('Euler','Quaternion');

function angle = getQuaternionAngle(w,x,y,z)
    q = quaternion(w,x,y,z);
    angle = acosd(w);
end

function angle = getEulerAngle(w, x, y, z)
    mag = (2*(y * w - z * x));
    angle = rad2deg(asin(mag));
end

Solution

  • Assuming you rotate around an axis [x;y;z] , the post you refer to states that the quaternion associated with the rotation is of the form:

    q(1) = cos(r/2);
    q(2) = sin(r/2)*x;
    q(3) = sin(r/2)*y;
    q(4) = sin(r/2)*z;
    

    Where r is the angle in radians and [x;y;z] is the 3d vector representing the axis around which you rotate.

    To get the instantaneous rotation r you need to calculate 2*acos(q(1)) for an angle in radians, or 2*acosd(q(1)) for an angle in degrees.

    Now plotting the data from your csv file gives the following:

    enter image description here

    Which is coherent with the assumption that w is the first coordinate of your quaternion, and that you indeed rotate mostly around z.

    Euler angles (Or more likely Tait Bryan angles) are a different way to represent a rotation. A rotation is represented by a composition of 3 elemental rotations. These 3 rotations are sometimes called yaw, pitch and roll. If you want to fully represent your rotation, you will have to calculate all of these 3 elemental rotations:

    table = readtable("sample_data.csv", 'Delimiter', ',');
    
    w = table.w;
    x = table.x;
    y = table.y;
    z = table.z;
    
    [yaw,pitch,roll] = getAngles(w, x, y, z);
    
    quaternion_angles = getQuaternionAngle(w);
    
    figure; plot([quaternion_angles,yaw,pitch,roll]);
    
    legend({'quat','yaw','pitch','roll'})
    
    function angle = getQuaternionAngle(w)
    
        angle = 2*acosd(w);
        
    end
    
    function [yaw,pitch,roll] = getAngles(w, x, y, z)
        
        yaw = atan2d(2*(y.*z + w.*x), w.*w - x.*x - y.*y + z.*z);
        pitch = asind(-2*(x.*z-w.*y));
        roll = atan2d(2*(x.*y + w.*z), w.*w + x.*x - y.*y - z.*z);
    
    end
    

    enter image description here

    See? The angle around Z is represented by the roll (the discontinuity is due to the use of arctan). When you rotate around yourself, the angle increases steadily between 0 and 360º. You can also see that you have a bit of yaw, i.e your IMU is a bit tilted