单位复数与2D旋转
对于任意复数\(z \in \mathbb{C}\)都可以表示为\(z = a + bi\)称之为复数的实部和虚部。其中\(z = a + bi\)其中\(i^2 = -1\)其实就是对\(\left[1,i \right]\)基(Basix)的线性组合(Linear Combination)， 因此也可以用向量表示：
\[
z = \left[\begin{matrix} a \\b \end{matrix}\right]
\]
对于复数乘法：
\[
\begin{aligned}
z_1z_2 &= (a + bi)(c + di) \\
&= ac + adi + bci + bdi^2 \\
&= ac - bd + adi + bci \\
&= ac -bd + (bc + ad)i \\
&= \left[ \begin{matrix} a & -b \\ b & a \end{matrix} \right] \left[ \begin{matrix} c \\ d \end{matrix} \right]
\end{aligned}
\]
可以看到左侧矩阵所代表的变换是和复数相乘等价的。
若对上述矩阵做一下变换, 将模长提出矩阵：
\[
\left[\begin{array}{cc}
a & -b \\
b & a
\end{array}\right]=\sqrt{a^{2}+b^{2}}\left[\begin{array}{cc}
\frac{a}{\sqrt{a^{2}+b^{2}}} & \frac{-b}{\sqrt{a^{2}+b^{2}}} \\
\frac{b}{\sqrt{a^{2}+b^{2}}} & \frac{a}{\sqrt{a^{2}+b^{2}}}
\end{array}\right]
\]
可以看到模长正是三角形斜边，a，b正是直角边。将上式变形：
\[
\begin{aligned}
\left[\begin{array}{cc}
a & -b \\
b & a
\end{array}\right] &=\sqrt{a^{2}+b^{2}}\left[\begin{array}{cc}
\cos (\theta) & -\sin (\theta) \\
\sin (\theta) & \cos (\theta)
\end{array}\right] \\
&=\|z\|\left[\begin{array}{cc}
\cos (\theta) & -\sin (\theta) \\
\sin (\theta) & \cos (\theta)
\end{array}\right] \\
&=\|z\| \cdot I\left[\begin{array}{cc}
\cos (\theta) & -\sin (\theta) \\
\sin (\theta) & \cos (\theta)
\end{array}\right] \\
&=\left[\begin{array}{cc}
\|z\| & 0 \\
0 & \|z\|
\end{array}\right]\left[\begin{array}{cc}
\cos (\theta) & -\sin (\theta) \\
\sin (\theta) & \cos (\theta)
\end{array}\right]
\end{aligned}
\]
可以看到上式分为两个矩阵，可以理解为缩放矩阵和旋转矩阵的组合。若复数z为单位复数，则上式就是我们熟悉的旋转矩阵，即单位复数可以表示旋转：
\[
z = cos \theta + i sin \theta
\]
根据欧拉公式(Euler`s Formula):
\[
cos \theta + i sin \theta = e^{i\theta}
\]
则复数可以表示为：
\[
\begin{aligned}
z &=\|z\|\left[\begin{array}{cc}
\cos (\theta) & -\sin (\theta) \\
\sin (\theta) & \cos (\theta)
\end{array}\right] \\
&= \|z\|(cos \theta + i sin \theta) \\
&= \|z\|e^{i\theta} \\
&= re^{i\theta}
\end{aligned}
\]
则得到复数的极坐标形式。同样的r可以认为是缩放因子，\(\theta\)为旋转角度。同样可以看到这里指数映射和对数映射反映了旋转角度\(\theta\)与复数\(z\)之间的关系。其中\(i\theta\)为单位圆的切线。
\[
\mathbf{z} = exp(i\theta) \\
i\theta = log(\mathbf{z})
\]
单位四元数与3D旋转
四元数定义成实部+虚部的形式(scalar + vector)：
\[
Q = q_w + q_x i + q_y j + q_z k = q_w + \boldsymbol{q}_v
\]
因此表示一个四元数为：
\[
\boldsymbol{q} = \left[ \begin{matrix} q_w \\ \boldsymbol{q}_v \end{matrix} \right] = \left[q_w \ q_x \ q_y \ q_w \right]^T
\]
其中：
\[
\begin{aligned}
&i^2 = j^2 = k^2 = ijk = -1\\
& ij = -ji = k \\
& jk = -kj = i \\
& ki = -ik = j
\end{aligned}
\]
四元数的乘法定义如下：
\[
\mathbf{p} \otimes \mathbf{q}=\left[\begin{array}{l}
{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] = \left[\begin{array}{c}
{p_{w} q_{w}-\mathbf{p}_{v}^{\top} \mathbf{q}_{v}} \\
{p_{w} \mathbf{q}_{v}+q_{w} \mathbf{p}_{v}+\mathbf{p}_{v} \times \mathbf{q}_{v}}
\end{array}\right]
\]
可以写成矩阵的形式：
\[
\boldsymbol{q}_1 \otimes \boldsymbol{q}_2 = \left[\boldsymbol{q}_1\right]_L \boldsymbol{q}_2 \quad and \quad \boldsymbol{q}_1 \otimes \boldsymbol{q}_2 = \left[ \boldsymbol{q}_2 \right]_R \boldsymbol{q}_1
\]
其中：
\[
\begin{aligned}
& [\mathbf{q}]_{L}
=\left[\begin{array}{cccc}
{q_{w}} & {-q_{x}} & {-q_{y}} & {-q_{z}} \\
{q_{x}} & {q_{w}} & {-q_{z}} & {q_{y}} \\
{q_{y}} & {q_{z}} & {q_{w}} & {-q_{x}} \\
{q_{z}} & {-q_{y}} & {q_{x}} & {q_{w}}
\end{array}\right]
= q_{w} \mathbf{I}+\left[\begin{array}{cc}
{0} & {-\mathbf{q}_{v}^{\top}} \\
{\mathbf{q}_{v}} & {\left[\mathbf{q}_{v}\right]_{\times}}
\end{array}\right] \\
& [\mathbf{q}]_{R}
=\left[\begin{array}{cccc}
{q_{w}} & {-q_{x}} & {-q_{y}} & {-q_{z}} \\
{q_{x}} & {q_{w}} & {q_{z}} & {-q_{y}} \\
{q_{y}} & {-q_{z}} & {q_{w}} & {q_{x}} \\
{q_{z}} & {q_{y}} & {-q_{x}} & {q_{w}}
\end{array}\right]
= q_{w} \mathbf{I}+\left[\begin{array}{cc}
{0} & {-\mathbf{q}_{v}^{\top}} \\
{\mathbf{q}_{v}} & {-\left[\mathbf{q}_{v}\right]_{\times}}
\end{array}\right]
\end{aligned}
\]
因此：
\[
(\mathbf{q} \otimes \mathbf{x}) \otimes \mathbf{p} = \left[ \mathbf{p} \right]_R \left[ \mathbf{q} \right]_L \mathbf{x} = \mathbf{q} \otimes (\mathbf{x} \otimes \mathbf{p}) = \left[ \mathbf{q} \right]_L \left[ \mathbf{p} \right]_R \mathbf{x}
\]
将\(cos \theta/2 = q_w \quad \mathbf{u} sin \theta/2 = \mathbf{q}_v\)带入罗德里格斯公式得：
\[
\begin{aligned}
\mathbf{D} &= \mathbf{I}+\sin \phi(\mathbf{u} \times)+(1-\cos \phi)(\mathbf{u} \times)^{2} \\
&= \mathbf{I} + 2 sin \theta/2 \ cos \theta/2 (\mathbf{u}_\times) + 2 sin^2 \theta/2 \ (\mathbf{u}_\times)^2 \\
&= \mathbf{I} + 2 cos \theta/2 (sin \theta/2 \ \mathbf{u}_\times) + 2 (sin \theta/2 \ \mathbf{u}_\times)^2 \\
&= \mathbf{I} + 2 q_w([\mathbf{q}_v]_\times) + 2([\mathbf{q}_v]_\times)^2 \\
&= \left[\begin{matrix}
1 - 2(q^2_y + q^2_z) & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\
2(q_xq_y + q_wq_z) & 1 - 2(q^2_x + q^2_z) & 2(q_yq_z - q_wq_x) \\
2(q_xq_z - q_wq_y) & 2(q_yq_z + q_wq_x) & 1 - 2(q^2_x + q^2_y)
\end{matrix}\right] \\
&= \left[\begin{matrix}
q^2_w + q^2_x - q^2_y - q^2_z & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\
2(q_xq_y + q_wq_z) & q^2_w - q^2_x + q^2_y - q^2_z & 2(q_yq_z - q_wq_x) \\
2(q_xq_z - q_wq_y) & 2(q_yq_z + q_wq_x) & q^2_w - q^2_x - q^2_y + q^2_z
\end{matrix}\right]
\end{aligned}
\tag{1.4.7}\label{1.4.7}
\]
建立了单位四元数与方向余弦矩阵之间的关系。
常用李群李代数求导
Reference
[1] A micro Lie theory for state estimation in robotics [2] Quaternion kinematics for the error-state Kalman filter [3] https://github.com/artivis/manif