AHRS Quaternions and Rotations

Quaternions

Quaternion์€ ์‚ฌ์›์ˆ˜ ๋˜๋Š” ์ฟผํ„ฐ๋‹ˆ์–ธ์ด๋ผ๊ณ  ๋ถ€๋ฅด๋ฉฐ, ๋ณต์†Œ์ˆ˜๋ฅผ ํ™•์žฅํ•ด ๋งŒ๋“  ์ˆ˜ ์ฒด๊ณ„์ž…๋‹ˆ๋‹ค.

i2=j2=k2=ijk=โˆ’1q=xโ€‰i+yโ€‰j+zโ€‰k+wqโ€พ=[xyzw]T\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{i}\mathbf{j}\mathbf{k} = -1 \\ q = x\,\mathbf{i} + y\,\mathbf{j} + z\,\mathbf{k} + w \\ \underline{q} = \begin{bmatrix}x & y & z & w\end{bmatrix}^T

Norm

โˆฅqโˆฅ=x2+y2+z2+w2\begin{Vmatrix}q\end{Vmatrix} = \sqrt{x^2 + y^2 + z^2 + w^2}

Conjugate quaternion

qห‰=โˆ’xโ€‰iโˆ’yโ€‰jโˆ’zโ€‰k+w\bar{q} = -x\,\mathbf{i} - y\,\mathbf{j} - z\,\mathbf{k} + w
pqโ€พ=qห‰pห‰\overline{pq} = \bar{q}\bar{p}

Pure quaternion

qโ€พ=[xyz0]T\underline{q} = \begin{bmatrix}x & y & z & 0\end{bmatrix}^T

Identity quaternion

Iโ€พ=[0001]T\underline{I} = \begin{bmatrix}0 & 0 & 0 & 1\end{bmatrix}^T

Inverse quaternion

qโˆ’1=qห‰โˆฃโˆฃqโˆฃโˆฃ2(qโˆ’1=qห‰ifย ||q||ย =ย 1)q^{-1} = \cfrac{\bar{q}}{||q||^2} \quad \begin{pmatrix} q^{-1} = \bar{q} & \text{if ||q|| = 1} \end{pmatrix}

Hamilton product

pq=L(p)qโ€พ=[pwโˆ’pzpypxpzpwโˆ’pxpyโˆ’pypxpwpzโˆ’pxโˆ’pyโˆ’pzpw][qxqyqzqw]=R(q)pโ€พ=[qwqzโˆ’qyqxโˆ’qzqwqxqyqyโˆ’qxqwqzโˆ’qxโˆ’qyโˆ’qzqw][pxpypzpw]\begin{aligned} pq & = L(p)\underline{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} \\ & = R(q)\underline{p} & = \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} \begin{bmatrix} p_x \\ p_y \\ p_z \\ p_w \end{bmatrix} \end{aligned}

Rotations

Rotate Vector

aโƒ—(โˆฃโˆฃaโƒ—โˆฃโˆฃ=1)\vec{a}\left(||\vec{a}|| = 1\right)๋ฅผ ๊ธฐ์ค€์œผ๋กœ ๋ฒกํ„ฐ๋ฅผ ฮธ\theta๋งŒํผ ํšŒ์ „์‹œํ‚ค๋Š” ๋ฐ ํ•„์š”ํ•œ ์ฟผํ„ฐ๋‹ˆ์–ธ์€ ์•„๋ž˜์™€ ๊ฐ™์Šต๋‹ˆ๋‹ค.

qโ€พ(aโƒ—,ฮธ)=[aโƒ—sinโก(ฮธ2)cosโก(ฮธ2)]T\underline{q}\left(\vec{a},\theta\right) = \begin{bmatrix} \vec{a}\sin{\left(\cfrac{\theta}{2}\right)} & \cos{\left(\cfrac{\theta}{2}\right)} \end{bmatrix}^T

๋ฒกํ„ฐ์™€ ์ฟผํ„ฐ๋‹ˆ์–ธ ์—ฐ์‚ฐ ์‹œ ๋ฒกํ„ฐ๋Š” pure quaternioin์œผ๋กœ ๋‚˜ํƒ€๋‚ผ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

