MPU-6050を使った慣性航法 (2)
生のデータを見る限り、角速度センサの処理のほうが簡単そうです。
そこで、角速度センサのバイアス補正と、角度の計算からはじめることにします。
座標系
ここで計算に使う座標系を定義しておきます。
位置の時間微分が速度、速度の時間微分が加速度なので、加速度センサの値を２回積分すれば位置が求まります。同様に、角度の時間微分が角速度なので、角速度を積分すれば角度が求まります。
原理は単純ですが、ここで考えなければいけないのは、センサが機体に固定されているので、機体が回転すればセンサの軸の向きも回転するということと、我々が知りたいのは宇宙空間（絶対静止座標系）の座標ではなく、地表の位置（緯度、経度）と高度であり、地球は丸く、太陽の周りを自転しながら公転しているということです。
自転と公転は角速度に影響を与えますし、重力は、万有引力と遠心力の合成であり、緯度によって異なります。
真面目な慣性航法であれば、これらをすべて考慮するのでしょう。しかし、ここでの目的は卓上程度の場所で５分程度動くロボットなので、位置計算において地球が丸い影響は考慮しないことにします。
角速度についても、自転は15°/h、公転は0.041°/hなので合計15.041°/h＝0.004178°/sの影響があると考えられます。しかし、MPU-6050の角速度センサの精度は1/131＝0.0076°/s程度なので、精度的にも考慮する必要はありません。
というわけで、ここでは、(1)機体を中心に、上方向をZ軸、前方向をX軸、左横方向をY軸とする「機体座標系」と、(2)重力の反対向きをZ軸、Z軸に垂直なX軸Y軸とする「基準座標系」の２つの座標系を考え、基準座標系での位置と角度を求めることにします。
「機体座標系」は、センサの座標系そのものです。
「基準座標系」の原点はセンサの初期位置、ロール角、ピッチ角の初期値は加速度センサから得た重力の値をもとに計算し、ヨー角の初期値は0とします。
角速度センサのバイアス補正
角速度は、次の式で補正することにします。
\(\begin{array}{l}{{g'}_x} = {A_x}({g_x} - {B_x})\\{{g'}_y} = {A_y}({g_y} - {B_y})\\{{g'}_z} = {A_z}({g_z} - {B_z})\end{array}\)
ただし、正確な角度を測定する環境がないため、今回は\({A_x},{A_y},{A_z}\)を１に固定して、\({B_x},{B_y},{B_z}\)のみ求めました。
角速度センサのバイアス（ゼロのズレ）は、求めるのが簡単そうなので、とりあず、Arduino Nanoの起動時に計算させることにします。
具体的には、起動時にはMPU-6050を静止した状態で維持しておきます。
次に、MPU-6050から取得したデータのうち、50回目から150回目までのデータを平均してバイアスとして利用することにします。
Arduino Nanoの起動直後はセンサが動いている可能性があるため、50回目までのデータは捨てます。
バイアス計算後は、取得した角速度データからバイアスを引いた値を実際の角速度として利用します。
この50回とか150回という値に根拠はありませんが、そこそこうまくいっているようです。
回転角の計算
角速度センサから得た角速度を積分して、回転角を求めます。
今回は、単純な台形公式を使って積分しました。
現在の角速度を\({\theta '_t}\)、前回の角速度を\({\theta '_{t - 1}}\)、前回から今回までの経過時間を\(\Delta t\)とすると、前回から今回までの回転角\({\theta _t}\)は、
\({\theta _t} = \frac{{{{\theta '}_t} + {{\theta '}_{t - 1}}}}{2}\Delta t\)
となります。
角度の計算
角度の情報は、クォータニオンの形式で保持することにします。
このクォータニオンは、「機体座標系」のベクトルを「基準座標系」に変換するためのものです。
■クォータニオンの初期値
クォータニオンの初期値は、加速度センサから得た重力の情報をもとに計算します。
まず、X軸周りの回転をロール角（φ）、Y軸周りの回転をピッチ角（θ）、Z軸周りの回転をヨー角（ψ）とします。
この場合、ロール軸、ピッチ軸、ヨー軸の順に回転させる回転行列は、
\(\begin{array}{c}{R_y}(\psi ){R_p}(\theta ){R_r}(\phi ) = \left( {\begin{array}{*{20}{c}}{\cos \psi }&{ - \sin \psi }&0\\{\sin \psi }&{\cos \psi }&0\\0&0&1\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos \theta }&0&{\sin \theta }\\0&1&0\\{ - \sin \theta }&0&{\cos \theta }\end{array}} \right)\left( {\begin{array}{*{20}{c}}1&0&0\\0&{\cos \phi }&{ - \sin \phi }\\0&{\sin \phi }&{\cos \phi }\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos \theta \cos \psi }&{ - \sin \psi }&{\sin \theta \cos \psi }\\{\cos \theta \sin \psi }&{\cos \psi }&{\sin \theta \sin \psi }\\{ - \sin \theta }&0&{\cos \theta }\end{array}} \right)\left( {\begin{array}{*{20}{c}}1&0&0\\0&{\cos \phi }&{ - \sin \phi }\\0&{\sin \phi }&{\cos \phi }\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos \theta \cos \psi }&{ - \cos \phi \sin \psi + \sin \phi \sin \theta \cos \psi }&{\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi }\\{\cos \theta \sin \psi }&{\cos \phi \cos \psi + \sin \phi \sin \theta \sin \psi }&{ - \sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi }\\{ - \sin \theta }&{\sin \phi \cos \theta }&{\cos \phi \cos \theta }\end{array}} \right)\end{array}\)
となります。
この変換によって、基準座標系の重力ベクトル(0,0,1)が、機体座標系の重力ベクトル\(({g_x},{g_y},{g_z})\)に変換されます。
\(\begin{array}{c}\left( {\begin{array}{*{20}{c}}{{g_x}}\\{{g_y}}\\{{g_z}}\end{array}} \right) = {R_y}(\psi ){R_p}(\theta ){R_r}(\phi )\left( {\begin{array}{*{20}{c}}0\\0\\1\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi }\\{ - \sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi }\\{\cos \phi \cos \theta }\end{array}} \right)\end{array}\)
ここでヨー角（ψ）を0とすると、
\(\left( {\begin{array}{*{20}{c}}{{g_x}}\\{{g_y}}\\{{g_z}}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{\cos \phi \sin \theta }\\{ - \sin \phi }\\{\cos \phi \cos \theta }\end{array}} \right)\)
となり、機体のロール角（φ）とピッチ角（θ）の初期値は、
\(\begin{array}{l}\theta = {\tan ^{ - 1}}\left( {\frac{{{g_x}}}{{{g_z}}}} \right)\\\phi = {\tan ^{ - 1}}\left( {\frac{{ - {g_y}}}{{{g_x}\sin \theta + {g_z}\cos \theta }}} \right)\end{array}\)
と求まります。
ただし、このロール角（φ）とピッチ角（θ）は、基準座標系から機体座標系への変換なので、クォータニオンの初期値を求める際は、符号を反転して使います。
ロール軸、ピッチ軸、ヨー軸の順に回転するクォータニオンは、次のように求まります。
\(\begin{array}{c}{q_\psi }{q_\theta }{q_\phi } = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)}\\0\\0\\{\sin (\psi /2)}\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\theta /2)}\\0\\{\sin (\theta /2)}\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\phi /2)}\\{\sin (\phi /2)}\\0\\0\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)\cos (\theta /2)}\\{ - \sin (\psi /2)\sin (\theta /2)}\\{\cos (\psi /2)\sin (\theta /2)}\\{\sin (\psi /2)\cos (\theta /2)}\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\phi /2)}\\{\sin (\phi /2)}\\0\\0\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)\cos (\theta /2)\cos (\phi /2) + \sin (\psi /2)\sin (\theta /2)\sin (\phi /2)}\\{\cos (\psi /2)\cos (\theta /2)\sin (\phi /2) - \sin (\psi /2)\sin (\theta /2)\cos (\phi /2)}\\{\cos (\psi /2)\sin (\theta /2)\cos (\phi /2) + \sin (\psi /2)\cos (\theta /2)\sin (\phi /2)}\\{ - \cos (\psi /2)\sin (\theta /2)\sin (\phi /2) + \sin (\psi /2)\cos (\theta /2)\cos (\phi /2)}\end{array}} \right)\end{array}\)
■回転軸の計算
次に、このクォータニオンを使って、角速度センサのXYZ軸、つまり機体座標系のXYZ軸を基準座標系に変換します。
機体座標系のX軸（ロール軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\1\\0\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - x}\\w\\z\\{ - y}\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - xw + xw + yz - yz}\\{{x^2} + {w^2} - {z^2} - {y^2}}\\{xy + wz + zw + xy}\\{xz - yw + xz - yw}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{{w^2} + {x^2} - {y^2} - {z^2}}\\{2(xy + zw)}\\{2(xz - yw)}\end{array}} \right)\)
機体座標系のY軸（ピッチ軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\0\\1\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - y}\\{ - z}\\w\\x\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - yw - xz + yw + xz}\\{xy - zw - zw + xy}\\{{y^2} - {z^2} + {w^2} - {x^2}}\\{yz + yz + xw + xw}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{2(xy - zw)}\\{{w^2} - {x^2} + {y^2} - {z^2}}\\{2(yz + xw)}\end{array}} \right)\)
機体座標系のZ軸（ヨー軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\0\\0\\1\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - z}\\y\\{ - x}\\w\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - zw + xy - xy + zw}\\{xz + yw + xz + yw}\\{yz + yz - xw - xw}\\{{z^2} - {y^2} - {x^2} + {w^2}}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{2(xz + yw)}\\{2(yz - xw)}\\{{w^2} - {x^2} - {y^2} + {z^2}}\end{array}} \right)\)
に変換されます。
■クォータニオンの更新
任意軸まわりの回転を示すクォータニオンは、
\({q_u}(\theta ) = \left( {\begin{array}{*{20}{c}}{\cos \frac{\theta }{2}}\\{{u_x}\sin \frac{\theta }{2}}\\{{u_y}\sin \frac{\theta }{2}}\\{{u_z}\sin \frac{\theta }{2}}\end{array}} \right)\)
なので、ロール軸、ピッチ軸、ヨー軸の、基準座標系での回転を示すクォータニオンを計算します。
新しいクォータニオン\(q\)は、現在のクォータニオンを\(q'\)、X軸（ロール軸）周りの回転を\({q_x}\)、Y軸（ピッチ軸）周りの回転を\({q_y}\)、Z軸（ヨー軸）周りの回転を\({q_z}\)とすると、
\(q = {q_z}{q_y}{q_x}q'\)
で求まります。
実行結果
以上の計算を実装して、クォータニオンの値をシリアルに出力するプログラムを実行しました。
（実際には、ロール角、ピッチ角、ヨー角の初期値は0で固定しており、重力ベクトルからの計算は省略しています。）
その出力をExcelに読み込み、クォータニオンからロール角（φ）、ピッチ角（θ）、ヨー角（ψ）を計算してグラフを描いてみたのが以下の図です。
クォータニオンからロール角、ピッチ角、ヨー角を計算するには、以下の式を使っています。
\(\begin{array}{c}\phi = \arctan \left( {\frac{{2({q_w}{q_x} + {q_y}{q_z})}}{{1 - 2({q_x}^2 + {q_y}^2)}}} \right)\\\theta = \arcsin \left( {2({q_w}{q_y} - {q_x}{q_z})} \right)\\\psi = \arctan \left( {\frac{{2({q_w}{q_z} + {q_x}{q_y})}}{{1 - 2({q_y}^2 + {q_z}^2)}}} \right)\end{array}\)
Z軸、Y軸、X軸の順に回転させたときの、ロール角、ピッチ角、ヨー角
Y軸（ピッチ軸）を中心に回したときのヨー角の動きが気になりますが、それ以外はおおむね、想定通りの動きが計算されています。なにより、\({A_x},{A_y},{A_z}\)を１に固定していても、最終的にロール角、ピッチ角、ヨー角ともに、初期状態とほぼ同じ姿勢に戻っているのが確認できました。
そこで、角速度センサのバイアス補正と、角度の計算からはじめることにします。
座標系
ここで計算に使う座標系を定義しておきます。
位置の時間微分が速度、速度の時間微分が加速度なので、加速度センサの値を２回積分すれば位置が求まります。同様に、角度の時間微分が角速度なので、角速度を積分すれば角度が求まります。
原理は単純ですが、ここで考えなければいけないのは、センサが機体に固定されているので、機体が回転すればセンサの軸の向きも回転するということと、我々が知りたいのは宇宙空間（絶対静止座標系）の座標ではなく、地表の位置（緯度、経度）と高度であり、地球は丸く、太陽の周りを自転しながら公転しているということです。
自転と公転は角速度に影響を与えますし、重力は、万有引力と遠心力の合成であり、緯度によって異なります。
真面目な慣性航法であれば、これらをすべて考慮するのでしょう。しかし、ここでの目的は卓上程度の場所で５分程度動くロボットなので、位置計算において地球が丸い影響は考慮しないことにします。
角速度についても、自転は15°/h、公転は0.041°/hなので合計15.041°/h＝0.004178°/sの影響があると考えられます。しかし、MPU-6050の角速度センサの精度は1/131＝0.0076°/s程度なので、精度的にも考慮する必要はありません。
というわけで、ここでは、(1)機体を中心に、上方向をZ軸、前方向をX軸、左横方向をY軸とする「機体座標系」と、(2)重力の反対向きをZ軸、Z軸に垂直なX軸Y軸とする「基準座標系」の２つの座標系を考え、基準座標系での位置と角度を求めることにします。
「機体座標系」は、センサの座標系そのものです。
「基準座標系」の原点はセンサの初期位置、ロール角、ピッチ角の初期値は加速度センサから得た重力の値をもとに計算し、ヨー角の初期値は0とします。
角速度センサのバイアス補正
角速度は、次の式で補正することにします。
\(\begin{array}{l}{{g'}_x} = {A_x}({g_x} - {B_x})\\{{g'}_y} = {A_y}({g_y} - {B_y})\\{{g'}_z} = {A_z}({g_z} - {B_z})\end{array}\)
ただし、正確な角度を測定する環境がないため、今回は\({A_x},{A_y},{A_z}\)を１に固定して、\({B_x},{B_y},{B_z}\)のみ求めました。
角速度センサのバイアス（ゼロのズレ）は、求めるのが簡単そうなので、とりあず、Arduino Nanoの起動時に計算させることにします。
具体的には、起動時にはMPU-6050を静止した状態で維持しておきます。
次に、MPU-6050から取得したデータのうち、50回目から150回目までのデータを平均してバイアスとして利用することにします。
Arduino Nanoの起動直後はセンサが動いている可能性があるため、50回目までのデータは捨てます。
バイアス計算後は、取得した角速度データからバイアスを引いた値を実際の角速度として利用します。
この50回とか150回という値に根拠はありませんが、そこそこうまくいっているようです。
回転角の計算
角速度センサから得た角速度を積分して、回転角を求めます。
今回は、単純な台形公式を使って積分しました。
現在の角速度を\({\theta '_t}\)、前回の角速度を\({\theta '_{t - 1}}\)、前回から今回までの経過時間を\(\Delta t\)とすると、前回から今回までの回転角\({\theta _t}\)は、
\({\theta _t} = \frac{{{{\theta '}_t} + {{\theta '}_{t - 1}}}}{2}\Delta t\)
となります。
角度の計算
角度の情報は、クォータニオンの形式で保持することにします。
このクォータニオンは、「機体座標系」のベクトルを「基準座標系」に変換するためのものです。
■クォータニオンの初期値
クォータニオンの初期値は、加速度センサから得た重力の情報をもとに計算します。
まず、X軸周りの回転をロール角（φ）、Y軸周りの回転をピッチ角（θ）、Z軸周りの回転をヨー角（ψ）とします。
この場合、ロール軸、ピッチ軸、ヨー軸の順に回転させる回転行列は、
\(\begin{array}{c}{R_y}(\psi ){R_p}(\theta ){R_r}(\phi ) = \left( {\begin{array}{*{20}{c}}{\cos \psi }&{ - \sin \psi }&0\\{\sin \psi }&{\cos \psi }&0\\0&0&1\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos \theta }&0&{\sin \theta }\\0&1&0\\{ - \sin \theta }&0&{\cos \theta }\end{array}} \right)\left( {\begin{array}{*{20}{c}}1&0&0\\0&{\cos \phi }&{ - \sin \phi }\\0&{\sin \phi }&{\cos \phi }\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos \theta \cos \psi }&{ - \sin \psi }&{\sin \theta \cos \psi }\\{\cos \theta \sin \psi }&{\cos \psi }&{\sin \theta \sin \psi }\\{ - \sin \theta }&0&{\cos \theta }\end{array}} \right)\left( {\begin{array}{*{20}{c}}1&0&0\\0&{\cos \phi }&{ - \sin \phi }\\0&{\sin \phi }&{\cos \phi }\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos \theta \cos \psi }&{ - \cos \phi \sin \psi + \sin \phi \sin \theta \cos \psi }&{\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi }\\{\cos \theta \sin \psi }&{\cos \phi \cos \psi + \sin \phi \sin \theta \sin \psi }&{ - \sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi }\\{ - \sin \theta }&{\sin \phi \cos \theta }&{\cos \phi \cos \theta }\end{array}} \right)\end{array}\)
となります。
この変換によって、基準座標系の重力ベクトル(0,0,1)が、機体座標系の重力ベクトル\(({g_x},{g_y},{g_z})\)に変換されます。
\(\begin{array}{c}\left( {\begin{array}{*{20}{c}}{{g_x}}\\{{g_y}}\\{{g_z}}\end{array}} \right) = {R_y}(\psi ){R_p}(\theta ){R_r}(\phi )\left( {\begin{array}{*{20}{c}}0\\0\\1\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi }\\{ - \sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi }\\{\cos \phi \cos \theta }\end{array}} \right)\end{array}\)
ここでヨー角（ψ）を0とすると、
\(\left( {\begin{array}{*{20}{c}}{{g_x}}\\{{g_y}}\\{{g_z}}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{\cos \phi \sin \theta }\\{ - \sin \phi }\\{\cos \phi \cos \theta }\end{array}} \right)\)
となり、機体のロール角（φ）とピッチ角（θ）の初期値は、
\(\begin{array}{l}\theta = {\tan ^{ - 1}}\left( {\frac{{{g_x}}}{{{g_z}}}} \right)\\\phi = {\tan ^{ - 1}}\left( {\frac{{ - {g_y}}}{{{g_x}\sin \theta + {g_z}\cos \theta }}} \right)\end{array}\)
と求まります。
ただし、このロール角（φ）とピッチ角（θ）は、基準座標系から機体座標系への変換なので、クォータニオンの初期値を求める際は、符号を反転して使います。
ロール軸、ピッチ軸、ヨー軸の順に回転するクォータニオンは、次のように求まります。
\(\begin{array}{c}{q_\psi }{q_\theta }{q_\phi } = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)}\\0\\0\\{\sin (\psi /2)}\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\theta /2)}\\0\\{\sin (\theta /2)}\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\phi /2)}\\{\sin (\phi /2)}\\0\\0\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)\cos (\theta /2)}\\{ - \sin (\psi /2)\sin (\theta /2)}\\{\cos (\psi /2)\sin (\theta /2)}\\{\sin (\psi /2)\cos (\theta /2)}\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\cos (\phi /2)}\\{\sin (\phi /2)}\\0\\0\end{array}} \right)\\ = \left( {\begin{array}{*{20}{c}}{\cos (\psi /2)\cos (\theta /2)\cos (\phi /2) + \sin (\psi /2)\sin (\theta /2)\sin (\phi /2)}\\{\cos (\psi /2)\cos (\theta /2)\sin (\phi /2) - \sin (\psi /2)\sin (\theta /2)\cos (\phi /2)}\\{\cos (\psi /2)\sin (\theta /2)\cos (\phi /2) + \sin (\psi /2)\cos (\theta /2)\sin (\phi /2)}\\{ - \cos (\psi /2)\sin (\theta /2)\sin (\phi /2) + \sin (\psi /2)\cos (\theta /2)\cos (\phi /2)}\end{array}} \right)\end{array}\)
■回転軸の計算
次に、このクォータニオンを使って、角速度センサのXYZ軸、つまり機体座標系のXYZ軸を基準座標系に変換します。
機体座標系のX軸（ロール軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\1\\0\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - x}\\w\\z\\{ - y}\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - xw + xw + yz - yz}\\{{x^2} + {w^2} - {z^2} - {y^2}}\\{xy + wz + zw + xy}\\{xz - yw + xz - yw}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{{w^2} + {x^2} - {y^2} - {z^2}}\\{2(xy + zw)}\\{2(xz - yw)}\end{array}} \right)\)
機体座標系のY軸（ピッチ軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\0\\1\\0\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - y}\\{ - z}\\w\\x\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - yw - xz + yw + xz}\\{xy - zw - zw + xy}\\{{y^2} - {z^2} + {w^2} - {x^2}}\\{yz + yz + xw + xw}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{2(xy - zw)}\\{{w^2} - {x^2} + {y^2} - {z^2}}\\{2(yz + xw)}\end{array}} \right)\)
機体座標系のZ軸（ヨー軸）は、
\(\left( {\begin{array}{*{20}{c}}w\\x\\y\\z\end{array}} \right)\left( {\begin{array}{*{20}{c}}0\\0\\0\\1\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - z}\\y\\{ - x}\\w\end{array}} \right)\left( {\begin{array}{*{20}{c}}w\\{ - x}\\{ - y}\\{ - z}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{ - zw + xy - xy + zw}\\{xz + yw + xz + yw}\\{yz + yz - xw - xw}\\{{z^2} - {y^2} - {x^2} + {w^2}}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}0\\{2(xz + yw)}\\{2(yz - xw)}\\{{w^2} - {x^2} - {y^2} + {z^2}}\end{array}} \right)\)
に変換されます。
■クォータニオンの更新
任意軸まわりの回転を示すクォータニオンは、
\({q_u}(\theta ) = \left( {\begin{array}{*{20}{c}}{\cos \frac{\theta }{2}}\\{{u_x}\sin \frac{\theta }{2}}\\{{u_y}\sin \frac{\theta }{2}}\\{{u_z}\sin \frac{\theta }{2}}\end{array}} \right)\)
なので、ロール軸、ピッチ軸、ヨー軸の、基準座標系での回転を示すクォータニオンを計算します。
新しいクォータニオン\(q\)は、現在のクォータニオンを\(q'\)、X軸（ロール軸）周りの回転を\({q_x}\)、Y軸（ピッチ軸）周りの回転を\({q_y}\)、Z軸（ヨー軸）周りの回転を\({q_z}\)とすると、
\(q = {q_z}{q_y}{q_x}q'\)
で求まります。
実行結果
以上の計算を実装して、クォータニオンの値をシリアルに出力するプログラムを実行しました。
（実際には、ロール角、ピッチ角、ヨー角の初期値は0で固定しており、重力ベクトルからの計算は省略しています。）
その出力をExcelに読み込み、クォータニオンからロール角（φ）、ピッチ角（θ）、ヨー角（ψ）を計算してグラフを描いてみたのが以下の図です。
クォータニオンからロール角、ピッチ角、ヨー角を計算するには、以下の式を使っています。
\(\begin{array}{c}\phi = \arctan \left( {\frac{{2({q_w}{q_x} + {q_y}{q_z})}}{{1 - 2({q_x}^2 + {q_y}^2)}}} \right)\\\theta = \arcsin \left( {2({q_w}{q_y} - {q_x}{q_z})} \right)\\\psi = \arctan \left( {\frac{{2({q_w}{q_z} + {q_x}{q_y})}}{{1 - 2({q_y}^2 + {q_z}^2)}}} \right)\end{array}\)
Y軸（ピッチ軸）を中心に回したときのヨー角の動きが気になりますが、それ以外はおおむね、想定通りの動きが計算されています。なにより、\({A_x},{A_y},{A_z}\)を１に固定していても、最終的にロール角、ピッチ角、ヨー角ともに、初期状態とほぼ同じ姿勢に戻っているのが確認できました。
コメント