将设备中的磁场X、Y、Z值转换为全局参考系。

Question

当您使用TYPE_MAGNETOMETER传感器时，您会得到磁场强度的X、Y、Z值与设备方向有关。我想要的是将这些值转换为全局参考框架，澄清：用户拿起设备，测量这些值，然后围绕任何轴旋转设备一定角度并获得几乎相同的值。 请在下面找到类似的问题： 获取全局坐标中的磁场值 如何获取磁场矢量，不受设备旋转的影响？ 在这个答案示例中描述了一个简单的解决方案（它是针对线性加速度的，但我认为这并不重要）：https://dev59.com/wGgu5IYBdhLWcg3wGjZF#11614404 我使用它并得到了3个值，X始终非常小（我认为这不正确），Y和Z还可以，但它们在我旋转设备时仍然会稍微改变。如何调整呢？能否完全解决？我使用简单的卡尔曼滤波器来近似测量值，因为如果没有它，即使设备根本没有移动/旋转，我也会得到非常不同的值。请在下面找到我的代码：

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;
import android.os.Bundle;
import android.view.View;
import android.widget.CheckBox;
import android.widget.TextView;
import com.test.statistics.filter.kalman.KalmanState;
import com.example.R;

/**
 * Activity for gathering magnetic field statistics.
 */
public class MagneticFieldStatisticsGatheringActivity extends Activity implements SensorEventListener {

    public static final int KALMAN_STATE_MAX_SIZE = 80;
    public static final double MEASUREMENT_NOISE = 5;

    /** Sensor manager. */
    private SensorManager mSensorManager;
    /** Magnetometer spec. */
    private TextView vendor;
    private TextView resolution;
    private TextView maximumRange;

    /** Magnetic field coordinates measurements. */
    private TextView magneticXTextView;
    private TextView magneticYTextView;
    private TextView magneticZTextView;

    /** Sensors. */
    private Sensor mAccelerometer;
    private Sensor mGeomagnetic;
    private float[] accelerometerValues;
    private float[] geomagneticValues;

    /** Flags. */
    private boolean specDefined = false;
    private boolean kalmanFiletring = false;

    /** Rates. */
    private float nanoTtoGRate = 0.00001f;
    private final int gToCountRate = 1000000;

    /** Kalman vars. */
    private KalmanState previousKalmanStateX;
    private KalmanState previousKalmanStateY;
    private KalmanState previousKalmanStateZ;
    private int previousKalmanStateCounter = 0;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.main2);
        mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);

        mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        mGeomagnetic = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

        vendor = (TextView) findViewById(R.id.vendor);
        resolution = (TextView) findViewById(R.id.resolution);
        maximumRange = (TextView) findViewById(R.id.maximumRange);

        magneticXTextView = (TextView) findViewById(R.id.magneticX);
        magneticYTextView = (TextView) findViewById(R.id.magneticY);
        magneticZTextView = (TextView) findViewById(R.id.magneticZ);

        mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_FASTEST);
        mSensorManager.registerListener(this, mGeomagnetic, SensorManager.SENSOR_DELAY_FASTEST);
    }

    /**
     * Refresh statistics.
     *
     * @param view - refresh button view.
     */
    public void onClickRefreshMagneticButton(View view) {
        resetKalmanFilter();
    }

    /**
     * Switch Kalman filtering on/off
     *
     * @param view - Klaman filetring switcher (checkbox)
     */
    public void onClickKalmanFilteringCheckBox(View view) {
        CheckBox kalmanFiltering = (CheckBox) view;
        this.kalmanFiletring = kalmanFiltering.isChecked();
    }

    @Override
    public void onSensorChanged(SensorEvent sensorEvent) {
        if (sensorEvent.accuracy == SensorManager.SENSOR_STATUS_UNRELIABLE) {
            return;
        }
        synchronized (this) {
            switch(sensorEvent.sensor.getType()){
                case Sensor.TYPE_ACCELEROMETER:
                    accelerometerValues = sensorEvent.values.clone();
                    break;
                case Sensor.TYPE_MAGNETIC_FIELD:
                    if (!specDefined) {
                        vendor.setText("Vendor: " + sensorEvent.sensor.getVendor() + " " + sensorEvent.sensor.getName());
                        float resolutionValue = sensorEvent.sensor.getResolution() * nanoTtoGRate;
                        resolution.setText("Resolution: " + resolutionValue);
                        float maximumRangeValue = sensorEvent.sensor.getMaximumRange() * nanoTtoGRate;
                        maximumRange.setText("Maximum range: " + maximumRangeValue);
                    }
                    geomagneticValues = sensorEvent.values.clone();
                    break;
            }
            if (accelerometerValues != null && geomagneticValues != null) {
                float[] Rs = new float[16];
                float[] I = new float[16];

                if (SensorManager.getRotationMatrix(Rs, I, accelerometerValues, geomagneticValues)) {

                    float[] RsInv = new float[16];
                    Matrix.invertM(RsInv, 0, Rs, 0);

                    float resultVec[] = new float[4];
                    float[] geomagneticValuesAdjusted = new float[4];
                    geomagneticValuesAdjusted[0] = geomagneticValues[0];
                    geomagneticValuesAdjusted[1] = geomagneticValues[1];
                    geomagneticValuesAdjusted[2] = geomagneticValues[2];
                    geomagneticValuesAdjusted[3] = 0;
                    Matrix.multiplyMV(resultVec, 0, RsInv, 0, geomagneticValuesAdjusted, 0);

                    for (int i = 0; i < resultVec.length; i++) {
                        resultVec[i] = resultVec[i] * nanoTtoGRate * gToCountRate;
                    }

                    if (kalmanFiletring) {

                        KalmanState currentKalmanStateX = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[0], (double)resultVec[0], previousKalmanStateX);
                        previousKalmanStateX = currentKalmanStateX;

                        KalmanState currentKalmanStateY = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[1], (double)resultVec[1], previousKalmanStateY);
                        previousKalmanStateY = currentKalmanStateY;

                        KalmanState currentKalmanStateZ = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[2], (double)resultVec[2], previousKalmanStateZ);
                        previousKalmanStateZ = currentKalmanStateZ;

                        if (previousKalmanStateCounter == KALMAN_STATE_MAX_SIZE) {
                            magneticXTextView.setText("x: " + previousKalmanStateX.getX_estimate());
                            magneticYTextView.setText("y: " + previousKalmanStateY.getX_estimate());
                            magneticZTextView.setText("z: " + previousKalmanStateZ.getX_estimate());

                            resetKalmanFilter();
                        } else {
                            previousKalmanStateCounter++;
                        }

                    } else {
                        magneticXTextView.setText("x: " + resultVec[0]);
                        magneticYTextView.setText("y: " + resultVec[1]);
                        magneticZTextView.setText("z: " + resultVec[2]);
                    }
                }
            }
        }
    }

    private void resetKalmanFilter() {
        previousKalmanStateX = null;
        previousKalmanStateY = null;
        previousKalmanStateZ = null;
        previousKalmanStateCounter = 0;
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int i) {
    }
}

