四元数乘法

四元数定义：
$$\mathbf{q}=q_w+q_{x}i+q_{y}j+q_{z}k=\left[\begin{array}{c}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right]$$
在 Hamilton 表示法中：
$$\begin{gathered} i^2=j^2=k^2=ijk=-1 \\ ij=-ji=k,jk=-kj=i,ki=-ik=j \end{gathered}$$
在 JPL 表示法中，$-ij=ji=k$，Eigen3 中使用的是 Hamilton 表示法，所以下面公式都是用的 Hamilton 表示法。

四元数相乘：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]$$

四元数 $\mathbf{q}$ 对向量 $\mathbf{v}$ 进行旋转，首先将向量写成四元数形式（实部为0）：
$$\mathbf{p}=[0,x,y,z{]}^T=[0,\mathbf{v}{]}^T$$
然后计算：
$${\mathbf{p}}^{\prime }={\mathbf{q}\mathbf{p}\mathbf{q}}^{-1}$$
可以验证 ${\mathbf{p}}^{\prime }$ 实部也是0，取出其虚部作为旋转后的向量 ${\mathbf{v}}^{\prime }$:
$${\mathbf{v}}^{\prime }=\left[\begin{array}{c}(q_w^2+q_x^2-q_y^2-q_z^2)x+2(q_x{q}_y-q_w{q}_z)y+2(q_w{q}_y+q_x{q}_z)z\\ 2(q_w{q}_z+q_x{q}_y)x+(q_w^2-q_x^2+q_y^2-q_z^2)y+2(q_y{q}_z-q_w{q}_x)z\\ 2(q_x{q}_z-q_w{q}_y)x+2(q_w{q}_x+q_y{q}_z)y+(q_w^2-q_x^2-q_y^2+q_z^2)z\end{array}\right]$$