传感器融合视频看起来很大，但没有code： http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

The Sensor Fusion video looks great, but there's no code: http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

下面是我刚刚使用加速度计和罗盘code。我也用在3方向值卡尔曼滤波器，但这是太多了code在这里展示。最终，该工程确定，但结果是不是太紧张的或太laggy取决于我做的结果，以及如何低我做滤波因素。

Here is my code which just uses accelerometer and compass. I also use a Kalman filter on the 3 orientation values, but that's too much code to show here. Ultimately, this works ok, but the result is either too jittery or too laggy depending on what I do with the results and how low I make the filtering factors.

/** Just accelerometer and magnetic sensors */
public abstract class SensorsListener2
    implements
        SensorEventListener
{
    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float accelFilteringFactor = 0.1f;
    private static final float magFilteringFactor = 0.01f;

    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) {
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor);
                mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor);
                mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor);
                accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor);
                accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor);
                break;

            default:
                return;
        }




        if(mags != null && accels != null && isReady) {
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) {
                outR = rot;
            } else {
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            }

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) {
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
            } else {
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            }

            onOrientationChanged(azimuth,pitch,roll);
        }
    }




    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];



    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}

我试图找出如何添加陀螺仪数据，但我只是不这样做是正确的。在谷歌文档的http://developer.android.com/reference/android/hardware/SensorEvent.html显示了一些code，以获得陀螺仪数据的增量矩阵。这个想法似乎是，我摇下过滤器的加速度计和磁传感器，使他们真正稳定。这将跟踪长期方向。

I tried to figure out how to add gyroscope data, but I am just not doing it right. The google doc at http://developer.android.com/reference/android/hardware/SensorEvent.html shows some code to get a delta matrix from the gyroscope data. The idea seems to be that I'd crank down the filters for the accelerometer and magnetic sensors so that they were really stable. That would keep track of the long term orientation.

然后，我会保持从陀螺仪的最新ñ三角矩阵的历史。每当我有一个新的我会落最早的繁衍他们一起获得最后的矩阵，我就乘反对加速度计和磁传感器返回的稳定矩阵。

Then, I'd keep a history of the most recent N delta matrices from the gyroscope. Each time I got a new one I'd drop off the oldest one and multiply them all together to get a final matrix which I would multiply against the stable matrix returned by the accelerometer and magnetic sensors.

似乎这不工作。或者，至少，我实现了这是行不通的。其结果是远远不仅仅是加速更加风声鹤唳。增加陀螺仪的历史的规模实际上增加这让我觉得，我不是从陀螺仪计算正确的价值观抖动。

This doesn't seem to work. Or, at least, my implementation of it does not work. The result is far more jittery than just the accelerometer. Increasing the size of the gyroscope history actually increases the jitter which makes me think that I'm not calculating the right values from the gyroscope.

public abstract class SensorsListener3
    implements
        SensorEventListener
{
    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float kFilteringFactor = 0.001f;
    private static final float magKFilteringFactor = 0.001f;


    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) {
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor);
                mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor);
                mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor);
                accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor);
                accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor);
                break;

            case Sensor.TYPE_GYROSCOPE:
                gyroscopeSensorChanged(event);
                break;

            default:
                return;
        }




        if(mags != null && accels != null && isReady) {
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) {
                outR = rot;
            } else {
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            }

            if(gyroUpdateTime!=0) {
                matrixHistory.mult(matrixTmp,matrixResult);
                outR = matrixResult;
            }

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) {
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
            } else {
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            }

            onOrientationChanged(azimuth,pitch,roll);
        }
    }



    private void gyroscopeSensorChanged(SensorEvent event) {
        // This timestep's delta rotation to be multiplied by the current rotation
        // after computing it from the gyro sample data.
        if(gyroUpdateTime != 0) {
            final float dT = (event.timestamp - gyroUpdateTime) * NS2S;
            // Axis of the rotation sample, not normalized yet.
            float axisX = event.values[0];
            float axisY = event.values[1];
            float axisZ = event.values[2];

            // Calculate the angular speed of the sample
            float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);

            // Normalize the rotation vector if it's big enough to get the axis
            if(omegaMagnitude > EPSILON) {
                axisX /= omegaMagnitude;
                axisY /= omegaMagnitude;
                axisZ /= omegaMagnitude;
            }

            // Integrate around this axis with the angular speed by the timestep
            // in order to get a delta rotation from this sample over the timestep
            // We will convert this axis-angle representation of the delta rotation
            // into a quaternion before turning it into the rotation matrix.
            float thetaOverTwo = omegaMagnitude * dT / 2.0f;
            float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
            float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * axisX;
            deltaRotationVector[1] = sinThetaOverTwo * axisY;
            deltaRotationVector[2] = sinThetaOverTwo * axisZ;
            deltaRotationVector[3] = cosThetaOverTwo;
        }
        gyroUpdateTime = event.timestamp;
        SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
        // User code should concatenate the delta rotation we computed with the current rotation
        // in order to get the updated rotation.
        // rotationCurrent = rotationCurrent * deltaRotationMatrix;
        matrixHistory.add(deltaRotationMatrix);
    }



    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];

    // gyroscope stuff
    private long gyroUpdateTime = 0;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float[] deltaRotationMatrix = new float[9];
    private final float[] deltaRotationVector = new float[4];
