如何获取重力方向

Question

我需要基于加速度计、陀螺仪和磁力计来计算线性加速度。我找到了一个针对Android的应用程序，可以完全实现我的要求：https://play.google.com/store/apps/details?id=com.kircherelectronics.fusedlinearacceleration 。https://github.com/KEOpenSource/FusedLinearAcceleration。

我正试图将其移植到纯Java中。因为代码的某些元素是基于虚拟传感器（重力传感器），所以我想通过计算基于三个基本传感器的重力方向来实现相同的结果。我读到可以使用低通滤波器（与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中），它返回与Android类似的重力方向数组(x,y,z)，但不使用Android API。

- Bresiu

---

请将您的问题格式化，以便人类能够阅读。 - eliasah

@eliasah 我需要进一步提高可读性，还是现在就可以了？ - Bresiu

你的问题仍然不够清晰。 - eliasah

这可能会有所帮助：https://dev59.com/M07Sa4cB1Zd3GeqP2ERs#2986854 - bcorso

我发现引力向量可以从四元数计算出来。 我使用了这些方法：https://gist.github.com/Bresiu/c0a69552881b8df34a2e。 在第一种方法中，我计算deltaRotationVector，它们是四元数，而在第二种方法中，我的代码基于：http：//diydrones.com/forum/topics/accelerometer-sensor-gravity-compensation equations. 但是在我实施后，只有一个轴没有计算引力。 - Bresiu

@bcorso 我认为要计算“旋转矩阵”，我需要重力向量... - Bresiu

Answer 1

重力是加速度计信号（x、y和z）中的稳定贡献。 因此，为了隔离时间函数中的重力值，只需以2Hz的频率低通滤波3个加速度计信号即可。 一个简单的FIR可以完成这项工作。 在这个网站上，我计算出了以下系数：

[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]

基于以下特征： Fa=0Hz，Fb=1Hz，长度=21个点，Fs=100Hz，Att=60dB。

您将获得一个信号，它将是重力随时间变化的三个值。

您可以在这里找到一些有关FIR的说明和Java实现。

- LM.Croisez

---

我已经通过低/高通滤波器对输入进行了处理。不过我会再看看的。谢谢。 - Bresiu

Answer 2

你需要的是旋转矩阵（SensorManager.getRotationMatrix）。它的最后三个分量（即rotation[6]、rotation[7]、rotation[8]）是指向正上方的向量，因此指向地心的方向是其相反数。要从加速度计读数中减去重力，只需将该向量乘以g（约为9.8m/s^2，但你可能想更精确地知道）。

- Theodore Sternberg