Android 将设备的磁场X、Y、Z值转换为全局参考坐标系

当您使用_型磁强计传感器时，您可以获得与设备方向相关的磁场强度的X、Y、Z值。我想得到的是将这些值转换成全局参考系，说明：用户拿着设备，测量这些值，然后将设备绕任意轴旋转几度，得到相同的值。 请在下面查找类似问题： 在这个回答中，描述了示例解决方案（这是针对线性加速度的，但我认为这无关紧要）： 我用了它，得到了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) {
    }
}

导入android.app.Activity；
导入android.hardware.Sensor；
导入android.hardware.SensorEvent；
导入android.hardware.SensorEventListener；
导入android.hardware.SensorManager；
导入android.opengl.Matrix；
导入android.os.Bundle；
导入android.view.view；
导入android.widget.CheckBox；
导入android.widget.TextView；
导入com.test.statistics.filter.kalman.KalmanState；
导入com.example.R；
/**
*收集磁场统计数据的活动。
*/
公共类磁场统计CatheringActivity扩展活动实现SensorEventListener{
公共静态最终int KALMAN_状态_最大尺寸=80；
公共静态最终双重测量_噪声=5；
/**传感器管理器*/
私人传感器管理器；
/**磁强计规格*/
私有文本视图供应商；
私有文本视图解析；
私有文本视图最大范围；
/**磁场坐标测量*/
私有文本视图；
私有文本视图磁性文本视图；
私有文本视图；文本视图；
/**传感器*/
专用传感器mAccelerometer；
专用传感器；
私人浮动[]加速计值；
私人地磁价值；
/**旗帜*/
private boolean specDefined=false；
私有布尔值KalmanFileString=false；
/**费率*/
私人浮动利率=0.00001f；
私人最终int GTOContate=1000000；
/**Kalman-vars*/
私有KalmanState之前的Kalmanstatex；
私人卡尔曼斯州以前的卡尔曼斯州；
私人卡尔曼斯泰特（KalmanState）和前卡尔曼斯泰兹（Kalmanstatez）；
private int previousKalmanStateCounter=0；
@凌驾
创建时受保护的void（Bundle savedInstanceState）{
super.onCreate（savedInstanceState）；
setContentView（R.layout.main2）；
msSensorManager=（SensorManager）getSystemService（传感器服务）；
mAccelerometer=msSensorManager.getDefaultSensor（传感器类型\加速计）；
mGeomagnetic=mSensorManager.getDefaultSensor（传感器类型\u磁场）；
供应商=（TextView）findViewById（R.id.vendor）；
分辨率=（TextView）findViewById（R.id.resolution）；
maximumRange=（TextView）findViewById（R.id.maximumRange）；
magneticxtview=（TextView）findViewById（R.id.magneticX）；
磁性TextView=（TextView）findViewById（R.id.磁性）；
magneticZTextView=（TextView）findViewById（R.id.magneticZ）；
msSensorManager.registerListener（此、mAccelerometer、SensorManager.SENSOR\u DELAY\u最快）；
msSensorManager.registerListener（此、MGEOMagnitic、SensorManager.SENSOR\u DELAY\u最快）；
}
/**
*刷新统计信息。
*
*@param视图-刷新按钮视图。
*/
单击RefreshMagneticButton（视图）时出现公共无效{
resetKalmanFilter（）；
}
/**
*打开/关闭卡尔曼滤波
*
*@param view-Klaman文件串切换器（复选框）
*/
单击Kalman FilteringCheckBox（视图）时出现公共无效{
复选框kalmanFiltering=（复选框）视图；
this.kalmanFileString=kalmanFiltering.isChecked（）；
}
@凌驾
传感器更改时的公共无效（传感器事件传感器事件）{
if（sensorEvent.accuracy==传感器管理器。传感器状态不可靠）{
返回；
}
已同步（此）{
开关（sensorEvent.sensor.getType（））{
外壳传感器.U型加速计：
accelerometerValues=sensorEvent.values.clone（）；
打破
外壳传感器。类型\u磁场：
如果（！specDefined）{
vendor.setText（“vendor:+sensorEvent.sensor.getVendor（）+”“+sensorEvent.sensor.getName（））；
float resolutionValue=sensorEvent.sensor.getResolution（）*NanoTograte；
resolution.setText（“resolution:+resolutionValue”）；
float maximumRangeValue=sensorEvent.sensor.getMaximumRange（）*NanoTograte；
maximumRange.setText（“最大范围：”+maximumRangeValue）；
}
geomagneticValues=sensorEvent.values.clone（）；
打破
}
if（加速度计值！=null和地磁值！=null）{
浮动[]Rs=新浮动[16]；
浮动[]I=新浮动[16]；
if（SensorManager.getRotationMatrix（Rs、I、加速度计值、地磁值））{
浮动[]RsInv=新浮动[16]；
逆矩阵M（RsInv，0，Rs，0）；
float resultVec[]=新的float[4]；
浮动[]地磁值调整=新浮动[4]；
地磁值调整[0]=地磁值[0]；
地磁值调整[1]=地磁值[1]；
地磁值调整[2]=地磁值[2]；
地磁值调整[3]=0；
矩阵多重
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]);

        }
    }

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