ppz飞控代码不同姿态表示之间转换理解

1、世界坐标系采用NED(North,East,Down)坐标系，为右手坐标系；
物体坐标系也为右手坐标系；两坐标系开始时重合在一起
2、欧拉角可以表示为(φ,θ,ψ)分别对应绕x，y，z轴旋转的角度,根据绕不同
轴旋转的顺序有不同表示，主要有两种321(z->y->x)及312(z->x->y)。
1）绕z轴旋转ψ，P’=Rz*P (P为世界坐标系矢量，P’为对应物体坐标系的矢量,Rz为旋转矩阵）
Rz=
2) 绕y轴旋转θ：
Ry＝
3) 绕x轴旋转φ：
Rx =
4) 按321顺序旋转：
=Rx*Ry*Rz=
5）按312顺序旋转：
=Ry*Rx*Rz=

6) 绕 单位矢量(Nx, Ny, Nz)旋转θ,P”=R(N,θ)*P (P,P”对应世界坐标系中的矢量，P”为旋转后在世界坐标系中的矢量，P为旋转前的矢量）
R(N,θ) =
7）单位四元数Q=,Q’=
p=(x,y,z)对应世界坐标系的点，用四元数P=(0,x,y,z)表示，
P”=Q*P*Q’=(0,x”,y”,z”), 记p”=(x”,y”,z”)
则p”=R(N,θ)*p
8) 四元数与欧拉角之间的转换
根据旋转矩阵的对应关系进行转换，以321为例，p”=*p, R(N,θ)=