Convert magnetic field X, Y, Z values from device into global reference frame

Yahor picture Yahor · Mar 9, 2013 · Viewed 27.6k times · Source

When you use TYPE_MAGNETOMETER sensor, you get X, Y, Z values of magnetic field strength in relation to the device orientation. What I want to get is to convert these values into global reference frame, clarifying: user takes the device, measure these values, than rotate the device for some degrees around any axis and gets ~the same values. Please, find similar questions below: Getting magnetic field values in global coordinates How can I get the magnetic field vector, independent of the device rotation? In this answer sample solution is described (it is for linear acceleration, but I think it doesn't matter): https://stackoverflow.com/a/11614404/2152255 I used it and I got 3 values, X is always very small (don't think that it is correct), Y and Z are OK, but they still changed a bit when I rotate the device. How could it be adjusted? And could it be solved all? I use simple Kalman filter to approximate measurement values, because w/o it I get quiet different values even if the device is not moving/rotating at all. Please, find my code below:

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) {
    }
}

Thanks everybody who read this post and who post some thoughts about the problem in advance.

Answer

Hoan Nguyen picture Hoan Nguyen · Mar 10, 2013

In my comment on the checked answer on the link you provided above, I referred to my simple answer at calculate acceleration in reference to true north

Let me answer here again with more clarification. The answer is the product of the rotation matrix and the magnetic field values. If you read further on the "X is always very small" is the correct value.

The accelerometer and magnetic field sensors measure the acceleration of the device and the magnetic field of the earth at the device location respectively. They are vectors in 3 dimentional space, let call them a and m respectively.
If you stand still and rotate your device, theoretically m does not change assuming there are no magnetic interference from surrounding objects (actually m should change little, if you move around since the magnetic field of the earth should change little in a short distance). But a does change, even though it should not be drastic in most situation.

Now a vector v in 3 dimensional space can be represented by a 3-tuples (v_1, v_2, v_3) with respect to some basis (e_1, e_2, e_3), i.e v = v_1 e_1 + v_2 e_2 + v_3 e_3. (v_1, v_2, v_3) are called the coordinates of v with respect to the basis (e_1, e_2, e_3).

In Android devices, the basis is (x, y, z) where, for most phone, x is along the shorter side and pointing right, y is along the longer side and pointing up and z is perpendicular to the screen and pointing out.
Now this basis changes as the position of the device changes. One can think these bases as a function of time (x(t), y(t), z(t)), in mathematics term it is a moving coordinate system.

Thus even though m does not change, but the event.values returns by the sensors are different because the basis is different (I will talk about fluctuation later). As is, the event.values are useless because it gives us the coordinates but we do not know what the basis is, i.e with respect to some basis we know.

Now the question is: is it possible to find the coordinates of a and m with respect to the fixed world basis (w_1, w_2, w_3) where w_1 points toward East, w_2 points toward magnetic North and w_3 points up toward the sky?

The answer is yes provided 2 important assumptions are satisfied.
With these 2 assumptions it is simple to calculate (just a few cross products) the change of basis matrix R from the basis (x, y, z) to the basis (w_1, w_2, w_3), which in Android is called the Rotation matrix. Then the coordinates of a vector v with respect to the basis (w_1, w_2, w_3) is obtained by multiply R with the coordinates of v with respect to (x, y, z). Thus the coordinates of m with respect to the world coordinates system is just the product of the rotation matrix and the event.values returned by the TYPE_MAGNETIC_FIELD sensor and similarly for a.

In android the rotation matrix is obtained by calling getRotationMatrix (float[] R, float[] I, float[] gravity, float[] geomagnetic) and we normally pass in the returned accelerometer values for the gravity parameter and the magnetic field values for the geomagnetic.

The 2 important assumptions are:
1- The gravity parameter represents a vector lying in w_3, more particular it is the minus of the vector influenced by gravity alone.
Thus if you pass in the accelerometer values without filtering, the rotation matrix will be slightly off. That is why you need to filter the accelerometer so that the filter values are approximately just the minus gravity vector. Since the gravitational acceleration is the dominant factor in the accelerometer vector, normally low pass filter is sufficient.
2- The geomagnetic parameter represents a vector lying in the plane spanned by the w_2 and the w_3 vectors. That is it lies in the North-Sky plane. Thus in term of the (w_1, w_2, w_3) basis, the first coordinate should be 0. Therefore, the "X is always very small" as you stated it above is the correct value, ideally it should be 0. Now the magnetic field values will fluctuate quite a bit. This is kind of expected, just as a regular compass needle will not stand still if you keep it in your hand and your hand shakes a little. Also, you may get interference from objects surround you and in this case the magnetic field values are unpredictable. I once test my compass app sitting near a "stone" table and my compass was off by more than 90 degrees, only by using a real compass that I found out that there is nothing wrong with my app and the "stone" table produces a real strong magnetic field.
With gravity as a dominant factor you can filter accelerometer values, but without any other knowledge, how do you fitler magnetic values? How do you know if there is or isn't any interference from surrounding objects?

You can do a lot more like complete knowledge of your device spatial position etc with the understanding of rotation matrix.