四元数定义：
q
=
q
w
+
q
x
i
+
q
y
j
+
q
z
k
=
[
q
w
q
x
q
y
q
z
]
\bold{q} = q_w+q_xi+q_yj+q_zk = \begin{bmatrix} q_w\\ q_x\\ q_y\\ q_z \end{bmatrix}
q=qw+qxi+qyj+qzk=⎣⎢⎢⎡qwqxqyqz⎦⎥⎥⎤
在 Hamilton 表示法中：
i
2
=
j
2
=
k
2
=
i
j
k
=
−
1
i
j
=
−
j
i
=
k
,
j
k
=
−
k
j
=
i
,
k
i
=
−
i
k
=
j
i^2=j^2=k^2=ijk=-1 \\ ij=-ji=k,\ jk=-kj=i,\ ki=-ik=j
i2=j2=k2=ijk=−1ij=−ji=k, jk=−kj=i, ki=−ik=j
在 JPL 表示法中，
−
i
j
=
j
i
=
k
-ij=ji=k
−ij=ji=k，Eigen3 中使用的是 Hamilton 表示法，所以下面公式都是用的 Hamilton 表示法。
四元数相乘：
p
⊗
q
=
[
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
]
\bold{p}\otimes\bold{q}= \begin{bmatrix} p_wq_w-p_xq_x-p_yq_y-p_zq_z\\ p_wq_x+p_xq_w+p_yq_z-p_zq_y\\ p_wq_y-p_xq_z+p_yq_w+p_zq_x\\ p_wq_z+p_xq_y-p_yq_x+p_zq_w \end{bmatrix}
p⊗q=⎣⎢⎢⎡pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw⎦⎥⎥⎤
四元数
q
\bold{q}
q 对向量
v
\bold{v}
v 进行旋转，首先将向量写成四元数形式（实部为0）：
p
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
\bold{p} = [0,x,y,z]^T = [0, \bold{v}]^T
p=[0,x,y,z]T=[0,v]T
然后计算：
p
′
=
q
p
q
−
1
\bold{p}’ = \bold{qpq}^{-1}
p′=qpq−1
可以验证
p
′
\bold{p}’
p′ 实部也是0，取出其虚部作为旋转后的向量
v
′
\bold{v}’
v′:
v
′
=
[
(
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
]
\bold{v}’=\begin{bmatrix} (q_w^2+q_x^2-q_y^2-q_z^2)x+2(q_xq_y-q_wq_z)y+2(q_wq_y+q_xq_z)z\\ 2(q_wq_z+q_xq_y)x+(q_w^2-q_x^2+q_y^2-q_z^2)y+2(q_yq_z-q_wq_x)z\\ 2(q_xq_z-q_wq_y)x+2(q_wq_x+q_yq_z)y+(q_w^2-q_x^2-q_y^2+q_z^2)z \end{bmatrix}
v′=⎣⎡(qw2+qx2−qy2−qz2)x+2(qxqy−qwqz)y+2(qwqy+qxqz)z2(qwqz+qxqy)x+(qw2−qx2+qy2−qz2)y+2(qyqz−qwqx)z2(qxqz−qwqy)x+2(qwqx+qyqz)y+(qw2−qx2−qy2+qz2)z⎦⎤