How to get direction of gravity

Question

I need to calculate the linear acceleration based on the accelerometer, gyroscope and magnetometer. I found an application for android, which does exactly what I want to achieve:

https://play.google.com/store/apps/details?id=com.kircherelectronics.fusedlinearacceleration. https://github.com/KEOpenSource/FusedLinearAcceleration

I'm trying to port it to a pure java. Because some elements of the code are based on virtual sensors (Gravity Sensor), I would like to achieve the same result by compute direction of gravity based on three basic sensors. I read that the force of gravity can be calculated using the Low Pass Filter (same as Android < 4.0), but this method does not give very accurate results.

From android 4.0, the force of gravity on each axis is calculated using sensor fusion. I found the code responsible for these measurements, but it is written in the CPP:

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

Method used there is called "getRotationMatrix". The same method in SensorManager.java class: 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;
}

takes four arguments:

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

One of them is just gravity... The code I'm working on currently is similar to

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

with the exception of methods that refer to SensorManager. These are copied from android source:

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

I did not found any examples of how implement this in Java.

So my question is: How I can implement method (in java), based only on three basic sensors, which returns me array of gravity direction (x, y, z), similar to Android one, but without using Android API.

LM.Croisez · Accepted Answer

Gravity is a steady contribution in the accelerometer signals (x, y & z). So, logically, to isolate the gravity values in function of time, just low-pass filter the 3 accelerometer signals, at a frequency of 2Hz, for example. A simple FIR would do the job.

On this site I calculated the following coefficients:

[0.000381, 0.001237, 0.002634, 0.004607, 0.007100, 0.009956, 0.012928, 
0.015711, 0.017987, 0.019480, 0.020000, 0.019480, 0.017987, 0.015711, 
0.012928, 0.009956, 0.007100, 0.004607, 0.002634, 0.001237, 0.000381]

based on those caracteristics: Fa=0Hz, Fb=1Hz, Length=21Pts, Fs=100Hz, Att=60dB.

You will get a signal that will be the three values of gravity in function of time.

You can find here some FIR explaination and Java implementation.

How to get direction of gravity

Answers (2)

Related Questions