A=[cos(y),0,-sin(y);0 1,0;sin(y),0,cos(y)];
B=[1,0,0;0,cos(x),sin(x);0,-sin(x),cos(x)];
C=[cos(z),sin(z),0;-sin(z),cos(z),0;0,0,1];
plot3(Q(:,1),Q(:,2),Q(:,3),'black')
scatter3(Cf(1),Cf(2),Cf(3),'r')
scatter3(Ci(1),Ci(2),Ci(3),100,'b','filled')
plot3([0,10,0,-10,0,0,0,0,0,0,0,0],...
[0,0,0,0,0,10,0,-10,0,0,0,0],[0,0,0,0,0,0,0,0,0,10,0,-10],'b')
scatter3(Cf(1),Cf(2),Cf(3),100,'b','filled')
text(Cf(1)+i,Cf(2),Cf(3),['P_{final}',num2str(i)],'VerticalAlignment','top')
pitch_calc = -asin(Cf(3)*Ci(2))
yaw_calc = asind((Cf(3)*Ci(1))/cosd(real(pitch_calc)))
roll_calc = asind((Cf(1)*Ci(2))/cosd(real(pitch_calc)))
roll_calc = 9.0000e+01 - 2.2412e+02i