# 座標軸旋轉

## 旋轉方式

> 1. $v= a \times b$

其中ab為單位向量，則可推導出

$sin=|v|$ ， since $|a{\times}b|=|a||b|sinθ$

$cos=a \cdot b$ ， since ${|a||b|cosθ}={a \cdot b}$

> 2. 求旋轉矩陣$R=I+[v]_{\times}+ \frac{1−c}{s^2}[v]^2$

其中 I 為單位矩陣，$[v]_{\times}$為$[v]$的`反對稱矩陣`，定義為

$[v]_{\times} \stackrel{\rm def}{=}
\begin{bmatrix}
\,\,0 & \!-v_3 & \,\,\,v_2\\
\,\,\,v_3 & 0 & \!-v_1\\
\!-v_2 & \,\,v_1 &\,\,0
\end{bmatrix}$

## 套用感測器數值

==**第一步 將重力向量轉到[0,0,1]求旋轉矩陣**==

這裡的$g_x$、$g_y$、$g_z$為**傳感器數值除以$G(9.80665)$**

$a=\left[
\begin{array}{1}
0 \\ 0 \\ 1 \\
\end{array}
\right]$ ， $b=\left[
\begin{array}{1}
g_x \\ g_y \\ g_z \\
\end{array}
\right]$ ， $v= a \times b =\left[
\begin{array}{1}
-g_y \\ g_x \\ 0 \\
\end{array}
\right]$

則 $cos = g_z$，$sin=\sqrt{g_y^2+g_x^2}$

$[v]_{\times}=
\begin{bmatrix}
\ 0 & 0 & \ g_x\\
\ 0 & 0 & \ g_y\\
\ -g_x & \ -g_y &\ 0
\end{bmatrix}$

$[v]^2_{\times}=
\begin{bmatrix}
\ -g^2_x & -g_xg_y & \ 0\\
\ -g_xg_y & -g^2_y & \ 0\\
\ 0 & \ 0 &\ -g^2_x-g^2_y
\end{bmatrix}$

套用公式即可得到旋轉矩陣$R$

==**第二步 將線性加速度向量透過旋轉矩陣轉為結果**==

$Result {\times} R = A$ ，則 $Result = A {\times} R^{-1}$

invert用jama->Matrix類中的inverse()

:::info
Source
1 取得旋轉矩陣
https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
:::