//TODO: I have no idea how small this value should be.
    private static final float EPSILON = 0.000001f;
    private float[] matrixMult = new float[9];
    private MatrixHistory matrixHistory = new MatrixHistory(100);
    private float[] matrixTmp = new float[9];
    private float[] matrixResult = new float[9];


    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West 
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. 
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}


public class MatrixHistory
{
    public MatrixHistory(int size) {
        vals = new float[size][];
    }

    public void add(float[] val) {
        synchronized(vals) {
            vals[ix] = val;
            ix = (ix + 1) % vals.length;
            if(ix==0)
                full = true;
        }
    }

    public void mult(float[] tmp, float[] output) {
        synchronized(vals) {
            if(full) {
                for(int i=0; i<vals.length; ++i) {
                    if(i==0) {
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                    } else {
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    }
                }
            } else {
                if(ix==0)
                    return;
                for(int i=0; i<ix; ++i) {
                    if(i==0) {
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                    } else {
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    }
                }
            }
        }
    }


    private int ix = 0;
    private boolean full = false;
    private float[][] vals;
}

的code中的第二块中包含从code中的第一个块我的变化，这陀螺仪加进来。

The second block of code contains my changes from the first block of code which add the gyroscope to the mix.

具体地，加速的过滤系数变小（使值更加稳定）。该MatrixHistory类保持其计算在gyroscopeSensorChanged方法在过去100陀螺仪deltaRotationMatrix值的轨道。

Specifically, the filtering factor for accel is made smaller (making the value more stable). The MatrixHistory class keeps track of the last 100 gyroscope deltaRotationMatrix values which are calculated in the gyroscopeSensorChanged method.

我已经看到了关于这个主题的这个网站的许多问题。他们已经帮助我得到了这一点，但我不能找出下一步该怎么做。我真的希望传感器融合家伙刚刚发布了一些code的地方。很明显，他拥有了一切放在一起。

I've seen many questions on this site on this topic. They've helped me get to this point, but I cannot figure out what to do next. I really wish the Sensor Fusion guy had just posted some code somewhere. He obviously had it all put together.

推荐答案

好了，+1到你甚至不知道卡尔曼滤波器是什么。如果你愿意，我会编辑这篇文章，给你code我写了一对夫妇几年前做你想要做什么。

Well, +1 to you for even knowing what a Kalman filter is. If you'd like, I'll edit this post and give you the code I wrote a couple years ago to do what you're trying to do.

但首先，我会告诉你为什么，你不需要它。

But first, I'll tell you why you don't need it.

的Andr​​oid的传感器堆栈使用的传感器融合的，斯坦上面提到的现代实现。这只是意味着所有的可用数据 - 加速，MAG，陀螺 - 被收集在一起的一种算法，然后所有的输出回读出的Andr​​oid传感器的形式

Modern implementations of the Android sensor stack use Sensor Fusion, as Stan mentioned above. This just means that all of the available data -- accel, mag, gyro -- is collected together in one algorithm, and then all the outputs are read back out in the form of Android sensors.

在本质上，传感器融合是一个黑盒子。我已经研究过了Android实现的源$ C ​​$ C，它是用C写的一大卡尔曼滤波器++。有些pretty的良好code。在那里，远远超过任何过滤我写过更加复杂，而且可能更复杂，你在写什么。请记住，这些人这样做是为了谋生。

In essence, Sensor Fusion is a black box. I've looked into the source code of the Android implementation, and it's a big Kalman filter written in C++. Some pretty good code in there, and far more sophisticated than any filter I ever wrote, and probably more sophisticated that what you're writing. Remember, these guys are doing this for a living.

我也知道至少有一个芯片制造商都有自己的传感器融合的实现。该设备的制造，那么机器人和基于它们自己的标准供应商的实现之间进行选择。

I also know that at least one chipset manufacturer has their own sensor fusion implementation. The manufacturer of the device then chooses between the Android and the vendor implementation based on their own criteria.

最后，斯坦如上所述，Invensense公司都有自己的传感器融合实现芯片级。

Finally, as Stan mentioned above, Invensense has their own sensor fusion implementation at the chip level.

不管怎样，这一切归结为是，内置的传感器融合在你的设备很可能优于任何你或者我可以凑齐。所以，你的真正的想要做的是访问。

Anyway, what it all boils down to is that the built-in sensor fusion in your device is likely to be superior to anything you or I could cobble together. So what you really want to do is to access that.

在Android的，有物理和虚拟传感器。虚拟传感器是被从可用物理传感器中合成的那些。最有名的例子是TYPE_ORIENTATION这需要加速度计和磁力计和创建卷/俯仰/航向输出。 （顺便说一句，你不应该使用这种传感器;它有太多的限制）

