加速度センサーとジャイロと気圧計でポジショントラッキングするメモ

もう結構前になりますが，OclusRift DK2を買いました．
DK2 はカメラでポジショントラッキングしていますが，スマホのセンサでもポジショントラッキングしたかったので，試行錯誤した結果のメモ．

加速度センサーとジャイロ

理論的には加速度を二回積分すれば位置がわかります．また端末を立体的に動かすと回転運動も加わるのでジャイロで回転量を取得する必要があります．

二回積分する時点で常識的に考えると現実的ではないように思われますが，最近のスマートフォンのセンサの精度と処理速度ならそれなりに行けるのではと考えての挑戦です．

このへんは1年前に「ななか+INSIDE PRESS Vol.4」で Oculus Rift もどきを作った話を書いた時と，だいたい同じ．

メモ：

• 加速度センサのゼロ点がずれてたりするので自前キャリブレーション必須
• 手元の端末見た限りだとジャイロはそこまで個体差は大きくない
• 端末が静止している状態で重力加速度の方向と大きさを調べる
• ジャイロの値を積分して回転量を求める
• 加速度センサの値を回転した後，重力加速度を引く
• 積分する
• 人間が長時間等速直線運動することはほぼ無いので，静止状態を検出して適当に速度を補正

数秒程度ならそれなりの精度でトラッキング出来ます．
地球上にいる限り，9.8m/s^2で加速している状態の中での細かい動きを取得しないといけないので，重力加速度を精度よく分離するためにもジャイロの値は重要です．

コードとか

(一旦適当に貼っただけです．動くコードは https://github.com/binzume/android-pos-track )．
謎のオレオレVectorクラスが使われてしまってますが，雰囲気はわかると思います．

センサの基本的な使い方は http://developer.android.com/reference/android/hardware/Sensor.html を参照してください．

角速度

単に行列にジャイロの値を掛けていくだけです．

AndroidのAPIには SensorManager.getRotationMatrix() という，加速度センサと地磁気コンパスの値から，回転行列を求めてくれる便利関数がありますが，その瞬間の加速度だけを使って計算するため，重力加速度以外の加速度がかかっている場合は残念ながら使えません．

if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
    if (lastGyroTime > 0) {
        float dt = (event.timestamp - lastGyroTime) * 0.000000001f;
        if (landscape) {
            gyroVec.set(-event.values[1], event.values[0], event.values[2]);
        } else {
            gyroVec.set(event.values[0], event.values[1], event.values[2]);
        }
        Matrix.rotateM(rotationMatrix, 0, gyroVec.length() * dt * 180 / PI, gyroVec.array()[0], gyroVec.array()[1], gyroVec.array()[2]);
    }
    lastGyroTime = event.timestamp;
}

加速度

誤差をごまかすために色々やっているので，主な部分だけ．
eventはAndroidから渡されたものではなく，valuesの値にキャリブレーション結果を反映させてあります．

TODO: 謎の定数たくさん出てくるのをあとでどうにかする．

