Android:SensorManager.getRotationMatrix 和 SensorManager.getOrientation() 的算法

Question

要在 Android 中从欧拉角(例如俯仰、滚转、方位角)中获取方向，需要执行以下操作:

SensorManager.getRotationMatrix(float[] R, float[] I, float[] Gravity, float[] geomagnetic);

SensorManager.getOrientation(float[] R, float[]orientation);

在第一个中，我意识到它使用了一种 TRIAD 算法；旋转矩阵(R[])由重力、地磁X重力、重力X(地磁X重力)---X为叉积组成。请参阅以下代码:

    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;
        }
    }

但是，我无法理解 SensorManager.getOrientation()。

 azimuth = (float)Math.atan2(R[1], R[4]);
 pitch = (float)Math.asin(-R[7]);
 roll = (float)Math.atan2(-R[6], R[8]);

获得欧拉角的确切算法是什么？

Answer 1

让我试着解释一下:
getRotationMatrix 根据重力和磁矢量组成旋转矩阵。

我们的主要目标是构建 NED frame

我们假设重力指向地球的中心，而磁铁指向北极。但在实际情况下，这些向量是非垂直的，这就是为什么我们首先计算与 E 和 A 正交并属于切线平面的向量 H。 H 是叉积 (E x A)，与 E 和 A 正交。

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);

归一化加速度和 H 向量(因为这些向量将构成 ENU 坐标系的基础)

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;

求最后一个基向量 (M) 作为 H 和 A 的叉积:

double Mx = Ay * Hz - Az * Hy;
double My = Az * Hx - Ax * Hz;
double Mz = Ax * Hy - Ay * Hx;

体坐标系中任意向量 (a) 的坐标通过 NED 坐标表示为 a = Ra'
R - 变换矩阵矩阵，其列是旧基中新基向量的坐标

但是NED框架中的坐标是
计算为 a' = T^(-1) * a。对于正交变换矩阵逆等于转置矩阵。因此我们有:

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;

一旦我们有了旋转矩阵，我们就可以将其转换为欧拉角表示。转换公式取决于您使用的 convention。
你的公式

azimuth = (float)Math.atan2(R[1], R[4]);
pitch = (float)Math.asin(-R[7]);
roll = (float)Math.atan2(-R[6], R[8]);

对于具有约定 Y-X-Z 的 Tiat Bryan 角是正确的。为了更好地理解从旋转矩阵到欧拉角的转换，我建议研究 Gregory G. Slabaugh - "Computing Euler angles from a rotation matrix" 的一篇文章