回転行列の補正

# 三次元の向きの逐次更新

三次元での向きを表現する方法として、オイラー角を使って

```math

\begin{eqnarray}

座標系 &：& 右手系 \\
X 軸 &：& 左方向 \\
Y 軸 &：& 上方向 \\
Z 軸 &：& 前方向 \\
\\
yow &=& \theta_{yow} \ (ZX 平面回転) \\
pitch &=& \theta_{pitch} \ (YZ 平面回転) \\
roll &=& \theta_{roll} \ (XY 平面回転) \\
\\
T_{yow} &=& \left(
\begin{array}{ccc}
cos\ \theta_{yow} & 0 & sin\ \theta_{yow} \\
0 & 1 & 0 \\
-sin\ \theta_{yow} & 0 & cos\ \theta_{yow} \\
\end{array}
\right) \\

T_{pitch} &=& \left(
\begin{array}{ccc}
1 & 0 & 0 \\
0 & cos\ \theta_{pitch} & -sin\ \theta_{pitch} \\
0 & sin\ \theta_{pitch} & cos\ \theta_{pitch} \\
\end{array}
\right) \\

T_{roll} &=& \left(
\begin{array}{ccc}
cos\ \theta_{roll} & -sin\ \theta_{roll} & 0 \\
sin\ \theta_{roll} & cos\ \theta_{roll} & 0 \\
0 & 0 & 1 \\
\end{array}
\right) \\

T = T_{yow} T_{pitch} T_{roll} &=& \left(
\begin{array}{ccc}
m_{11} & m_{12} & m_{13} \\
m_{21} & m_{22} & m_{23} \\
m_{31} & m_{32} & m_{33} \\
\end{array}
\right) \\
\\
m_{11} &=& + cos\ \theta_{yow}\ cos\ \theta_{roll} + sin\ \theta_{yow}\ sin\ \theta_{pitch}\ sin\ \theta_{roll} \\
m_{12} &=& - cos\ \theta_{yow}\ sin\ \theta_{roll} + sin\ \theta_{yow}\ sin\ \theta_{pitch}\ cos\ \theta_{roll} \\
m_{13} &=& + sin\ \theta_{yow}\ cos\ \theta_{pitch} \\
m_{21} &=& + cos\ \theta_{pitch}\ sin\ \theta_{roll} \\
m_{22} &=& + cos\ \theta_{pitch}\ cos\ \theta_{roll} \\
m_{23} &=& - sin\ \theta_{pitch} \\
m_{31} &=& - sin\ \theta_{yow}\ cos\ \theta_{roll} + cos\ \theta_{yow}\ sin\ \theta_{pitch}\ sin\ \theta_{roll} \\
m_{32} &=& + sin\ \theta_{yow}\ sin\ \theta_{roll} + cos\ \theta_{yow}\ sin\ \theta_{pitch}\ cos\ \theta_{roll} \\
m_{33} &=& + cos\ \theta_{yow}\ cos\ \theta_{pitch} \\

\end{eqnarray}

```

として、逐次更新するには、現在の回転行列 $T_{curr}$ に差分となる回転行列 $T$ をオイラー角でつくり、次の回転行列 $T_{next}$ を掛けて

```math

T_{next} = T_{curr} T

```

で求めることになります。繰り返し適用していくと $T_{next}$ は誤差が蓄積する(空間が歪む)ので補正が必要です。

# オイラー角に戻す方法

$T_{next}$ からオイラー角を求めるには、単位行列から $T_{next}$ への差分を $T$ とみなして

```math

\begin{eqnarray}

\theta_{yow} &=& tan^{-1} \frac{m13}{m33} = tan^{-1} \frac{ sin\ \theta_{yow}\ cos\ \theta_{pitch} }{ cos\ \theta_{yow}\ cos\ \theta_{pitch} } \\
\theta_{pitch} &=& sin^{-1} \left(-m23\right) \\
\theta_{roll} &=& tan^{-1} \frac{m21}{m22} = tan^{-1} \frac{ cos\ \theta_{pitch}\ sin\ \theta_{roll} }{ cos\ \theta_{pitch}\ cos\ \theta_{roll} } \\

\end{eqnarray}

```

とできますが、$\left( cos\ \theta_{pitch} = 0 \right)$ のとき、$\theta_{yow}, \theta_{roll}$ が求まりません。しかし、真上か真下を向いているから $\left( \theta_{roll} = 0 \right)$ とできるので

```math

\begin{eqnarray}

\theta_{yow} &=& \frac{ \theta_{pitch} }{ \left| \theta_{pitch} \right| } tan^{-1} \frac{m12}{m32} \\
\theta_{roll} &=& 0 \\

\end{eqnarray}

```

となります。方向情報の更新では $T_{next}$ の累積誤差は解消されます。オイラー角から $T_{curr}$ へ。


機械計算で困るのは $\left( cos\ \theta_{pitch} ≒ 0 \right)$ の場合で、演算精度の問題から $\theta_{yow}, \theta_{roll}$ かなり違う方向を示します。20年くらい前までは、16bit 固定小数点を使っていたので

