WiiYourself!の機能で角速度までは取れるので、それを時間積分して角度を算出するということをやります。なんだか難しそうに聞こえますが、とりあえず高校の物理と数学（数Ⅱ？）がわかれば理解できると思います。
ソースが6つありますが、見るのはGyroEstimator.cppだけでいいと思います。 他はDirectXの描画のためのものになります。 また、DirectX部分の解説は省略します。
| ソース構成 |
|---|
|
| リソース |
|
| ソースダウンロード |
| Wii_GyroEstimator.zip |
| プロジェクトの設定 |
| プロジェクトの種類：Win32アプリケーション |
| 備考 |
| Debugモードで実行すると、デバッグエラーが出るかもしれません。 無視しても（たぶん）正常に動きます。 謎のエラーでいまだに取れてないのですが、Rereaseモードで実行すると問題なく動きます。 私のWiiYourself!の使い方が悪いようで、コールバック関数を使うといつもこうなります。 もし解決法を知っている方がいらっしゃったら教えてください。 |
| 実行結果のイメージ |
 |
原理
まず、先に原理的なところから説明しておきます。
ジャイロ
一つ目に、モーションプラスの中に入っているジャイロについてです。
ジャイロセンサは角速度を検出する計測器です。 ジャイロセンサのセンサ部分についてはWiiRemoteの仕様を見てください。
これを使った姿勢推定は動的な姿勢推定になります。 どういうことかというと、例えば対比として加速度センサは静的な姿勢推定になります。
加速度センサは重力の方向を見て、角度を出します。 なので、Yaw角度がわかりません。（動かしても重力変化がないから） ですが、例えばめちゃくちゃに動かした後に元に戻しても正確に姿勢が推定できます。
これに対して、ジャイロセンサは“ある時間内での角度”（要するに角速度）を出します。 これを積分するので、誤差が乗った時に、その誤差がそのまま反映されていきます。 なので、例えばめちゃくちゃに動かした後に元に戻しても正確に姿勢が推定できません。 その時点からの姿勢の動きになります。
若干説明しにくいのですが、見た方が早いです。（実際に自分で動かした方がわかると思います。）
積分
二つ目に、積分についてです。
WiiYourself!の機能を使うと、角速度まで取れます。 これを積分するのですが、それについて説明します。
積分とは、簡単に言ってしまえば「掛け算の進化系」です。 グラフで見るとわかりやすいです。 このサイトに詳しく書かれています。
要は、「細かく分割して、掛け算して、全部足す」のです。
で、これが、今回のプログラムの場合はどうなるかというと、
「順次送られてくる（分割されている）角速度を時間で積分（掛け算）して、角度にし、それを足すことで姿勢の変化を求める」
ということになります。
すると最初の姿勢（初期値）からの姿勢の変化が推定することができます。
積分、積分言ってるとわかりにくくなるかと思うのですが、要するに
角度＝角速度×時間
です。これを毎回足していけば姿勢の変化量になります。 図にした方がわかりやすいですね。
今回は「角速度の値が送られてくる間の時間が微小と考える方法」を使ってプログラムを組みます。 このやり方はあんまり頭の良いやり方ではないですが、説明がしやすいのと、わかりやすいです。
どんな方法かというと、上の図でいう“値が送られてくる間の一個一個の時間”を限りなく小さいと考えれば、積分の誤差（面積のはみ出てる部分とか足りない部分）がないと近似できるので、正確な値が求まるという方法です。
当然、一応求まります。 ですが、精度が良い方法ではありません。 ↑の図だとあんまりなさそうに見えますが、実際はそうでもないので、どんどん誤差が蓄積していきます。
ですが、言わば単純に掛け算をするだけなので、すごく簡単です。 確か区分求積法とかいう名前だったと思います。
精度を求める場合は何をするかというと、これに補間という計算をしてあげます。 それについては、別のところに書こうと思います。
プログラム解説
では、プログラミングしていきます。
① ヘッダのインクルード
GyroEstimator.cpp
#include "Common.h" #include "Renderer.h" #include "Scene.h" #include "wiimote.h" #include <mmsystem.h>
上3つはDirectXの描画用です。 wiimote.hはWiiYourself!を使うためです。
mmsystem.hですが、これはtimeGetTime()を使うためです。
② 定義とグローバル変数
GyroEstimator.cpp
#define CONNECT_TRY_NUM 2 // 繋がらなかったときに試す回数 #define STATIC_CALIB_NUM 100 // キャリブレーションの際のサンプリング数 // モーションプラスから取得した角速度 float g_speed_yaw = 0.0f; float g_speed_pitch = 0.0f; float g_speed_roll = 0.0f; // ポーズリセット用 bool g_isResetPose = false; bool g_isStaticCalibration = false;
CONNECT_TRY_NUMは、WiiYourself!を使ってWiiRemoteに接続する際に、一発で繋がらなかった時に試行する回数です。 実際のプログラムとはあんまり関係ないです。
STATIC_CALIB_NUMは静的キャリブレーションの際に使うサンプリング数です。 ジャイロにはオフセットというものが乗っているので、それを取り除くためにキャリブレーションを行います。これは、そのときに使う値です。 詳しくは後の方で説明します。
グローバル変数については、状態変化コールバック関数on_state_change()やメッセージ処理用コールバック関数WinProc()とメインプログラムWinMain()の値の受け渡し用です。
③ 状態変化コールバック関数
GyroEstimator.cpp
//=======================================================
// state-change callback
//=======================================================
void on_state_change( wiimote &remote,
state_change_flags changed,
const wiimote_state &new_state ) {
// 接続されたら
if( changed & CONNECTED )
{
if( new_state.ExtensionType != wiimote::BALANCE_BOARD ) {
if( new_state.bExtension )
remote.SetReportType( wiimote::IN_BUTTONS_ACCEL_IR_EXT ); // no IR dots
else
remote.SetReportType( wiimote::IN_BUTTONS_ACCEL_IR ); // IR dots
}
}
// モーションプラスの検出したら
if( changed & MOTIONPLUS_DETECTED ) {
if( remote.ExtensionType == wiimote_state::NONE ) {
bool res = remote.EnableMotionPlus();
_ASSERT(res);
}
// 拡張コネクタにモーションプラスが接続されてたら
} else if( changed & MOTIONPLUS_EXTENSION_CONNECTED ) {
if( remote.MotionPlusEnabled() )
remote.EnableMotionPlus();
// モーションプラスが拡張コネクタから切断されたら
} else if( changed & MOTIONPLUS_EXTENSION_DISCONNECTED ) {
// 再びモーションプラスのデータを有効にする
if( remote.MotionPlusConnected() )
remote.EnableMotionPlus();
// その他の拡張機器が接続されたら
} else if( changed & EXTENSION_CONNECTED ) {
if( !remote.IsBalanceBoard() )
remote.SetReportType( wiimote::IN_BUTTONS_ACCEL_EXT );
// その他の拡張機器が切断されたら
} else if( changed & EXTENSION_DISCONNECTED ) {
remote.SetReportType( wiimote::IN_BUTTONS_ACCEL_IR );
}
// 何らかの変化が起こったら
if( changed & CHANGED_ALL ) {
// リフレッシュ
remote.RefreshState();
// モーションプラスの状態変化が起こったら
if( changed & MOTIONPLUS_SPEED_CHANGED ) {
g_speed_yaw = remote.MotionPlus.Speed.Yaw;
g_speed_pitch = remote.MotionPlus.Speed.Pitch;
g_speed_roll = remote.MotionPlus.Speed.Roll;
}
}
}
重要なのは”何らかの変化が起こったら”のコメントの部分です。他はWiiYourself!のデモプログラムのコピペです。
まず、とりあえずリフレッシュします。 リフレッシュしないとWiiRemoteの値が更新されません。
そして、モーションプラスの状態変化が起こったら、各グローバル変数に現在のモーションプラスの角速度値を入れるようにします。 この角速度ですが、（おそらく）単位は[deg/s]です。 「1秒間あたりの角度」ですね。
この角速度値はメインループの方で使われます。
④ 初期化
GyroEstimator.cpp（WinMain関数内）
///------------ ここからwiiremote関連初期処理 ------------///
// wiimoteオブジェクト
wiimote wm;
// コールバック関数を設定
wm.ChangedCallback = on_state_change;
// コールバック条件を全フラグに設定
wm.CallbackTriggerFlags = (state_change_flags)(CONNECTED | CHANGED_ALL);
int count = 0;
while( !wm.Connect( wiimote::FIRST_AVAILABLE ) ) {
Sleep( 1000 );
if( count >= CONNECT_TRY_NUM ) { // CONNECT_TRY_NUM回試してもダメだったら
MessageBox( NULL, L"Wiiリモコンに接続できませんでした", L"エラー", MB_OK | MB_ICONERROR );
return 0;
}
count++;
}
count = 0;
///------------ ここまでwiiremote関連初期処理 ------------///
この辺もあまり特殊なことはしていません。
コールバック条件に全フラグを設定していますが、コネクトとモーションプラスの変化くらいでもいいかもしれないです。
⑤ メインループ – 変数
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; // テンポラリ TCHAR text[80]; // 転送用文字配列
各変数の説明をします。 このグラフを見つつ、イメージをつかんでみてください。
yaw、pitch、roll
まず、yaw、pitch、rollですが、算出した角度の変化量が入ります。 これはグラフでいう、一個一個の長方形の面積です。
angle_yaw、angle_pitch、angle_roll
次に、angle_yaw、angle_pitch、angle_rollですが、現在の姿勢が入ります。 これはグラフでいう、全部の面積です。 グラフでは値がプラスにしか行ってないので、全部足したらどんどん増えていってしまうと感じてしまうかと思いますが、実際はマイナスもあるので、そんなことはありません。 また、後で説明しますが、オーバーフロー対策も行っています。
offset_yaw、offset_pitch、offset_roll
次に、offset_yaw、offset_pitch、offset_rollですが、オフセット修正値が入ります。 このオフセットとは、WiiRemoteの仕様でも説明したように、ゼロ点からの差です。 そのゼロ点からの差をなくすための修正値が、この変数に入ります。 初期値として、今回は私のWiiRemoteでのオフセット修正値を入れましたが、各々のWiiRemoteによって特性が若干異なりますので、適宜書き換えてください。 実際のソフトウェアで使うような場合は、こんなことは絶対してはいけなくて、必ず使用前にキャリブレーションをさせるようにするのですが（実際のWiiのソフトでもキャリブレーションはゲームの前に行っている（置いて数秒待ってください的なあれ））、私はいちいち毎回キャリブレーションさせるのが面倒なので、このようにしました。 キャリブレーションについては後で説明します。
temp_yaw、temp_pitch、temp_roll
次に、temp_yaw、temp_pitch、temp_rollですが、キャリブレーションの際に使う格納用変数です。
text
最後に、textですが、これは文字の転送用の変数です。 実際の処理的にはそんなに関係ありません。
static
staticで定義されている変数ですが、意味を理解すれば当然だとわかるかと思いますが、毎回初期化されては困る変数たちです。
⑥ メインループ – キャリブレーションとオフセットの設定
GyroEstimator.cpp（WinMain関数メインループ内）
// 静的キャリブレーション
if( g_isStaticCalibration ) {
temp_yaw += g_speed_yaw;
temp_pitch += g_speed_pitch;
temp_roll += g_speed_roll;
count++;
if( count >= STATIC_CALIB_NUM ) {
offset_yaw = temp_yaw / STATIC_CALIB_NUM;
offset_pitch = temp_pitch / STATIC_CALIB_NUM;
offset_roll = temp_roll / STATIC_CALIB_NUM;
temp_yaw = 0;
temp_pitch = 0;
temp_roll = 0;
count = 0;
g_isStaticCalibration = false;
}
}
// 角速度補正
g_speed_yaw -= offset_yaw;
g_speed_pitch -= offset_pitch;
g_speed_roll -= offset_roll;
// オフセットの表示
_stprintf_s( text, TEXT("%4.3f %4.3f %4.3f"),
offset_yaw, offset_pitch, offset_roll);
for( int i=0; i<80; i++ ) {
renderer.m_text3[i] = text[i];
}
ここでは、静的キャリブレーションを行い、オフセットの設定をしています。 この辺はとてもセンサの制御っぽぃところです。
静的キャリブレーションとは、“動かさない”キャリブレーションです。 要するに、「WiiRemoteを置いたまま（WiiRemoteが動かない状態）」で行う処理です。 Wiiでいう「置いたまま数秒待ってください」的なあれです。
g_isStaticCalibrationフラグがtrueになると（キーボード操作）、静的キャリブレーションをし始めます。
実際何をするかというと、動かない状態での角速度を見て、それをオフセットにするだけです。 あとは、値に多少バラつきがあるので、平均を取っているだけです。
まず、テンポラリ変数に現在の角速度を足しこんでいきます。 STATIC_CALIB_NUM回分の角速度を取ったら、STATIC_CALIB_NUMでテンポラリ変数を割って平均を出します。 それをオフセットにします。
そのままほっとくとマズイので、初期化をしてあげて、g_isStaticCalibrationフラグをfalseに戻します。
その後にやっているのは、そのオフセットを使って、角速度を補正しているだけです。 ゼロ点補正ですね。
最後にどのくらいのオフセットが出ているのかを確認するために、オフセットの表示を行っています。 ここの値を参考にオフセットの初期値を決めてもいいかもしれません。
⑦ メインループ – 角度算出
GyroEstimator.cpp（WinMain関数メインループ内）
// フレームレート計測
static DWORD time = timeGetTime(); // 単位は[ms]
// 1フレームあたりの時間計測
DWORD frametime = timeGetTime() - time;
time = timeGetTime();
// 角度算出
if( g_speed_yaw > 10 || g_speed_yaw < -10 ) {
yaw = g_speed_yaw * frametime / 1000;
angle_yaw += yaw;
}
if( g_speed_pitch > 10 || g_speed_pitch < -10 ) {
pitch = g_speed_pitch * frametime / 1000;
angle_pitch += pitch;
}
if( g_speed_roll > 10 || g_speed_roll < -10 ) {
roll = g_speed_roll * frametime / 1000;
angle_roll += roll;
}
ここが重要な角度算出の部分です。
フレームレート計測
まず、フレームレート（単位時間あたり何度画面が更新されるかを表す数）を計測します。 1フレームあたりの時間（＝frametime）が必要になるので、これを求めています。
timeGetTime()で取った時間を、初期値とします。 現在の時間（timeGetTime()）から、1つ前のフレームのときの数（＝time）を引けば、1フレームあたりの時間が求まります。
この1フレームあたりの時間がグラフでいう、“値が送られてくる間の一個一個の時間”になります。
角度算出
次に、この1フレームあたりの時間を使って、角度を算出していきます。 実際の計算は、
角度＝角速度[deg/sec]×時間[sec]
をしているだけです。
timeGetTime()で得られる時間の単位は[msec]（ミリ秒）なので、1000で割っています。
角度が求まったら、それを姿勢の変化量にするためにangleに足しこんでいきます。
ここで、センサの制御的に重要なのが、if文で囲んでいるところです。
WiiRemoteことはじめにも書きましたが、センサの値にはバラつきがあります。 この辺が誤差の問題に絡んでくるのですが、このif文で囲まないと、全ての角速度の値の変化が姿勢に影響していきます。 これは大きな誤差の元になるので、低周波成分（変化量が少ないときの角速度）をカット（無視）します。 これの基準となる値のことをカットオフ周波数とか読んだりします。 こうすることで、バラつきによる影響を軽減させています。
10という値（カットオフ周波数）についてですが、WiiRemoteを静止させた状態で値を見ていき、このくらいなら大丈夫だろうという値を設定しました。 なので、あまりにもゆっくり回転させると、カットされて姿勢に反映されないかもしれません。 この辺は感度の問題になるかもしれないです。
一応目安として、WiiRemoteの分解能を参考にするといいかもしれないです。 これもWiiRemoteの仕様に書きましたが、WiiRemoteには低速モードと高速モードがあります。 それぞれの分解能が、
SlowMode（低速時モード）→ 1.45[deg/s]
FastMode（高速時モード）→ 6.59[deg/s]
となっています。 今のカットオフ周波数では低速時モードの最大限にゆっくりなときには対応できなくなっていますが、バラつきを考えるとこのくらいでいいかと思います。 適宜書き換えて試してみてください。
⑧ メインループ – 姿勢リセット、オーバーフロー対策など
GyroEstimator.cpp（WinMain関数メインループ内）
// 姿勢のリセット
if( g_isResetPose ) {
angle_yaw = angle_pitch = angle_roll = 0.0f;
g_isResetPose = false;
}
// 角度のオーバーフロー対策
if( yaw <= -180 ) {
angle_yaw = 180;
}
if( yaw > 180 ) {
angle_yaw = -180;
}
if( pitch <= -180 ) {
angle_pitch = 180;
}
if( pitch > 180 ) {
angle_pitch = -180;
}
if( roll <= -180 ) {
angle_roll = 180;
}
if( roll > 180 ) {
angle_roll = -180;
}
// シーンに値を送る
scene.SetYawPitchRoll( angle_yaw, angle_pitch, angle_roll );
// 各姿勢の表示
_stprintf_s( text, TEXT("%4.3f %4.3f %4.3f"),
angle_yaw, angle_pitch, angle_roll);
for( int i=0; i<80; i++ ) {
renderer.m_text2[i] = text[i];
}
// 描画処理の実行
renderer.RenderScene( &scene );
あとは大したことないです。 山は越えました。
まず、姿勢のリセットですが、初期姿勢を定義するためのものです。 先ほども説明しましたが、ジャイロによる姿勢推定は動的なものです。 なので、初期姿勢を定義する必要があります。
「この姿勢を初期姿勢にする」という状態にWiiRemoteの姿勢をして、キーボードのキーを押してみてください。
次に、オーバーフロー対策ですが、これは単純に値がオーバーフローしないように、-180° ～ +180°で周波数的になるようにしているだけです。
次に、3D描画のために姿勢をシーンに送ります。 さらに、各姿勢の表示も行います。
最後に、描画処理の実行をして、メインループの処理は終わりです。
まとめ
以上で、WiiYourself!でモーションプラスを使った角度の算出（単純積分）及び、それを使った姿勢推定についての説明は終了です。 DirectX部分については一切説明しませんでしたが、角度の算出や姿勢推定の角度自体は出ているので、他の事に使ってもいいかと思います。
カットオフ周波数や積分の蓄積誤差などが気になる方は、加速度の姿勢推定も組み合わせて使うと、Wiiで実際に使われている並みの姿勢推定ができるようになるかもしれません。 実際にぶんぶん振り回して遊ぶ場合は、こんな風に姿勢推定を3D画面で表示させるようなことはなく、この場合はモーション解析などが必要になってくるので、そんなに気にしなくてもいい気はします。
あとは、社長が訊く『Wiiモーションプラス』あたりを見ていると、実際には学習アルゴリズムかなんかも使われてそうなイメージはあります。
ちなみに、ジャイロセンサについての知識は、センサ関連の技術として結構ネット上にあったりします。主にロボット関連で使われているので、その辺りから探ってみてもいいかもしれません。
まぁなんか色々やって遊んでみてください。