求姿态角欧拉角(ZYX)

Z=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];
Y=[cos(b) 0 sin(b);0 1 0;-sin(b) 0  cos(b)];
X=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
R=Z*Y*X
      
b1=atan2(-R(3,1),+-sqrt(R(3,2)^2+R(3,3)^2))
r1=atan2(R(2,1)/cos(b1),R(1,1)/cos(b1))
a1=atan2(R(3,2)/cos(b1),R(3,3)/cos(b1))