# 四元數
###### tags: `Math`
[TOC]

## 甚麼是四元數?

數學家**哈密頓**在1843年創立出的數學概念，通常記為$H$
大量用於電腦繪圖（及相關的圖像分析）上表示**三維物件的旋轉及方位**
### 小故事時間
數學家哈密頓先生一直想尋找**三維的複數係**，在經過柏林的布魯姆大橋時，他驚覺不應該為只為複數係加一個維度，而是**加兩個虛數維度**，為了紀念這件事情，橋上刻了這個故事以及這流傳幾百年的方程式。


### 四元數是[**複數的不可交換延伸**]

哈密頓將複數 $a+bi$ 延伸為四元數表示成:
$$
q = a+bi+cj+dk=
\left[
\begin{array}{}
a \\b \\c\\d\\
\end{array}
\right]
$$其中 $a,b,c,d$ 是實數，虛數單位 $i,j,k$ 滿足基本公式:
$$
i^2=j^2=k^2=ijk=-1
$$

四元數的乘法運算定義可由以下表格表示:


我們發現四元數**乘法的不可交換性**:
$$
ij=k \neq ji=-k\\
jk=i \neq kj=-i\\
ki=j \neq ik=-j\\
$$
相較於複數的乘法，四元數的乘法是不可交換的！！

另外我們可以將四元數 $q=a+bi+cj+dk$ 視為**純量─向量和**
$q=a+\mathbf{v}$，其中 $a$ 是純量，$\mathbf{v}=
\left[
\begin{array}{}
b \\c\\d\\
\end{array}
\right]\in\mathbb{R}^3$

## 基本運算
對於 $q_1=a_1+\mathbf{v}_1$ 和 $q_2=a_2+\mathbf{v}_2$
* 四元數加法：
$$\displaystyle q_1+q_2=(a_1+\mathbf{v}_1)+(a_2+\mathbf{v}_2)=(a_1+a_2)+(\mathbf{v}_1+\mathbf{v}_2)$$

* 四元數乘法:
$$\displaystyle\begin{aligned} q_1q_2&=(a_1+\mathbf{v}_1)(a_2+\mathbf{v}_2)\\ &=(a_1+b_1i+c_1j+d_1k)(a_2+b_2i+c_2j+d_2k)\\ &=(a_1a_2-b_1b_2-c_1c_2-d_1d_2)+(a_1b_2+b_1a_2+c_1d_2-d_1c_2)i\\ &~~~~+(a_1c_2-b_1d_2+c_1a_2+d_1b_2)j+(a_1d_2+b_1c_2-c_1b_2+d_1a_2)k\end{aligned}$$
將
$$\displaystyle\begin{aligned}\mathbf{v}_1\cdot\mathbf{v}_2&=b_1b_2+c_1c_2+d_1d_2\\
\mathbf{v}_1\times\mathbf{v}_2&=\begin{vmatrix} i&j&k\\ b_1&c_1&d_1\\ b_2&c_2&d_2 \end{vmatrix}=(c_1d_2-c_2d_1)i+(d_1b_2-d_2b_1)j+(b_1c_2-b_2c_1)k \end{aligned}$$
代入上式，得到
$$\displaystyle\begin{aligned} q_1q_2&=(a_1a_2-\mathbf{v}_1\cdot\mathbf{v}_2)+a_1(b_2i+c_2j+d_2k)+a_2(b_1i+c_1j+d_1k)\\ &~~~~+(c_1d_2-d_1c_2)i+(d_1b_2-d_2b_1)j+(b_1c_2-b_2c_1)k\\ &=(a_1a_2-\mathbf{v}_1\cdot\mathbf{v}_2)+(a_1\mathbf{v}_2+a_2\mathbf{v}_1+\mathbf{v}_1\times\mathbf{v}_2) \end{aligned}$$
另一方面，我們利用分配律可以得到
$$\displaystyle\begin{aligned} q_1q_2=(a_1+\mathbf{v}_1)(a_2+\mathbf{v}_2)=a_1a_2+a_1\mathbf{v}_2+a_2\mathbf{v}_1+\mathbf{v}_1\mathbf{v}_2\end{aligned}$$
這裡定義了向量乘法 $$\displaystyle\begin{aligned} \mathbf{v}_1\mathbf{v}_2=\mathbf{v}_1\times\mathbf{v}_2-\mathbf{v}_1\cdot\mathbf{v}_2\end{aligned}$$
表面上，這個運算非常奇怪，因為$\mathbf{v}_1\mathbf{v}_2$ 既非向量也不是純量。所以「向量乘法」僅有形式上的意義，並不具備幾何意義。

