Преобразование значений магнитного поля X, Y, Z с устройства в глобальную систему отсчета

Когда вы используете датчик TYPE_MAGNETOMETER, вы получаете значения X, Y, Z напряженности магнитного поля в зависимости от ориентации устройства. Я хочу преобразовать эти значения в глобальную систему отсчета, поясняя: пользователь берет устройство, измеряет эти значения, затем поворачивает устройство на несколько градусов вокруг любой оси и получает ~ те же значения. Пожалуйста, найдите похожие вопросы ниже: Получение значений магнитного поля в глобальных координатах Как получить магнитный вектор поля, не зависящий от вращения устройства? В этом ответе описан пример решения (он предназначен для линейного ускорения, но я думаю, что это не имеет значения): https://stackoverflow.com/a/11614404/2152255 Я использовал его и получил 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) {
    }
}

Спасибо всем, кто прочитал этот пост и заранее поделился своими мыслями о проблеме.