v=vxโ€‰i+vyโ€‰j+vzโ€‰kv = v_x\,\mathbf{i} + v_y\,\mathbf{j} +v_z\,\mathbf{k}
vโ€พ=[vxvyvz0]\underline{v}= \begin{bmatrix} v_x & v_y & v_z & 0 \end{bmatrix}

aโƒ—(โˆฃโˆฃaโƒ—โˆฃโˆฃ=1)\vec{a}\left(||\vec{a}|| = 1\right)๋ฅผ ๊ธฐ์ค€์œผ๋กœ vโƒ—\vec{v}๋ฅผ ฮธ\theta๋งŒํผ ํšŒ์ „์‹œํ‚จ vโƒ—โ€ฒ\vec{v}'๋ฅผ ๊ตฌํ•˜๋Š” ์‹์€ ์•„๋ž˜์™€ ๊ฐ™์Šต๋‹ˆ๋‹ค.

vโ€ฒ=qvqห‰vโ€พโ€ฒ=R(qห‰)L(q)vโ€พ=[1โˆ’2(qy2+qz2)2(qxqyโˆ’qzqw)2(qxqz+qyqw)02(qxqy+qzqw)1โˆ’2(qx2+qz2)2(qyqzโˆ’qxqw)02(qxqzโˆ’qyqw)2(qyqz+qxqw)1โˆ’2(qx2+qy2)00001][vxvyvz0]\begin{aligned} v' & = qv \bar{q} \\ \underline{v}' & = R(\bar{q}) L(q) \underline{v} \\ & = \begin{bmatrix} 1-2({q_y}^2+{q_z}^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) & 0 \\ 2(q_x q_y + q_z q_w) & 1-2({q_x}^2+{q_z}^2) & 2(q_y q_z - q_x q_w) & 0 \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1-2({q_x}^2+{q_y}^2) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ v_z \\ 0 \end{bmatrix} \end{aligned}

์—ฐ์†์ ์ธ ํšŒ์ „์€ ์•„๋ž˜์™€ ๊ฐ™์ด ๊ตฌํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

v1=q01v0qห‰01v2=q12v1qห‰12=q12q01v0qห‰01qห‰12v3=q23v2qห‰23=q23q12q01v0qห‰01qห‰12qห‰23โ‹ฎโ‹ฎโ‹ฎ\begin{aligned} v_{1} & = & q_{0}^{1} v_{0} \bar{q}_{0}^{1} & & \\ v_{2} & = & q_{1}^{2} v_{1} \bar{q}_{1}^{2} & = & q_{1}^{2} q_{0}^{1} v_{0} \bar{q}_{0}^{1} \bar{q}_{1}^{2} \\ v_{3} & = & q_{2}^{3} v_{2} \bar{q}_{2}^{3} & = & q_{2}^{3} q_{1}^{2} q_{0}^{1} v_{0} \bar{q}_{0}^{1} \bar{q}_{1}^{2} \bar{q}_{2}^{3} \\ \vdots & & \vdots & & \vdots \end{aligned}
vk=qkโˆ’1kqkโˆ’2kโˆ’1โ‹ฏq01v0qห‰01โ‹ฏqห‰kโˆ’2kโˆ’1qห‰kโˆ’1kv_{k} = q_{k-1}^{k} q_{k-2}^{k-1} \cdots q_{0}^{1} v_{0} \bar{q}_{0}^{1} \cdots \bar{q}_{k-2}^{k-1} \bar{q}_{k-1}^{k}
q0k=qkโˆ’1kqkโˆ’2kโˆ’1โ‹ฏq01q_{0}^{k} = q_{k-1}^{k} q_{k-2}^{k-1} \cdots q_{0}^{1}

Rotate Frame

์ฒ˜์Œ์— A์™€ B ์ขŒํ‘œ๊ณ„๋Š” ๋™์ผํ–ˆ๋‹ค๊ฐ€ B ์ขŒํ‘œ๊ณ„๊ฐ€ aโƒ—(โˆฃโˆฃaโƒ—โˆฃโˆฃ=1)\vec{a}\left(||\vec{a}|| = 1\right)๋ฅผ ๊ธฐ์ค€์œผ๋กœ ฮธ\theta๋งŒํผ ํšŒ์ „ํ–ˆ๋‹ค๊ณ  ๊ฐ€์ •ํ•˜๊ฒ ์Šต๋‹ˆ๋‹ค.