* 共軛
$$\displaystyle\begin{aligned} q^\ast&=a-\mathbf{v}\\
\end{aligned}$$
* 絕對值
$$\displaystyle\begin{aligned} \vert q\vert&=\sqrt{a^2+\mathbf{v}\cdot\mathbf{v}}=\sqrt{a^2+\Vert\mathbf{v}\Vert^2}=\vert q^\ast\vert\\ \end{aligned}$$

## 定義線性變換 $q\mathbf{x}q^\ast$
(p.s.這是為了鋪陳下面的旋轉公式！)

我們定義一個四元數於 $\mathbb{R}^3$ 的線性變換 $L_q$
給定一個四元數 $q=a+\mathbf{v}$，定義 $L_q:\mathbb{R}^3\to\mathbb{R}^3$為：
$$
\displaystyle L_q(\mathbf{x})=q\mathbf{x}q^\ast
$$其中 $\mathbf{x}\in\mathbb{R}^3$ 當作實部等於零的四元數 $0+\mathbf{x}$。
$$
\displaystyle\begin{aligned} q\mathbf{x}q^\ast&=(a+\mathbf{v})(0+\mathbf{x})(a-\mathbf{v})\\ &=(-\mathbf{v}\cdot\mathbf{x}+a\mathbf{x}+\mathbf{v}\times\mathbf{x})(a-\mathbf{v})\\ &=-(\mathbf{v}\cdot\mathbf{x})a-(a\mathbf{x}+\mathbf{v}\times\mathbf{x})\cdot(-\mathbf{v})+(-\mathbf{v}\cdot\mathbf{x})(-\mathbf{v})\\ &~~~~+a(a\mathbf{x}+\mathbf{v}\times\mathbf{x})+(a\mathbf{x}+\mathbf{v}\times\mathbf{x})\times(-\mathbf{v})\\ &=-(\mathbf{v}\cdot\mathbf{x})a+a(\mathbf{x}\cdot\mathbf{v})+(\mathbf{v}\times\mathbf{x})\cdot\mathbf{v}+(\mathbf{v}\cdot\mathbf{x})\mathbf{v}\\ &~~~~+a^2\mathbf{x}+a(\mathbf{v}\times\mathbf{x})-a(\mathbf{x}\times\mathbf{v})-(\mathbf{v}\times\mathbf{x})\times\mathbf{v}\\ &=(\mathbf{v}\cdot\mathbf{x})\mathbf{v}+a^2\mathbf{x}+2a(\mathbf{v}\times\mathbf{x})-\mathbf{x}(\mathbf{v}\cdot\mathbf{v})+\mathbf{v}(\mathbf{x}\cdot\mathbf{v})\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{x}+2(\mathbf{v}\cdot\mathbf{x})\mathbf{v}+2a(\mathbf{v}\times\mathbf{x}) \end{aligned}$$
故證明 $L_q(\mathbf{x})\in\mathbb{R}^3$。
下面列舉 $L_q$ 的三個重要性質:
1. $L_q:\mathbb{R}^3\to\mathbb{R}^3$ 是一個線性變換，也就是說，對於任意 $\mathbf{x},\mathbf{y}\in\mathbb{R}^3$ 和純量 $\alpha\in\mathbb{R}$，
$$
\displaystyle L_q(\mathbf{x}+\mathbf{y})=L_q(\mathbf{x})+L_q(\mathbf{y}),~~~L_q(\alpha\mathbf{x})=\alpha L_q(\mathbf{x})$$
證明:
使用上述 $L_q(\mathbf{x})$ 的展開式，
$$
\displaystyle\begin{aligned} L_q(\mathbf{x}+\mathbf{y})&=(a^2-\Vert\mathbf{v}\Vert^2)(\mathbf{x}+\mathbf{y})+2(\mathbf{v}\cdot(\mathbf{x}+\mathbf{y}))\mathbf{v}+2a(\mathbf{v}\times(\mathbf{x}+\mathbf{y}))\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{x}+(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{y}+2(\mathbf{v}\cdot\mathbf{x})\mathbf{v}+2(\mathbf{v}\cdot\mathbf{y})\mathbf{v}+2a(\mathbf{v}\times\mathbf{x})+2a(\mathbf{v}\times\mathbf{y})\\ &=L_q(\mathbf{x})+L_q(\mathbf{y}) \end{aligned}
$$
且
$$
\displaystyle\begin{aligned} L_q(\alpha\mathbf{x})&=(a^2-\Vert\mathbf{v}\Vert^2)(\alpha\mathbf{x})+2(\mathbf{v}\cdot(\alpha\mathbf{x}))\mathbf{v}+2a(\mathbf{v}\times(\alpha\mathbf{x}))\\ &=\alpha(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{x}+2\alpha(\mathbf{v}\cdot\mathbf{x})\mathbf{v}+2\alpha a(\mathbf{v}\times\mathbf{x})\\ &=\alpha L_q(\mathbf{x}). \end{aligned}
$$
2. 若 $q=a+\mathbf{v}$ 為單位四元數，即 $\vert q\vert=1$，則每一 $\mathbf{x}\in\mathbb{R}^3$ 滿足 $\Vert L_q(\mathbf{x})\Vert=\Vert\mathbf{x}\Vert$。
直接計算即可驗證，如下：
$$
\displaystyle \Vert L_q(\mathbf{x})\Vert=\Vert q\mathbf{x}q^\ast\Vert=\vert q\vert~\Vert\mathbf{x}\Vert~\vert q^\ast\vert=\Vert\mathbf{x}\Vert
$$
3. 若 $q=a+\mathbf{v}$ 為單位四元數，則 $L_q(\alpha\mathbf{v})=\alpha\mathbf{v}，\alpha\in\mathbb{R}$。
證明於下：
$$
\displaystyle\begin{aligned} L_q(\alpha\mathbf{v})&=q(\alpha\mathbf{v})q^\ast\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\alpha\mathbf{v}+2(\mathbf{v}\cdot \alpha\mathbf{v})\mathbf{v}+2a(\mathbf{v}\times \alpha\mathbf{v})\\ &=\alpha(a^2+\Vert\mathbf{v}\Vert^2)\mathbf{v}=\alpha\vert q\vert^2\mathbf{v}=\alpha\mathbf{v}. \end{aligned}
$$

## 羅德里格旋轉公式
若 $q=a+\mathbf{v}$ 為單位四元數，即 $\vert q\vert^2=a^2+\Vert\mathbf{v}\Vert^2=1$。
設 $a^2=\cos^2\frac{\theta}{2}$ 且 $\Vert\mathbf{v}\Vert^2=\sin^2\frac{\theta}{2}$。
因此，存在 $\theta\in[0,2\pi)$ 使得 $a=\cos\frac{\theta}{2}$ 且 $\Vert\mathbf{v}\Vert=\sin\frac{\theta}{2}\ge 0$。
令 $\mathbf{u}=\frac{\mathbf{v}}{\Vert\mathbf{v}\Vert}$。每一個單位四元數 $q=a+\mathbf{v}$ 定可表示為
$$
\displaystyle q=\cos\frac{\theta}{2}+\sin\frac{\theta}{2}\mathbf{u}
$$

其中 $\Vert\mathbf{u}\Vert=1$。
我們想要證明 : 線性變換 $L_q(\mathbf{x})=q\mathbf{x}q^\ast$ 代表 $\mathbf{x}$以單位向量 $\mathbf{u}$ 為轉軸，逆時針旋轉 $\theta$ 徑度。
證明流程:
將給定向量 $\mathbf{x}$ 分解成 $\mathbf{x}=\mathbf{z}+\mathbf{n}$，其中 $\mathbf{z}$ 是 $\mathbf{x}$ 在轉軸 $\mathbf{u}$ 的正交投影，$\mathbf{n}$ 代表垂直於 $\mathbf{u}$ 的成分。因為 $L_q(\mathbf{x})=L_q(\mathbf{z}+\mathbf{n})=L_q(\mathbf{z})+L_q(\mathbf{n})$，我們要證明 $L_q$ 不改變 $\mathbf{z}$ 且 $L_q$ 將 $\mathbf{n}$ 逆時針旋轉 $\theta$ 角度。
1. 因為存在唯一 $\alpha\in\mathbb{R}$ 使 $\mathbf{z}=\alpha\mathbf{u}$，由前述性質 (2) 可知 $L_q(\mathbf{z})=\alpha\mathbf{}L_q(\mathbf{u})=\alpha\mathbf{u}=\mathbf{z}$
2. 利用 $\mathbf{n}\perp\mathbf{u}$，即 $\mathbf{n}\perp\mathbf{v}$，可得
$$
\displaystyle\begin{aligned} L_q(\mathbf{n})&=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{n}+2(\mathbf{v}\cdot\mathbf{n})\mathbf{v}+2a(\mathbf{v}\times\mathbf{n})\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{n}+2a(\mathbf{v}\times\mathbf{n})\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{n}+2a\Vert\mathbf{v}\Vert(\mathbf{u}\times\mathbf{n})\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{n}+2a\Vert\mathbf{v}\Vert\mathbf{n}_{\perp} \end{aligned}
$$

最後，將 $a=\cos\frac{\theta}{2}$ 和 $\Vert\mathbf{v}\Vert=\sin\frac{\theta}{2}$ 代入 $L_q(\mathbf{n})$ 的表達式，使用倍角公式，
$$
\displaystyle\begin{aligned} L_q(\mathbf{n})&=\left(\cos^2\frac{\theta}{2}-\sin^2\frac{\theta}{2}\right)\mathbf{n}+\left(2\cos\frac{\theta}{2}\sin\frac{\theta}{2}\right)\mathbf{n}_\perp\\ &=\cos\theta\mathbf{n}+\sin\theta\mathbf{n}_\perp,\end{aligned}
$$
證明 $L_q(\mathbf{n})$ 即是垂直於轉軸 $\mathbf{u}$ 的平面上 $\mathbf{n}$ 逆時針旋轉 $\theta$ 而得的向量。

將單位四元數 $q=\cos\frac{\theta}{2}+\sin\frac{\theta}{2}\mathbf{u}$ 代入旋轉變換 $L_q(\mathbf{x})$ 的展開式，套用倍角公式和半角公式，推得
$$
\displaystyle\begin{aligned} L_q(\mathbf{x})&=\left(\cos^2\frac{\theta}{2}-\sin^2\frac{\theta}{2}\right)\mathbf{x}+2\left(\sin\frac{\theta}{2}\mathbf{u}\cdot\mathbf{x}\right)\sin\frac{\theta}{2}\mathbf{u}+2\cos\frac{\theta}{2}\left(\sin\frac{\theta}{2}\mathbf{u}\times\mathbf{x}\right)\\ &=\cos\theta\mathbf{x}+(1-\cos\theta)(\mathbf{u}\cdot\mathbf{x})\mathbf{u}+\sin\theta(\mathbf{u}\times\mathbf{x}). \end{aligned}
$$

上式稱為羅德里格旋轉公式 (Rodrigues’ rotation formula)
### 矩陣形式
給定 $\mathbf{u}=(u_1,u_2,u_3)$ 和 $\mathbf{x}=(x_1,x_2,x_3)$，外積運算 $\mathbf{u}\times \mathbf{x}$ 可用矩陣乘法表示如下：
$$
\displaystyle \mathbf{u}\times\mathbf{x}=\begin{bmatrix} u_2x_3-u_3x_2\\ u_3x_1-u_1x_3\\ u_1x_2-u_2x_1 \end{bmatrix}=\left[\!\!\begin{array}{rrr} 0&-u_3&u_2\\ u_3&0&-u_1\\ -u_2&u_1&0 \end{array}\!\!\right]\begin{bmatrix} x_1\\ x_2\\ x_3 \end{bmatrix}=\begin{bmatrix} \mathbf{u} \end{bmatrix}_{\times}\mathbf{x}，
$$
上面我們定義「外積矩陣」為 $\begin{bmatrix}\mathbf{u} \end{bmatrix}_{\times}=\left[\!\!\begin{array}{rrr} 0&-u_3&u_2\\ u_3&0&-u_1\\ -u_2&u_1&0 \end{array}\!\!\right]$。
再將內積改寫成 $\mathbf{u}\cdot\mathbf{x}=\mathbf{u}^T\mathbf{x}$，羅德里格旋轉公式可表示成矩陣形式：
$$
\displaystyle\begin{aligned} L_q(\mathbf{x})&=\cos\theta I\mathbf{x}+(1-\cos\theta)\mathbf{u}\mathbf{u}^T\mathbf{x}+\sin\theta\begin{bmatrix} \mathbf{u} \end{bmatrix}_{\times}\mathbf{x}\\ &=\left(\cos\theta I+(1-\cos\theta)\mathbf{u}\mathbf{u}^T+\sin\theta\begin{bmatrix} \mathbf{u} \end{bmatrix}_{\times}\right)\mathbf{x}. \end{aligned}
$$
令 $R_\mathbf{u}(\theta)$ 表示旋轉軸為 $\mathbf{u}$，轉角等於 $\theta$ 的 $3\times 3$ 階旋轉矩陣。從上面的羅德里格旋轉公式，可得
$$
\displaystyle \begin{aligned} R_\mathbf{u}(\theta)&=\cos\theta I+(1-\cos\theta)\mathbf{u}\mathbf{u}^T+\sin\theta\begin{bmatrix} \mathbf{u} \end{bmatrix}_{\times}\\ &=\begin{bmatrix} u_1u_1v\theta+c\theta&u_1u_2v\theta-u_3s\theta&u_1u_3v\theta+u_2s\theta\\ u_1u_2v\theta+u_3s\theta&u_2u_2v\theta+c\theta&u_2u_3v\theta-u_1s\theta\\ u_1u_3v\theta-u_2s\theta&u_2u_3v\theta+u_1s\theta&u_3u_3v\theta+c\theta \end{bmatrix},\end{aligned}
$$
其中 $c\theta=\cos\theta$，$s\theta=\sin\theta$，$v\theta=1-\cos\theta$。轉角 $\theta$ 的大小由右手法則決定，即大拇指朝向轉軸的正方向 $\mathbf{u}$，其餘四根手指的彎曲方向為旋轉方向。

旋轉矩陣的另一種表示法:
給定一個單位四元數 $q=a+\mathbf{v}$，其中 $\mathbf{v}=(b,c,d)^T$，利用 $L_q(\mathbf{x})$ 公式亦可得到以 $a,b,c,d$ 表示的旋轉矩陣 $R$，如下：
$$
\displaystyle\begin{aligned} R\mathbf{x}&=L_q(\mathbf{x})=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{x}+2(\mathbf{v}\cdot\mathbf{x})\mathbf{v}+2a(\mathbf{v}\times\mathbf{x})\\ &=(a^2-\Vert\mathbf{v}\Vert^2)\mathbf{x}+2\mathbf{v}\mathbf{v}^T\mathbf{x}+2a\begin{bmatrix} \mathbf{v}\end{bmatrix}_\times\mathbf{x}\\ &=\left((a^2-\Vert\mathbf{v}\Vert^2)I+2\mathbf{v}\mathbf{v}^T+2a\begin{bmatrix} \mathbf{v}\end{bmatrix}_\times\right)\mathbf{x}\\ &=\begin{bmatrix} a^2+b^2-c^2-d^2&2bc-2ad&2bd+2ac\\ 2bc+2ad&a^2-b^2+c^2-d^2&2cd-2ab\\ 2bd-2ac&2cd+2ab&a^2-b^2-c^2+d^2 \end{bmatrix}\mathbf{x}. \end{aligned}$$
## [實際應用]參考論文
### １. "以追星儀配合一或二軸陀螺儀進行姿態判定之研究"
發展一套新式估測演算法用以整合追星儀與陀螺儀的輸出資訊來進
行衛星姿態判定。
* 選定座標系。內文介紹很多常用坐標系，但都是衛星、地心相關。EX:地心座標系...
* 姿態表示法。本報告主要以四元數法為主，尤拉角法為輔。
**原因** : 現當俯仰角(註1)為+/- 90∘時，也就是載具垂直向上或垂直向下飛行，尤拉角法轉換矩陣會發生奇異性(註2)。故利用此方程式來計算姿態的轉換矩陣時，俯仰角會有所限制。

註:
俯仰角:在航空學中俯仰角表示為機鼻的上/下的旋轉，也等於對附體座標的 y 軸來旋轉。
奇異性:Not full rank，detA = 0。如果A為奇異矩陣，則$Ax=0$有無窮解，$Ax=b$有無窮解或者無解。

大致流程圖 :

#### 補充追星儀
令四元數向量為
$Q=q_0+iq_1 +jq_2 +kq_3$ 其中

追星儀可測得以上四項的值

#### 尤拉角與四元數的轉換



**轉換原因** : 因為尤拉角相較於四元數在平常看時較有物理意義

**論文結果**: 預測姿態結果相當良好!

### 2. 以慣性感測訊號進行步態分析與跌倒偵測
希望藉由至於脛骨、腳踝等的慣性感測量器測出帕金森氏症的主要症狀，包括步距、步頻、左右步協調等，以及跌倒判斷演算法。

**工具**: 加速規(量測加速度)、陀螺儀(量測角加速度訊號)


基本卡爾曼濾波器是限制在線性的假設之下。上一篇用的擴展卡爾曼濾波器觀測模型不需要是狀態的線性函數，可替換為（可微的）函數。

**論文結果**:預測的結果不太準確

## 歐拉角

任何一個三維空間旋轉皆可表示為三個基本旋轉的複合變換。
採用右手法則，設 $\theta$ 為轉角，稱為歐拉角，$R_X(\theta)，R_Y(\theta)，R_Z(\theta)$ 分別代表對 $X，Y$ 和 $Z$ 軸的旋轉矩陣，如下：
$$
\displaystyle \begin{aligned} R_X(\theta)&=\begin{bmatrix} 1&0&0\\ 0&\cos\theta&-\sin\theta\\ 0&\sin\theta&\cos\theta \end{bmatrix}\\ R_Y(\theta)&=\begin{bmatrix} \cos\theta&0&\sin\theta\\ 0&1&0\\ -\sin\theta&0&\cos\theta \end{bmatrix}\\ R_Z(\theta)&=\begin{bmatrix} \cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1 \end{bmatrix}.\end{aligned}$$
### 性質
1. 歐拉角是按照一個固定的坐標軸的順序旋轉的，因此不同的順序會造成不同結果
2. 歐拉角如果在旋轉中不幸讓某些坐標軸重合，就會發生萬向鎖現象，這時就會丟失一個方向上的旋轉能力，也就是說在這種狀態下，我們無論怎么旋轉，都不可能得到某些想要的結果，除非打破原先的旋轉順序或者同時旋轉三個軸。


萬向鎖解決辦法：將歐拉角轉換為四元數，這種做法缺點在於消耗內存

| 四元數 | 歐拉角 |
| ----- | ----- |
| 複雜 | 直觀 |
| 可以避免萬向鎖|萬向鎖|
|存儲空間小，計算效率高|不同的旋轉順序下會有不同的結果|
|不能表示超過180度的旋轉|可以旋轉大於180度的角度|

## 三元數呢?
假設三元數存在，它是以 a+bi+cj的形式表示的數，其中 a、b 和 c 皆是實數，那我們要怎麼表示ij 這個三元數呢
設 ij=a+bi+cj，其中 a、b 和 c 是實數。

則實部、i 、j 不互相獨立。