谢谢所有阅读此帖并事先发表一些关于问题的想法的人。

- Yahor

---

你能分享 com.test.statistics.filter.kalman.KalmanState 的代码吗？我需要使用卡尔曼滤波器来过滤传感器输出。 - Thracian

Answer 1

在您提供的链接上，我在评论中提到了我的简单答案，链接为calculate acceleration in reference to true north。

让我在这里再次回答并更加明确。答案是旋转矩阵和磁场值的乘积。如果您进一步阅读“X总是非常小”则是正确的值。

加速度计和磁场传感器分别测量设备的加速度和地球在设备位置处的磁场。它们是三维空间中的向量，称之为a和m。
如果您站着不动并旋转设备，则理论上m不会改变，假设周围没有来自其他物体的磁干扰（实际上，如果您在短距离内移动，则m应该会稍微改变，因为地球的磁场应该会在短距离内略微改变）。但是，a会发生变化，即使在大多数情况下，它不应该很剧烈。

现在，三维空间中的向量v可以由与某个基础（e_1，e_2，e_3）相关的三元组（v_1、v_2、v_3）表示，即v = v_1 e_1 + v_2 e_2 + v_3 e_3。（v_1、v_2、v_3）被称为v相对于基础（e_1，e_2，e_3）的坐标。

在Android设备中，基础是（x，y，z），其中对于大多数手机，x沿着较短的一侧指向右侧，y沿着较长的一侧指向上方，z垂直于屏幕并指向外部。
现在随着设备位置的变化，这个基础也会发生变化。我们可以将这些基础视为时间的函数（x(t)、y(t)、z(t)），在数学术语中，它是一个移动坐标系。

因此，即使m不变，但由传感器返回的event.values是不同的，因为基础不同（稍后我们将谈论波动）。因为它给出了坐标，但我们不知道基础是什么，即相对于我们所知道的一些基础。

现在的问题是：能否找到相对于固定世界基础(w_1, w_2, w_3)，其中w_1指向东方，w_2指向磁北，w_3指向天空，a和m的坐标？答案是肯定的，前提是满足两个重要的假设。有了这两个假设就可以简单地计算基础(x, y, z)到基础(w_1, w_2, w_3)的变换矩阵R（也叫做Android中的旋转矩阵，只需进行几个叉乘即可）。然后，相对于基础(w_1, w_2, w_3)，向量v的坐标是通过将基础(x, y, z)的坐标与矩阵R相乘得到的。因此，相对于世界坐标系，m的坐标就是传感器TYPE_MAGNETIC_FIELD返回的event.values与旋转矩阵的乘积，a同理。在Android中，可以通过调用getRotationMatrix(float[] R, float[] I, float[] gravity, float[] geomagnetic)来获取旋转矩阵，在gravity参数中通常传入返回的加速度计值，在geomagnetic中传入磁场值。