In Android, there are both physical and virtual sensors. The virtual sensors are the ones that are synthesized from the available physical sensors. The best-known example is TYPE_ORIENTATION which takes accelerometer and magnetometer and creates roll/pitch/heading output. (By the way, you should not use this sensor; it has too many limitations.)

但重要的是，新版本的Andr​​oid包含这两个新的虚拟传感器：

But the important thing is that newer versions of Android contain these two new virtual sensors:

TYPE_GRAVITY是加速度计输入与运动的过滤出来的效果 TYPE_LINEAR_ACCELERATION与重力分量加速度计滤除。

TYPE_GRAVITY is the accelerometer input with the effect of motion filtered out TYPE_LINEAR_ACCELERATION is the accelerometer with the gravity component filtered out.

这些两个虚拟传感器通过加速计输入和陀螺仪输入的组合来合成。

These two virtual sensors are synthesized through a combination of accelerometer input and gyro input.

另一个值得注意的传感器是TYPE_ROTATION_VECTOR这与加速度计，磁力计，陀螺仪和合成的四元数。它重新presents所述设备的完整三维取向与线性加速度的滤出的效果。

Another notable sensor is TYPE_ROTATION_VECTOR which is a Quaternion synthesized from accelerometer, magnetometer, and gyro. It represents the full 3-d orientation of the device with the effects of linear acceleration filtered out.

然而，四元数都有点抽象对于大多数人来说，因为你很可能会用3-D转换工作，无论如何，你最好的办法是通过SensorManager.getRotationMatrix（）。

However, Quaternions are a little bit abstract for most people, and since you're likely working with 3-d transformations anyway, your best approach is to combine TYPE_GRAVITY and TYPE_MAGNETIC_FIELD via SensorManager.getRotationMatrix().

还有一点：如果你正在使用的设备运行Android的旧版本，你需要检测你没有收到TYPE_GRAVITY事件，并使用TYPE_ACCELEROMETER代替。从理论上讲，这将是使用自己的卡尔曼滤波的地方，但如果您的设备不具有传感器融合内置的，它可能不具备陀螺仪无论是。

One more point: if you're working with a device running an older version of Android, you need to detect that you're not receiving TYPE_GRAVITY events and use TYPE_ACCELEROMETER instead. Theoretically, this would be a place to use your own kalman filter, but if your device doesn't have sensor fusion built in, it probably doesn't have gyros either.

总之，这里的一些示例code，以显示我是怎么做的。

Anyway, here's some sample code to show how I do it.

  // Requires 1.5 or above

  class Foo extends Activity implements SensorEventListener {

    SensorManager sensorManager;
    float[] gData = new float[3];           // Gravity or accelerometer
    float[] mData = new float[3];           // Magnetometer
    float[] orientation = new float[3];
    float[] Rmat = new float[9];
    float[] R2 = new float[9];
    float[] Imat = new float[9];
    boolean haveGrav = false;
    boolean haveAccel = false;
    boolean haveMag = false;

    onCreate() {
        // Get the sensor manager from system services
        sensorManager =
          (SensorManager)getSystemService(Context.SENSOR_SERVICE);
    }

    onResume() {
        super.onResume();
        // Register our listeners
        Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME);
        sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME);
        sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME);
    }

    public void onSensorChanged(SensorEvent event) {
        float[] data;
        switch( event.sensor.getType() ) {
          case Sensor.TYPE_GRAVITY:
            gData[0] = event.values[0];
            gData[1] = event.values[1];
            gData[2] = event.values[2];
            haveGrav = true;
            break;
          case Sensor.TYPE_ACCELEROMETER:
            if (haveGrav) break;    // don't need it, we have better
            gData[0] = event.values[0];
            gData[1] = event.values[1];
            gData[2] = event.values[2];
            haveAccel = true;
            break;
          case Sensor.TYPE_MAGNETIC_FIELD:
            mData[0] = event.values[0];
            mData[1] = event.values[1];
            mData[2] = event.values[2];
            haveMag = true;
            break;
          default:
            return;
        }

        if ((haveGrav || haveAccel) && haveMag) {
            SensorManager.getRotationMatrix(Rmat, Imat, gData, mData);
            SensorManager.remapCoordinateSystem(Rmat,
                    SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2);
            // Orientation isn't as useful as a rotation matrix, but
            // we'll show it here anyway.
            SensorManager.getOrientation(R2, orientation);
            float incl = SensorManager.getInclination(Imat);
            Log.d(TAG, "mh: " + (int)(orientation[0]*DEG));
            Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG));
            Log.d(TAG, "roll: " + (int)(orientation[2]*DEG));
            Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG));
            Log.d(TAG, "inclination: " + (int)(incl*DEG));
        }
      }
    }

嗯，如果你碰巧有四元数库方便的，它可能只是简单接受TYPE_ROTATION_VECTOR和其转换成一个数组。

Hmmm; if you happen to have a Quaternion library handy, it's probably simpler just to receive TYPE_ROTATION_VECTOR and convert that to an array.