Как получить направление гравитации

Мне нужно рассчитать линейное ускорение на основе акселерометра, гироскопа и магнитометра. Я нашел приложение для android, которое делает именно то, что я хочу достичь:

https://play.google.com/store/apps/details?id=com.kircherelectronics.fusedлинейное ускорение. https://github.com/KEOpenSource/FusedLinearAcceleration

Я пытаюсь перенести его в чистую java. Поскольку некоторые элементы кода основаны на виртуальных датчиках (Gravity Sensor), я хотел бы добиться того же результата путем вычисления направления силы тяжести на основе трех базовых датчиков. Я читал, что сила тяжести может быть рассчитана с использованием фильтра низких частот (так же, как Android <4.0), но этот метод не дает очень точных результатов.

Начиная с android 4.0, сила тяжести на каждой оси рассчитывается с использованием слияния датчиков. Я нашел код, отвечающий за эти измерения, но он написан в CPP:

https://github.com/android/platform_frameworks_base/blob/ics-mr1/services/sensorservice/GravitySensor.cpp

Используемый там метод называется "getRotationMatrix". Тот же метод в классе SensorManager.java: https://gitorious.org/android-eeepc/base/source/9cb3e09ec49351401cf19b5ae5092dd9ca90a538:core/java/android/hardware/SensorManager.java#L1034

    public static boolean getRotationMatrix(float[] R, float[] I,
        float[] gravity, float[] geomagnetic) {
    // TODO: move this to native code for efficiency
    float Ax = gravity[0];
    float Ay = gravity[1];
    float Az = gravity[2];
    final float Ex = geomagnetic[0];
    final float Ey = geomagnetic[1];
    final float Ez = geomagnetic[2];
    float Hx = Ey*Az - Ez*Ay;
    float Hy = Ez*Ax - Ex*Az;
    float Hz = Ex*Ay - Ey*Ax;
    final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
    if (normH < 0.1f) {
        // device is close to free fall (or in space?), or close to
        // magnetic north pole. Typical values are  > 100.
        return false;
    }
    final float invH = 1.0f / normH;
    Hx *= invH;
    Hy *= invH;
    Hz *= invH;
    final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
    Ax *= invA;
    Ay *= invA;
    Az *= invA;
    final float Mx = Ay*Hz - Az*Hy;
    final float My = Az*Hx - Ax*Hz;
    final float Mz = Ax*Hy - Ay*Hx;
    if (R != null) {
        if (R.length == 9) {
            R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
            R[3] = Mx;     R[4] = My;     R[5] = Mz;
            R[6] = Ax;     R[7] = Ay;     R[8] = Az;
        } else if (R.length == 16) {
            R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
            R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
            R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
            R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
        }
    }
    if (I != null) {
        // compute the inclination matrix by projecting the geomagnetic
        // vector onto the Z (gravity) and X (horizontal component
        // of geomagnetic vector) axes.
        final float invE = 1.0f / (float)Math.sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
        final float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
        final float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
        if (I.length == 9) {
            I[0] = 1;     I[1] = 0;     I[2] = 0;
            I[3] = 0;     I[4] = c;     I[5] = s;
            I[6] = 0;     I[7] =-s;     I[8] = c;
        } else if (I.length == 16) {
            I[0] = 1;     I[1] = 0;     I[2] = 0;
            I[4] = 0;     I[5] = c;     I[6] = s;
            I[8] = 0;     I[9] =-s;     I[10]= c;
            I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
            I[15] = 1;
        }
    }
    return true;
}

принимает четыре аргумента:

float [] R, float [] I, float [] gravity, float [] Geomagnetic.

Один из них - просто гравитация... Код, над которым я сейчас работаю, похож на

https://github.com/KEOpenSource/FusedLinearAcceleration/blob/master/FusedLinearAcceleration/src/com/kircherelectronics/fusedlinearacceleration/sensor/LinearAccelerationSensor.java,

за исключением методов, которые относятся к SensorManager. Они скопированы из источника android:

https://gitorious.org/android-eeepc/base/source/9cb3e09ec49351401cf19b5ae5092dd9ca90a538:core/java/android/hardware/SensorManager.java.

Я не нашел примеров того, как реализовать это на Java.

Поэтому мой вопрос: как я могу реализовать метод (в java), основанный только на трех основных датчиках, который возвращает мне массив гравитационного направления (x, y, z), аналогичный Android one, но без использования API Android.