重要的两个假设是：
1- 重力参数表示一个向量，位于w_3中，更具体地说，它是仅受重力影响的向量的负值。
因此，如果您未经过滤地传递加速度计值，则旋转矩阵会略微偏离。这就是为什么需要过滤加速度计，以便过滤器值大约只是负重力向量的原因。由于重力加速度是加速度计向量中的主导因素，通常低通滤波器就足够了。
2- 地磁参数表示一个向量，位于由w_2和w_3向量张成的平面中。即它位于北天平面上。因此，在（w_1，w_2，w_3）基础上，第一个坐标应该为0。因此，正如您上面所述，“X始终非常小”是正确的值，理想情况下应该为0。现在磁场值将会波动相当大。这有点可以预料到，就像普通指南针在您手中抖动时不会静止一样。此外，您可能会受到周围物体的干扰，在这种情况下，磁场值是不可预测的。我曾经在一个“石头”桌子旁测试过我的指南针应用程序，我的指南针偏差超过90度，只有使用真正的指南针，我才发现我的应用程序没有问题，“石头”桌子产生了真正强的磁场。
由于重力是主导因素，因此您可以过滤加速度计值，但是如果没有其他知识，如何过滤磁性值？如何确定周围物体是否存在干扰？

通过理解旋转矩阵，您可以更全面地了解设备的空间位置等信息。

- Hoan Nguyen

---

你可以检查设备是否具有TYPE_GRAVITY并使用它来代替TYPE_ACCELEROMETER，这样可以避免使用卡尔曼滤波器进行处理。 - Hoan Nguyen

是的，您可以从TYPE_GRAVITY传递值。我认为TYPE_GRAVITY只是带有某种过滤器的TYPE_ACCELEROMETER。我不知道谷歌如何实现它，因此我不知道它是否更准确。当设备加速时，即使进行过滤，传递给重力参数的值也不会准确地近似重力。因此，假设1并没有完全满足，输出将会偏差。getRotationMatrix使用重力参数作为天空坐标系。 - Hoan Nguyen

我认为只要Android API在9或更高版本，任何设备都会有TYPE_GRAVITY。在您的项目中，您可以将目标设置为最新版本，并检查手机版本以打开TYPE_GRAVITY。 - Hoan Nguyen

@HoanNguyen感谢您提供详细的答案。然而，当我从TYPE_ACCELEROMETER返回的加速度中移除重力影响时，getRotationMatrix无法计算R，因为“设备处于自由落体状态”？我在这里错过了什么？ - Shai

你应该传入重力而不是去除重力。因此，如果你使用的是TYPE_ACCELEROMETER，则要过滤掉加速度而不是重力。 - Hoan Nguyen

Answer 2

根据上述解释，执行以下操作。

private static final int TEST_GRAV = Sensor.TYPE_ACCELEROMETER;
private static final int TEST_MAG = Sensor.TYPE_MAGNETIC_FIELD;
private final float alpha = (float) 0.8;
private float gravity[] = new float[3];
private float magnetic[] = new float[3];

public void onSensorChanged(SensorEvent event) {
    Sensor sensor = event.sensor;
    if (sensor.getType() == TEST_GRAV) {
            // Isolate the force of gravity with the low-pass filter.
              gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
              gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
              gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];
    } else if (sensor.getType() == TEST_MAG) {

            magnetic[0] = event.values[0];
            magnetic[1] = event.values[1];
            magnetic[2] = event.values[2];

            float[] R = new float[9];
            float[] I = new float[9];
            SensorManager.getRotationMatrix(R, I, gravity, magnetic);
            float [] A_D = event.values.clone();
            float [] A_W = new float[3];
            A_W[0] = R[0] * A_D[0] + R[1] * A_D[1] + R[2] * A_D[2];
            A_W[1] = R[3] * A_D[0] + R[4] * A_D[1] + R[5] * A_D[2];
            A_W[2] = R[6] * A_D[0] + R[7] * A_D[1] + R[8] * A_D[2];

            Log.d("Field","\nX :"+A_W[0]+"\nY :"+A_W[1]+"\nZ :"+A_W[2]);

        }
    }

- vineetv2821993

Answer 3

Stochastically提供了一个我认为比这里发布的更好的答案。

https://dev59.com/zW_Xa4cB1Zd3GeqP6u0z#16418016

看起来SensorManager.getOrientation()无法正确地转换为世界坐标系。

正确的代码应该是：

SensorManager.getRotationMatrix(gravityCompassRotationMatrix, inclinationValues, gravityValues, magnitudeValues);
SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.matrix, worldAxisX, worldAxisY, adjustedRotationMatrix);
float sin = adjustedRotationMatrix[1] - adjustedRotationMatrix[4];
float cos = adjustedRotationMatrix[0] + adjustedRotationMatrix[5];
float m_azimuth_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);

- Sameer J