WiiYourself!でモーションプラスを使い角度を算出してみる（単純積分）でWiiリモコンのモーションプラスを使って角度を算出し、姿勢を推定することはできました。 しかし、「角速度の値が送られてくる間の時間が微小と考える方法」だったので、精度がいまいちでした。
ここでは、それを少し改善するために、台形補間というものを行ってみます。 補間の方法はこれ以外にも沢山ありますが、台形補間が一番簡単でわかりやすく、また今回の使用用途に適しています。
というのも、この方法は単純かつ高速です。 ゲームなどのリアルタイム処理には高速性が要求されるので、下手に難しい補間法などを使うと、無駄な処理を行ってしまうことになります。 また、取ってくる角速度自体がそんなに精度のいいものではありません。 「ある程度補間したい（あまりにも精度が悪いのは困る）」「高速性は失われたくない」という点でいれば適していると思います。
補間法自体はもっと精度を上げるものが色々あるので、興味のある人は調べてみてください。
| ソース構成 |
|---|
以下、WiiYourself!でモーションプラスを使い角度を算出してみる（単純積分）と全く同じコードです。
|
| リソース |
|
| ソースダウンロード |
| Wii_GyroEstimator_Trapezoid.zip |
| プロジェクトの設定 |
| プロジェクトの種類：Win32アプリケーション |
| 備考 |
| Debugモードで実行すると、デバッグエラーが出るかもしれません。 無視しても（たぶん）正常に動きます。 謎のエラーでいまだに取れてないのですが、Rereaseモードで実行すると問題なく動きます。 私のWiiYourself!の使い方が悪いようで、コールバック関数を使うといつもこうなります。 もし解決法を知っている方がいらっしゃったら教えてください。 |
| 実行結果のイメージ |
 |
原理
まず、先に原理的なところから説明しときます。 WiiYourself!でモーションプラスを使い角度を算出してみる（単純積分）にも書いたように、積分は面積を求めています。 「角速度の値が送られてくる間の時間が微小と考える方法」では以下のような面積の求め方でした。
グラフを見ると、一個一個の面積がただの長方形でガタガタになっていて、本来求められるべき面積とは形が大きく異なっていることがわかります。 このガタガタを取り除いて、より本来求められるべき面積に近づける処理が補間になります。
では、実際どうするのか？
ただの長方形だからガタガタするので、長方形でなければいいのです。 そこで出てくるのが“台形”です。
台形の面積の求め方は覚えてますか？
（上底＋下底）× 高さ / 2
です。 これをプログラムに組み込んであげればいいのです。 実際にグラフを見た方が早いです。
現在の角速度だけでは、台形にできないので、一つ前の角速度を記憶しておいて台形にします。 単純に積分したものと見比べると、本来求められるべき面積に大きく近づいていることが明らかにわかります。
グラフで見るとそうでもないですが、実際には、これでも曲線ではないのでズームして見るとカクカクしています。ですが、今回の場合はこの程度で十分です。
プログラム解説
では、プログラミングしていきます。
今回はWiiYourself!でモーションプラスを使い角度を算出してみる（単純積分）と異なる部分のみをピックアップして解説していきます。
① 変数の追加
GyroEstimator.cpp（WinMain関数メインループ内）
float yaw, pitch, roll; // 算出角度 static float angle_yaw = 0, angle_pitch = 0, angle_roll = 0; // 姿勢 static float offset_yaw = -29, offset_pitch = 9, offset_roll = 18; // オフセット（値は初期値） static float temp_yaw = 0, temp_pitch = 0, temp_roll = 0; // テンポラリ static float old_speed_yaw = 0, old_speed_pitch = 0, old_speed_roll = 0; // 1つ前のデータ（台形補間用） TCHAR text[80]; // 転送用文字配列
追加したのは、1つ前のデータだけです。 これを使うことで、台形補間を行います。
記憶させておかなければならないので、staticにします。
② 角度算出
GyroEstimator.cpp（WinMain関数メインループ内）
// 角度算出
if( g_speed_yaw > 10 || g_speed_yaw < -10 ) {
yaw = (g_speed_yaw + old_speed_yaw) * frametime / (1000 * 2);
angle_yaw += yaw;
old_speed_yaw = g_speed_yaw;
}
if( g_speed_pitch > 10 || g_speed_pitch < -10 ) {
pitch = (g_speed_pitch + old_speed_pitch) * frametime / (1000 * 2);
angle_pitch += pitch;
old_speed_pitch = g_speed_pitch;
}
if( g_speed_roll > 10 || g_speed_roll < -10 ) {
roll = (g_speed_roll + old_speed_roll) * frametime / (1000 * 2);
angle_roll += roll;
old_speed_roll = g_speed_roll;
}
台形の計算で角度の算出を行います。
角度＝（現在の角速度＋1つ前の角速度）×1フレームの時間÷2
あとは、一つ前のデータが必要となるので、現在の角速度を1つ前の角速度用変数に入れて記憶させておきます。
まとめ
以上で、台形補間の説明は終了です。 とても簡単です。
実際に動かしてみると、あんまり違いは感じられないかもしれませんが、計算精度的に上がっているのは確かです。 私的な見解では、これ以上計算の精度を上げるよりも、加速度やIR（赤外線）センサとの組み合わせで制御していった方がきれいな感じにはなると思います。