```math

\begin{eqnarray}

T_{curr} &=& \frac{1}{16384} \left(
\begin{array}{rrr}
16321 &  1427 &      1 \\
    1 &     2 & -16382 \\
-1428 & 16321 &      2 \\
\end{array}
\right)

\end{eqnarray}

```

という値の行列になったりして、オイラー角に戻すと

```math

\begin{eqnarray}

\theta_{yow}   &=& 45° \\
\theta_{pitch} &=& 89° \\
\theta_{roll}  &=& 45° \\

\end{eqnarray}

```

あらぬ方向を示していました。

単精度浮動小数点演算では、ここまで極端ではないものの可能性がゼロではないので、 $\left( cos\ \theta_{pitch} ≒ 0 \right)$ は $\left( cos\ \theta_{pitch} = 0 \right)$ とみなして処理する必要があります。

# 任意軸と回転角を使う

任意軸の回転行列については [こちら](https://qiita.com/ikiuo/items/f13309f8b1023e47d20b "備忘録：任意軸の回転行列") を参照。

$T_{next}$ から任意軸と回転角を求めて、任意軸の回転行列を $T_{curr}$ とする。任意軸と回転角を求めるとき、ゼロ付近の除算に注意が必要です。

余談：三次元での向きを表現する方法として、一般的ではありませんが回転軸の単位ベクトル $\left( x,y,z \right)$ と回転角 $\theta$ から

```math

\begin{eqnarray}

r &=& \left( r_x,r_y,r_z \right) = \left( \theta x, \theta y, \theta z \right) \\
\left| r \right| &=& \theta \\

\end{eqnarray}

```

とすると、$r_x, r_y, r_z$ は、$X,Y,Z$ の各軸での同時回転に見えます。

**クォータニオンを使う**

$T_{next}$ からクォータニオンに変換し、正規化後に回転行列に変換して $T_{curr}$ にします。クォータニオンに変換するとき、ゼロ付近の除算に注意が必要です。

# 行列のまま補正する

回転行列

```math

T = \left(
\begin{array}{ccc}
m_{11} & m_{12} & m_{13} \\
m_{21} & m_{22} & m_{23} \\
m_{31} & m_{32} & m_{33} \\
\end{array}
\right)

```

は、次の性質

```math
\displaylines {

L_1 = \left( m_{11}, m_{12}, m_{13} \right) \\
L_2 = \left( m_{21}, m_{22}, m_{23} \right) \\
L_3 = \left( m_{31}, m_{32}, m_{33} \right) \\
\\
\left| L_1 \right| = \left| L_2 \right| = \left| L_3 \right| = 1 \\
\\
L_1 \times L_2 = L_3 \\
L_2 \times L_3 = L_1 \\
L_3 \times L_1 = L_2 \\
\\
C_1 = \left( m_{11}, m_{21}, m_{31} \right) \\
C_2 = \left( m_{12}, m_{22}, m_{32} \right) \\
C_3 = \left( m_{13}, m_{23}, m_{33} \right) \\
\\
\left| C_1 \right| = \left| C_2 \right| = \left| C_3 \right| = 1 \\
\\
C_1 \times C_2 = C_3 \\
C_2 \times C_3 = C_1 \\
C_3 \times C_1 = C_2

}
```

があります。しかし、演算誤差で

```math
\displaylines {

L_1 \times L_2 \neq L_3 \\
L_2 \times L_3 \neq L_1 \\
L_3 \times L_1 \neq L_2 \\
\\
C_1 \times C_2 \neq C_3 \\
C_2 \times C_3 \neq C_1 \\
C_3 \times C_1 \neq C_2 \\

}
```

となっています。そこで

```math
\begin{eqnarray}
L'_1 &=& L_2 \times L_3 \\
L'_2 &=& L_3 \times L_1 \\
L'_3 &=& L'_1 \times L'_2 \\
\\
T_{curr} &=& \left(
\begin{array}{c}
\frac{L'_1}{ \left| L'_1 \right| } \\
\frac{L'_2}{ \left| L'_2 \right| } \\
\frac{L'_3}{ \left| L'_3 \right| } \\
\end{array}
\right) \\
\end{eqnarray}
```

または

```math
\displaylines {

\begin{eqnarray}

C'_1 &=& C_2 \times C_3 \\
C'_2 &=& C_3 \times C_1 \\
C'_3 &=& C'_1 \times C'_2 \\

\end{eqnarray} \\
\\
T_{curr} = \left(
\begin{array}{ccc}
\left( \frac{C'_1}{ \left| C'_1 \right| } \right)^T &
\left( \frac{C'_2}{ \left| C'_2 \right| } \right)^T &
\left( \frac{C'_3}{ \left| C'_3 \right| } \right)^T
\end{array}
\right)

}
```

として補正します。他にも、$L'_1,L'_2,L'_3$ の部分を

```math

\begin{eqnarray}

L'_1 &=& \frac{ L_1 + \left( L_2 \times L_3 \right) }{2} \\
L'_2 &=& \frac{ L_2 + \left( L_3 \times L_1 \right) }{2} \\
L'_3 &=& \frac{ L_3 + \left( L_1 \times L_2 \right) }{2} \\

\end{eqnarray}

```

とする方法などが考えられます。

これらの方法では、ゼロ付近の除算がありません。