Avโƒ—{}^{A}\vec{v}๋Š” A์—์„œ ๊ด€์ฐฐํ•œ vโƒ—\vec{v}์ž…๋‹ˆ๋‹ค. Avโƒ—{}^{A}\vec{v}๋Š” ํšŒ์ „ํ•˜๊ธฐ ์ „ B ์ขŒํ‘œ๊ณ„์—์„œ ๊ด€์ฐฐํ•œ vโƒ—\vec{v}๋ผ๊ณ  ์ƒ๊ฐํ•  ์ˆ˜๋„ ์žˆ์Šต๋‹ˆ๋‹ค.

Bvโƒ—{}^{B}\vec{v}๋Š” ํšŒ์ „ ํ›„ B์—์„œ ๊ด€์ฐฐํ•œ vโƒ—\vec{v}์ž…๋‹ˆ๋‹ค.

B ์ขŒํ‘œ๊ณ„ ์œ„์— ์žˆ๋Š” ๊ด€์ฐฐ์ž ์ž…์žฅ์—์„œ ๋ณด๋ฉด Avโƒ—{}^{A}\vec{v}๊ฐ€ aโƒ—\vec{a}๋ฅผ ๊ธฐ์ค€์œผ๋กœ โˆ’ฮธ-\theta๋งŒํผ ํšŒ์ „ํ•˜์—ฌ Bvโƒ—{}^{B}\vec{v}๊ฐ€ ๋˜์—ˆ๋‹ค๊ณ  ์ƒ๊ฐํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

๋ฐ˜๋Œ€๋กœ Bvโƒ—{}^{B}\vec{v}๊ฐ€ aโƒ—\vec{a}๋ฅผ ๊ธฐ์ค€์œผ๋กœ ฮธ\theta๋งŒํผ ํšŒ์ „ํ•˜๋ฉด Avโƒ—{}^{A}\vec{v}๋ผ๊ณ  ์ƒ๊ฐํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

Vector ํšŒ์ „์‹์„ ์ด์šฉํ•˜์—ฌ Avโƒ—{}^{A}\vec{v}์™€ Bvโƒ—{}^{B}\vec{v}์˜ ๊ด€๊ณ„๋ฅผ ์‹์œผ๋กœ ๋‚˜ํƒ€๋‚ด๋ฉด ์•„๋ž˜์™€ ๊ฐ™์Šต๋‹ˆ๋‹ค. ์„

Av=q(Bv)qห‰=qBA(Bv)qห‰BA=[1โˆ’2(qy2+qz2)2(qxqyโˆ’qzqw)2(qxqz+qyqw)02(qxqy+qzqw)1โˆ’2(qx2+qz2)2(qyqzโˆ’qxqw)02(qxqzโˆ’qyqw)2(qyqz+qxqw)1โˆ’2(qx2+qy2)00001][BvxBvyBvz0]\begin{aligned} {}^{A}v & = q({}^{B}v)\bar{q} = q_{B}^{A}({}^{B}v)\bar{q}_{B}^{A} \\ & =\begin{bmatrix} 1-2({q_y}^2+{q_z}^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) & 0 \\ 2(q_x q_y + q_z q_w) & 1-2({q_x}^2+{q_z}^2) & 2(q_y q_z - q_x q_w) & 0 \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1-2({q_x}^2+{q_y}^2) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} {}^{B}v_x \\ {}^{B}v_y \\ {}^{B}v_z \\ 0 \end{bmatrix} \end{aligned}

์—ฐ์†์ ์ธ ํšŒ์ „์€ ์•„๋ž˜์™€ ๊ฐ™์ด ๊ตฌํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

