二维直角坐标的函数从原点旋转$\theta$°的旋转变换为: 对于函数上每点$(x,y)$右乘$R$:

$$(x', y') = (x, y) * R$$

得到新坐标$(x', y')$

其中$R$是二维旋转矩阵

$$R = \left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{matrix}\right]$$

三维

绕 x-轴的主动旋转

$$R_x(\theta_x) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \theta_x & -\sin \theta_x \\ 0 & \sin \theta_x & \cos \theta_x \end{bmatrix} = \exp \left( \theta_x \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix} \right)$$

这里的 $\theta_x$ 是 roll 角，和右手螺旋的方向相反（在 yz 平面顺时针）。

绕 y-轴的主动旋转

$$R_y(\theta_y) = \begin{bmatrix} \cos \theta_y & 0 & \sin \theta_y \\ 0 & 1 & 0 \\ -\sin \theta_y & 0 & \cos \theta_y \end{bmatrix} = \exp \left( \theta_y \begin{bmatrix} 0 & 0 & 1 \\ 0 & 1 & 0 \\ -1 & 0 & 0 \end{bmatrix} \right)$$

这里的 $\theta_y$ 是 pitch 角，和右手螺旋的方向相反（在 zx 平面顺时针）。

绕 z-轴的主动旋转

$$R_z(\theta_z) = \begin{bmatrix} \cos \theta_z & -\sin \theta_z & 0 \\ \sin \theta_z & \cos \theta_z & 0 \\ 0 & 0 & 1 \end{bmatrix} = \exp \left( \theta_z \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \right)$$

这里的 $\theta_z$ 是 yaw 角，和右手螺旋的方向相反（在 xy 平面顺时针）。