I'd like to visualize elemental rotations using Z-Y-X convention in Matlab. If you'd like to follow me exactly, I'll be refering to a paper Quadrotor Dynamics and Control by Randal Beard.
The base coordinate frame, $F^v$, is called the vehicle frame (page 6). It is a right-handed cartesian frame of reference. The unit vectors in $F^v$ are $x^v, y^v, z^v$ which can be visualized in Matlab using the below script:
x_v = [1;0;0]; %Define unit vectors in vehicle frame y_v = [0;1;0]; z_v = [0;0;1]; o_v = [x_v'; y_v'; z_v']; %Convert to quiver3 compliant form quiver3(zeros(3,1), zeros(3,1), zeros(3,1), o_v(:,1), o_v(:,2), o_v(:,3)) %Draw the frame
I have written a function to create a Direction Cosine Matrix from Euler angles, as defined on page 9 of the aforementioned paper:
function dcm = RPY_2_DCM(roll, pitch, yaw) cR = cos(roll); sR = sin(roll); cP = cos(pitch); sP = sin(pitch); cY = cos(yaw); sY = sin(yaw); dcm = [cP*cY cP*sY -sP; sR*sP*cY-cR*sY sR*sP*sY+cR*cY sR*cP; cR*sP*cY+sR*sY cR*sP*sY-sR*cY cR*cP]; end
Drawing the vehicle-1 $F^{v1}$ frame as a rotation around $z^v$ by yaw angle with the below script works fine (vehicle frame in blue, vehicle-1 frame in green):
yaw = pi/6; x_v1 = RPY_2_DCM(0,0,yaw)*x_v; %Draw vehicle-1 coordinate frame obtained by right-handed rotation of F_v around z_v y_v1 = RPY_2_DCM(0,0,yaw)*y_v; z_v1 = RPY_2_DCM(0,0,yaw)*z_v; o_v1 = [x_v1'; y_v1'; z_v1']; quiver3(zeros(3,1), zeros(3,1), zeros(3,1), o_v1(:,1), o_v1(:,2), o_v1(:,3))
Now, I'd like to rotate vectors in $F^{v1}$ around $y^{v1}$ axis of vehicle-1 by adding:
pitch = pi/6; x_v2 = RPY_2_DCM(0,pitch,0)*x_v1; y_v2 = RPY_2_DCM(0,pitch,0)*y_v1; z_v2 = RPY_2_DCM(0,pitch,0)*z_v1; o_v2 = [x_v2'; y_v2'; z_v2']; quiver3(zeros(3,1), zeros(3,1), zeros(3,1), o_v2(:,1), o_v2(:,2), o_v2(:,3))
But instead of rotating around $y^{v1}$ I obtain a rotation of $F^{v1}$ around $y^v$ (see the picture below, $F^{v2}$ in red). What am I doing wrong?