kโˆ’1v=qkkโˆ’1kvqห‰kkโˆ’1kโˆ’2v=qkโˆ’1kโˆ’2kโˆ’1vqห‰kโˆ’1kโˆ’2=qkโˆ’1kโˆ’2qkkโˆ’1kvqห‰kkโˆ’1qห‰kโˆ’1kโˆ’2kโˆ’3v=qkโˆ’2kโˆ’3kโˆ’2vqห‰kโˆ’2kโˆ’3=qkโˆ’2kโˆ’3qkโˆ’1kโˆ’2qkkโˆ’1kvqห‰kkโˆ’1qห‰kโˆ’1kโˆ’2qห‰kโˆ’2kโˆ’3โ‹ฎโ‹ฎโ‹ฎ\begin{aligned} {}^{k-1}v & = & q_{k}^{k-1} {}^{k}v \bar{q}_{k}^{k-1} & & \\ {}^{k-2}v & = & q_{k-1}^{k-2} {}^{k-1}v \bar{q}_{k-1}^{k-2} & = & q_{k-1}^{k-2} q_{k}^{k-1} {}^{k}v \bar{q}_{k}^{k-1} \bar{q}_{k-1}^{k-2} \\ {}^{k-3}v & = & q_{k-2}^{k-3} {}^{k-2}v \bar{q}_{k-2}^{k-3} & = & q_{k-2}^{k-3} q_{k-1}^{k-2} q_{k}^{k-1} {}^{k}v \bar{q}_{k}^{k-1} \bar{q}_{k-1}^{k-2} \bar{q}_{k-2}^{k-3} \\ \vdots & & \vdots & & \vdots \end{aligned}
0v=q10q21โ‹ฏqkkโˆ’1kvqห‰kkโˆ’1โ‹ฏqห‰21qห‰10{}^{0}v = q_{1}^{0} q_{2}^{1} \cdots q_{k}^{k-1} {}^{k}v \bar{q}_{k}^{k-1} \cdots \bar{q}_{2}^{1} \bar{q}_{1}^{0}
qk0=q10q21โ‹ฏqkkโˆ’1q_{k}^{0} = q_{1}^{0} q_{2}^{1} \cdots q_{k}^{k-1}

Small angle approximation

Taylor Series์— ์˜ํ•ด sinํ•จ์ˆ˜์™€ cosํ•จ์ˆ˜๋Š” ์•„๋ž˜์™€ ๊ฐ™์€ ๋‹คํ•ญ์‹์œผ๋กœ ๋‚˜ํƒ€๋‚ผ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

sinโกฮธ2=ฮธ2โˆ’13!(ฮธ2)3+15!(ฮธ2)5โˆ’โ‹ฏcosโกฮธ2=1โˆ’12!(ฮธ2)2+14!(ฮธ2)4โˆ’โ‹ฏ\begin{aligned} \sin{\cfrac{\theta}{2}} & = \cfrac{\theta}{2} - \cfrac{1}{3!}\left(\cfrac{\theta}{2}\right)^3 + \cfrac{1}{5!}\left(\cfrac{\theta}{2}\right)^5 - \cdots \\ \cos{\cfrac{\theta}{2}} & = 1 - \cfrac{1}{2!}\left(\cfrac{\theta}{2}\right)^2 + \cfrac{1}{4!}\left(\cfrac{\theta}{2}\right)^4 - \cdots \end{aligned}

ฮธโ†’0\theta\to0 ๋ผ๊ณ  ๊ฐ€์ •ํ•˜๋ฉด sinํ•จ์ˆ˜์™€ cosํ•จ์ˆ˜๋Š” ์•„๋ž˜์™€ ๊ฐ™์ด ๋‚˜ํƒ€๋‚ผ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

sinโกฮธ2โ‰ˆฮธ2cosโกฮธ2โ‰ˆ1\begin{aligned} \sin{\cfrac{\theta}{2}} & \approx \cfrac{\theta}{2} \\ \cos{\cfrac{\theta}{2}} & \approx 1 \end{aligned}

๋”ฐ๋ผ์„œ ํšŒ์ „๊ฐ์ด ์ถฉ๋ถ„ํžˆ ์ž‘์„ ๋•Œ, ํšŒ์ „ ์ฟผํ„ฐ๋‹ˆ์–ธ์€ ์•„๋ž˜์™€ ๊ฐ™์ด ๋‚˜ํƒ€๋‚ผ ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

qโ€พ(aโƒ—,ฮธ)=[aโƒ—sinโก(ฮธ2)cosโก(ฮธ2)]Tโ‰ˆ[ฮธ2aโƒ—1]T(ฮธโ†’0)\begin{aligned} \underline{q}\left(\vec{a},\theta\right) & = \begin{bmatrix} \vec{a}\sin{\left(\cfrac{\theta}{2}\right)} & \cos{\left(\cfrac{\theta}{2}\right)} \end{bmatrix}^T & \\ & \approx \begin{bmatrix} \cfrac{\theta}{2}\vec{a} & 1 \end{bmatrix}^T & (\theta\to0) \end{aligned}
Last updated on