四元數 旋轉矩陣 基礎運算

四元數相關運算

四元數 $q=\begin{bmatrix}q_x & q_y & q_z & q_w\end{bmatrix}^T = \begin{bmatrix}q_v & q_w\end{bmatrix}^T$ 則
$p \otimes q = \begin{bmatrix} p_w & -p_z & p_y & p_x \\ p_z & p_w & -p_x & p_y \\ -p_y & p_x & p_w & p_z \\ -p_x & -p_y & -p_z & p_w \end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z \\ q_w\end{bmatrix} = \begin{bmatrix} p_wq_x - p_zq_y + p_yq_z + p_xq_w \\ p_zq_x + p_wq_y - p_xq_z + p_yq_w \\ -p_yq_x + p_xq_y + p_wq_z + p_zq_w \\ -p_xq_x - p_yq_y - p_zq_z + p_wq_w \end{bmatrix} = \begin{bmatrix} p_wq_w - p_v^Tq_v \\ p_wq_v + q_wp_v + p_v \times q_v \end{bmatrix}$
$p_v \times q_v = \begin{bmatrix}p_x \\ p_y \\ p_z\end{bmatrix} \times \begin{bmatrix} q_x \\ q_y \\ q_z \end{bmatrix} = \begin{bmatrix}0 & -p_x & p_y \\ p_x & 0 & -p_z \\ -p_y & p_z & 0\end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z\end{bmatrix} = \begin{bmatrix} -p_xq_y + p_yq_z \\ p_xq_x - p_zq_z \\ -p_yq_x + p_zq_y \end{bmatrix}$
結合律：
$\left(p \otimes q\right) \otimes r = p \otimes \left(q \otimes r\right)$
分配律：
$p \otimes \left(q + r\right) = p \otimes q + p \otimes r$
$\left(p +ｑ\right) \otimes r = p \otimes r + q \otimes r$
左乘右乘：
$q_1 \otimes q_2 = \left[q_1\right]_Lq_2$
$q_1 \otimes q_2 = \left[q_2\right]_Rq_1$
$\left[q\right]_L = \begin{bmatrix} q_w & -q_z & q_y & q_x \\ q_z & q_w & -q_x & q_y \\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix} \left[q_v\right]_{\times} & q_v \\ \\ -q_v^{T} & 0 \end{bmatrix}$
$\left[q\right]_R = \begin{bmatrix} q_w & q_z & -q_y & q_x \\ -q_z & q_w & q_x & q_y \\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix}-\left[q_v\right]_{\times} & q_v \\ \\ -q_v^T & 0\end{bmatrix}$
$\left[a\right]_{\times} \triangleq \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}$
$\left[a\right]_{\times}^T = -\left[a\right]_{\times}$
$\left[a\right]_{\times}b = a \times b, \ \ \ \ \forall a, b \in \mathbb{R}$
Finally, since
$\left(q \otimes x\right) \otimes p = \left[p\right]_R\left[q\right]_L x$
$q\otimes\left(x\otimes p\right) = \left[q\right]_L\left[p\right]_Rx$
$\left[p\right]_R\left[q\right]_L = \left[q\right]_L\left[p\right]_R$
四元數的逆：
$q^{-1} = q^{*} \ \ \ \ \ \ \ q \otimes q^* = 1$
四元數的導數：
$\dot{q} = \frac{1}{2}q \otimes \omega$
指數映射：
$q = \exp\left(u \phi / 2\right)$
對數映射：
$\log\left(q\right) = u \phi / 2$
兩個四元數運算：
$q_1 \otimes q_2$
旋轉操作：
$q = \cos \left(\phi / 2\right) + u \sin \left(\phi / 2\right)$
旋轉作用：
$q \otimes x \otimes q^*$

旋轉矩陣相關運算

$R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) \\ 0 & \sin\left(\theta_x\right) & \cos\left(\theta_x\right) \end{bmatrix}$
$R_y = \begin{bmatrix} \cos\left(\theta_x\right) & 0 & \sin\left(\theta_x\right) \\ 0 & 1 & 0 \\ -\sin\left(\theta_x\right) & 0 & \cos\left(\theta_x\right) \end{bmatrix}$
$R_z = \begin{bmatrix} \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) & 0 \\ \sin\left(\theta_x\right) & \cos\left(\theta_x\right) & 0 \\ 0 & 0 & 1 \end{bmatrix}$
旋轉矩陣的逆：
$R^{-1} = R^T \ \ \ \ \ RR^{-1} = RR^T = I \ \ \ \ det\left(R\right) = 1$
旋轉矩陣的導數：
$\dot{R} = R\left[\omega\right]_{\times}$
指數映射：
$R = \exp\left(\left[u \phi\right]_{\times}\right)$
對數映射：
$\log\left(R\right) = \left[u \phi\right]_{\times}$
兩個旋轉矩陣運算：
$R_1 R_2$
旋轉操作：
$R = I + \sin\left(\phi \left[u\right]_{\times}\right) + \left(1 - \cos\left(\phi\right)\right)\left[u\right]_{\times}^2$
旋轉作用：
$Rx$