使用Android陀螺仪代替加速度计 . 我发现很多点点滴滴，但没有完整的代码-Java 学习之路

Sensor Fusion视频看起来很棒，但没有代码：http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

这是我的代码，它只使用加速度计和指南针 . 我还在3个方向值上使用卡尔曼滤波器，但这里显示的代码太多了 . 最终，这可行，但结果是太过于紧张或太迟，这取决于我对结果的处理方式以及制作滤波因子的程度 .

/** 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的google doc显示了一些代码，用于从陀螺仪数据中获取delta矩阵 . 我的想法似乎是我要为加速度计和磁传感器调低滤波器，使它们非常稳定 . 这将跟踪长期方向 .

然后，我将保留陀螺仪中最新N delta矩阵的历史记录 . 每次我得到一个新的，我都会丢掉最旧的一个并将它们相乘以得到一个最终矩阵，我将乘以加速度计和磁传感器返回的稳定矩阵 .

这似乎不起作用 . 或者，至少，我对它的实现不起作用 . 结果比加速度计更加紧张 . 增加陀螺仪历史的大小实际上增加了抖动，这让我觉得我没有从陀螺仪中计算出正确的值 .

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

第二个代码块包含我对第一个代码块的更改，该代码块将陀螺仪添加到混合中 .

具体来说，缩小了加速度的过滤因子（使值更稳定） . MatrixHistory类跟踪最后100个陀螺仪deltaRotationMatrix值，这些值是在gyroscopeSensorChanged方法中计算的 .

我在这个网站上看到过很多关于这个主题的问题 . 他们帮助我达到了这一点，但我无法弄清楚下一步该做什么 . 我真的希望Sensor Fusion的人刚刚在某处发布了一些代码 . 他显然把这一切都放在了一起 .

2 回答

5
好吧，1甚至知道卡尔曼滤波器是什么 . 如果你愿意，我会编辑这篇文章并给你几年前我写的代码来做你想做的事情 .

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

如上所述，Android传感器堆栈的现代实现使用Sensor Fusion . 这只意味着所有可用的数据 - accel，mag，gyro - 在一个算法中收集在一起，然后以Android传感器的形式读回所有输出 .

编辑：我只是偶然发现了关于这个主题的精湛Google Tech Talk：Sensor Fusion on Android Devices: A Revolution in Motion Processing . 如果您对该主题感兴趣，那么值得花45分钟观看它 .

从本质上讲，传感器融合是一个黑盒子 . 我查看了Android实现的源代码，它是一个用C编写的大型卡尔曼滤波器 . 那里有一些相当不错的代码，比我写过的任何过滤器都复杂得多，而且可能比你写的更复杂 . 请记住，这些家伙正以此为生 .

我也知道至少有一家芯片组制造商有自己的传感器融合实现 . 然后，设备制造商根据自己的标准在Android和供应商实施之间进行选择 .

最后，正如Stan所述，Invensense在芯片级别拥有自己的传感器融合实现 .

无论如何，这一切归结为你设备中的内置传感器融合可能优于你或我可以拼凑的任何东西 . 所以你真正想做的就是访问它 .

在Android中，有物理和虚拟传感器 . 虚拟传感器是从可用的物理传感器合成的传感器 . 最着名的例子是TYPE_ORIENTATION，它采用加速度计和磁力计，并产生滚动/俯仰/航向输出 . （顺便说一句，你不应该使用这个传感器;它有太多的限制 . ）

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

TYPE_GRAVITY是加速度计输入，具有滤除的运动效果TYPE_LINEAR_ACCELERATION是加速度计，其中重力分量被滤除 .

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

另一个值得注意的传感器是TYPE_ROTATION_VECTOR，它是由加速度计，磁力计和陀螺仪合成的四元数 . 它代表完整的三维方向滤除了线性加速度影响的装置 .

但是，对于大多数人来说，四元数有点抽象，因为无论如何你都可能使用三维变换，最好的方法是通过SensorManager.getRotationMatrix（）组合TYPE_GRAVITY和TYPE_MAGNETIC_FIELD .

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

无论如何，这里有一些示例代码来说明我是如何做到的 .
```
// 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));
        }
      }
    }
```
嗯;如果您碰巧有一个Quaternion库，那么接收TYPE_ROTATION_VECTOR并将其转换为数组可能更简单 .
回复于 2024-05-04T19:38:18+08:00
50

关于在哪里找到完整代码的问题，这里是Android软件bean的默认实现：https://android.googlesource.com/platform/frameworks/base/+/jb-release/services/sensorservice/首先检查fusion.cpp / h . 它使用Modified Rodrigues参数（接近欧拉角）而不是四元数 . 除了定向之外，卡尔曼滤波器估计陀螺仪漂移 . 对于测量更新，它使用磁力计，有点不正确，加速度（特定力） .

要使用代码，您应该是向导或了解INS和KF的基础知识 . 必须微调许多参数才能使滤波器工作 . 正如爱德华充分说的那样，这些家伙正在为生活做这件事 .

至少在google的galaxy nexus中，这个默认实现未被使用，并被Invense的专有系统覆盖 .

回复于 2024-05-04T19:38:18+08:00

使用Android陀螺仪代替加速度计 . 我发现很多点点滴滴，但没有完整的代码

2 回答

相关问题