三次元の向きの逐次更新
三次元での向きを表現する方法として、オイラー角を使って
\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}$ を掛けて
T_{next} = T_{curr} T
で求めることになります。繰り返し適用していくと $T_{next}$ は誤差が蓄積する(空間が歪む)ので補正が必要です。
オイラー角に戻す方法
$T_{next}$ からオイラー角を求めるには、単位行列から $T_{next}$ への差分を $T$ とみなして
\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)$ とできるので
\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 固定小数点を使っていたので
\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}
という値の行列になったりして、オイラー角に戻すと
\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)$ とみなして処理する必要があります。
任意軸と回転角を使う
任意軸の回転行列については こちら を参照。
$T_{next}$ から任意軸と回転角を求めて、任意軸の回転行列を $T_{curr}$ とする。任意軸と回転角を求めるとき、ゼロ付近の除算に注意が必要です。
余談：三次元での向きを表現する方法として、一般的ではありませんが回転軸の単位ベクトル $\left( x,y,z \right)$ と回転角 $\theta$ から
\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}$ にします。クォータニオンに変換するとき、ゼロ付近の除算に注意が必要です。
行列のまま補正する
回転行列
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)
は、次の性質
\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
}
があります。しかし、演算誤差で
\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 \\
}
となっています。そこで
\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}
または
\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$ の部分を
\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}
とする方法などが考えられます。
これらの方法では、ゼロ付近の除算がありません。