if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
    if (landscape) {
        accVecN.set(-event.values[1], event.values[0], event.values[2]);
    } else {
        accVecN.set(event.values[0], event.values[1], event.values[2]);
    }

    float dt = (event.timestamp - lastAccelTime) * 0.000000001f; // dt(sec)
    if (lastAccelTime > 0 && dt < 0.5f && System.currentTimeMillis() - resetTime > 500) {

        // m/s^2
        Matrix.multiplyMV(accVec.values, 0, rotationMatrix, 0, accVecN.values, 0); // rotMatrix * groundA
        accVec.values[1] -= G;

        // velocity(mm/s)
        vVec.values[0] += accVec.values[0] * dt * 1000;
        vVec.values[1] += accVec.values[1] * dt * 1000;
        vVec.values[2] += accVec.values[2] * dt * 1000;

        // velocity limit
        if (vVec.length() > 5000) {
            vVec.scale(0.95f);
        }

        boolean resting = false;
        accHistory[(accHistoryCount++) % accHistory.length] = accVec.length();
        if (accHistoryCount > accHistory.length) {
            final float l = accVec.length();
            float min = l, max = l, sum = 0;
            for (float a : accHistory) {
                sum += a;
                if (a > max)
                    max = a;
                if (a < min)
                    min = a;
            }
            if (sum < 2.5f && max - min < 0.2f) {
                resting = true;
                vVec.scale(0.9f);
                if (max - min < 0.1f) {
                    vVec.set(0, 0, 0);
                }
            }
        }

        // position(mm)
        if (vVec.length() > 0.5f) {
            posVec.values[0] += vVec.values[0] * dt;
            posVec.values[1] += vVec.values[1] * dt;
            posVec.values[2] += vVec.values[2] * dt;
        }
    // 以下略

下方向の調整

静止時に徐々に「下」方向をあわせる．リセット直後は一気に合わせるように．
外積とって回転するだけです．

Android の android.opengl.Matrix を使ってるせいで，操作が右からしか掛けられなくてすこし不便です．android.graphics.Matrix は pre と post 両方のメソッドあるのですが，こいつは残念ながら3*3行列版しかありません．

    // estimated ground vec.
    Matrix.multiplyMV(groundVec.array(), 0, rotationMatrix, 0, accVec.values, 0);
    float theta = (float) Math.acos(groundVec.dot(gravityVecI));
    if (theta > 0) {
        float[] cross = groundVec.cross(gravityVecI).normalize().array();
        float factor = (System.currentTimeMillis() - resetTime < 500) ? 0.8f : 0.0005f;
        Matrix.setRotateM(rotationMatrix_t1, 0, theta * 180 / PI * factor, cross[0], cross[1], cross[2]);
        Matrix.multiplyMM(rotationMatrix_t2, 0, rotationMatrix_t1, 0, rotationMatrix, 0);
        MatrixUtil.copy(rotationMatrix, rotationMatrix_t2);
    }

気圧センサー

気圧センサーでも高さ取れるよと少し前に教えてもらったので試してみます．

Androidの気圧センサーは Sensor.TYPE_PRESSURE を指定して SensorManager から取得します．
気圧センサーは搭載れてない端末もそれなりにあると思いますが，とりあえず Nexus 5 等は使えます．

動きが激しくて重力の方向を見失っている場合や，長時間等速度運動に近い動きをしている場合は加速度センサより大分良いです．

メモ：

• 約0.01hPa単位で取得できる
• 常温，1気圧の場所なら，0.01hPa の変化は 83mm 程度の高低差
• ノイズはそこそこあるが，200mmくらい動けばほぼ検出できる
• 室内だと換気扇等の影響はとても大きい(1hPa近く変化する)．窓開けておく必要ある
• 気圧センサの値は200msくらい遅延がある

コードとか

面倒なことはしてないです．必要なのは，pressureHeightCurrentだけですね．

if (event.sensor.getType() == Sensor.TYPE_PRESSURE) {
    final float v = event.values[0];
    pressHistory[pressHistoryCount++ % pressHistory.length] = v;
    if (pressHistoryCount >= pressHistory.length) {
        float min = v, max = v;
        for (float vv : pressHistory) {
            if (max < vv)
                max = vv;
            if (min > vv)
                min = vv;
        }
        if (max - min > 0.025f) {
            Log.d("Sensor", "TYPE_PRESSURE changed current: " + v + "min:max = " + min + ":" + max);
        }
        if (pressureHeightErrorBaseTime == 0) {
            pressureHeightErrorBaseTime = event.timestamp;
            pressureHeightBase = (max + min) / 2.0f;
        }
        pressureHeightCurrent = (pressureHeightBase - v) * 8300f; // todo: h=153.8*(temp+273.2)*(1-(v/ATOM_BASE)^0.1902) 
        pressureHeightErrorHigh + pressureHeightErrorFactor * ((event.timestamp - pressureHeightErrorBaseTime)*0.000001f)
        pressureHeightErrorLow + pressureHeightErrorFactor * ((event.timestamp - pressureHeightErrorBaseTime)*0.000001f)
    }
    lastPressureTime = event.timestamp;
}

結果

適当にスマホを上下に動かした時の結果(グラフ作るの面倒だったのでスクリーンショットそのまま)．
グラフの横幅は約30秒，縦は±1m．青が加速度センサとジャイロから予測した上下の移動量，赤が気圧から予測した移動量．緑はZ軸方向の移動量ですが今回は関係ないです．

だいたい500mmくらいの幅で適当に動かした状態．

激しくスマホを動かすと，青い線ははるか彼方へ行ってしまうので，気圧から求めた高度があると，高さに限っては制限が掛けられます．ただ，室内だと換気扇の機嫌次第で気圧が変わったりしていたので実際に使うかは微妙なところ．

まとめ

• Android用の実装： https://github.com/binzume/android-pos-track
• 短時間＆狭い範囲のトラッキングならそこそこいける
• あまり安定してないので実際に使うには色々面倒そう
• スマホのカメラ使ってリアルタイムSfMしようとして挫折中．．．