Reproduce pitch from Euler angles using Quaternions in MATLAB - matlab

Question

I have a followup question from this post : Extracting Yaw from a Quaternion .
Alike the OP, I want to move away from Euler rotations and use Quaternions. My scenario is as follows:
I am wearing an Apple smartwatch on my left hand and performing the bicep curls exercise. I do a few reps in the first set, and stop. I then rotate my body 90 degrees to the right in my room, and do another set of a few repetitions. I do this 4 times, and store the quaternions to disk using the xArbitraryCorrectedZVertical frame. When I use Euler rotations to describe pitch, using below code, I see a sinusoidal shape. When I want to reproduce the same pattern using quaternions, I get close, except the signal drifts. Here is my code:
clc;
clear;
quaternions_table = readtable(strcat("quaternions"), 'Delimiter',',');
attitude = quaternion(quaternions_table.w, quaternions_table.x, quaternions_table.y, quaternions_table.z);
quats = zeros(length(attitude), 1);
eulers = zeros(length(attitude), 1);
for i = 1:length(attitude)
[w,x,y,z] = parts(attitude(i) * quaternion(0,0,0,1));
quats(i) = rad2deg(atan2(z,w));
[w,x,y,z] = parts(attitude(i));
eulers(i) = rad2deg(asin(2*(y * w - z * x)));
end
figure(1);
clf;
hold on;
plot(quats, '.r');
plot(eulers, '.b');
legend('Q', 'E');
hold off;
Here is a plot of the difference:
How can I fix my code so that the quaternion generated pattern is equivalent to the euler's pitch, regardless of my "body" rotation?
Some sample data: https://filebin.net/02k2323ccbct1n38/quaternions

Answer 1

DISCLAIMER 1: this is a first attempt at putting together an answer. What follows is by no means conclusive. Any feedback aimed at improving this answer is sincerely welcome.
DISCLAIMER 2: I don't have access to any toolboxes featuring functions related to quaternions, so I relied on what's available on Mathworks's File Exchange, to which I added a function that builds quaternions by extracting their components from a table. Using the quaternion-related functions provided in Matlab's toolboxes, the OP should be able to replicate the calculations showcased hereinafter.
DISCLAIMER 3: the selection of the point to which the rotations are applied matters. Picking a different P will affect the resulting plot. In particular, with points far from P(1,0,0) the 3D map of the successive positions in space doesn't appear to be compatible with the physics of the OP moving the smart-watch in the way described in the question.
With the sample data provided by the OP, I was able to apply the quaternions to the point with coordinates P(1,0,0) i.e. a point on the global x-axis, using (pseudo-code):
% Pseudo code
rotatedP = Qi * P * conjugate(Qi);
where Qi is the i-th quaternion extracted from the sample data.
Here's a plot showing the positions in space assumed by the point after being rotated:
Once the coordinates of the point are available, calculating the pitch could be done by computing the acos of the dot product between the unit vector parallel to OP (O being the global origin) and the unit vector parallel to the z-axis (with components (0,0,1)), in pseudo-code:
% Pseudo code
pitch = acos(OP.z/norm(OP))
Here's a plot of the pitch shifted using the first value of the array, which is extremely close to what was obtained by the OP using Euler's angles:
Matlab code
Here's the code I used to generate the plots. As mentioned erlier, I relied on Przemyslaw Baranski's Quaternions. Please, note that my code is a very crude attempt at tackling the problem, with basically no optimization. Uncommenting the lines for the 3D plot slows down the process considerably:
function draw_quats()
% point to be rotated
P = [ 1, 0, 0 ];
% Import quaternion components
q_tab = readtable(strcat("quaternions_data"), 'Delimiter',',');
pitch = [];
pp = 1;
for k = 1:1:numel(q_tab.w)
% create a quaternion for rotation
Qrot = qGetQuaternionFromComponents(q_tab.x(k), q_tab.y(k), q_tab.z(k), q_tab.w(k));
% rotate point
Prot = qRotatePoint( P, Qrot );
Pnorm = sqrt(Prot(1)^2 + Prot(2)^2 + Prot(3)^2);
% % display axes
% quiver3( 0,0,0, 1,0,0 );
% hold on;
% quiver3( 0,0,0, 0,1,0 );
% quiver3( 0,0,0, 0,0,1 );
%
% % display point
% plot3( Prot(1), Prot(2), Prot(3), 'b.' );
%
% % setup the plot
% grid on;
% axis equal;
% axis([-1 1.0 -1 1.0 -1 1.0]);
% xlabel( 'x' );
% ylabel( 'y' );
% zlabel( 'z' );
%
% view( 45, 20 );
% drawnow;
% Compute pitch
pitch(pp) = acos(Prot(3)/Pnorm)*180/pi;
pp = pp + 1;
end
figure
plot(pitch-pitch(1),'.k');
end
And this is the definition of qGetQuaternionFromComponents:
function Q = qGetQuaternionFromComponents( x, y, z, w )
Q = [w x y z]';
end
In the event you have some of the toolboxes by MATLAB, you can do something like this:
for i = 1:length(attitude)
qPoint = quaternion(0,-1,0,0);
q = attitude(i);
[~,x,y,z] = parts(q * qPoint * conj(q));
Pnorm = sqrt(x^2 + y^2 + z^2);
quats(i) = asind(z/Pnorm);
end
References
Euclideanspace.com
Mathworks' FileExchange