如何用罗盘读数和陀螺仪读数获得手机的方位角？

Question

我希望通过以下方法获取手机的当前定位：

1. 首先通过getRotationMatrix()和getOrientation()获取初始方向（方位角）。
2. 随着时间的推移将陀螺仪读数的积分添加到其中以获得当前方向。

电话定位：

手机的x-y平面与地平面固定。即处于“发短信行走”的方向。

“getOrientation()”回归：

Android API可让我轻松地从getOrientation()获取方向，即方位角，俯仰，滚动。

请注意，此方法始终会在以下范围内返回其值： [0, -PI] 和 {{1} }

我的问题：

由于陀螺仪读数的整合（由[o, PI]表示）可能非常大，所以当我dR时，CurrentOrientation += dR可能会超过 CurrentOrientation 和 [0, -PI] 范围。

需要进行哪些操作才能让我始终在[o, PI]和[0, -PI]范围内获得当前方向？

我在Python中尝试了以下内容，但我非常怀疑它的正确性。

[o, PI]

---

说明

这并不是那么简单，因为它涉及以下麻烦制造者：

1. 方向传感器读数从rotation = scipy.integrate.trapz(gyroSeries, timeSeries) # integration if (headingDirection - rotation) < -np.pi: headingDirection += 2 * np.pi elif (headingDirection - rotation) > np.pi: headingDirection -= 2 * np.pi # Complementary Filter headingDirection = ALPHA * (headingDirection - rotation) + (1 - ALPHA) * np.mean(azimuth[np.array(stepNo.tolist()) == i]) if headingDirection < -np.pi: headingDirection += 2 * np.pi elif headingDirection > np.pi: headingDirection -= 2 * np.pi 变为0，然后 DIRECTLY JUMPS 变为-PI，并逐渐通过{+PI返回0 {1}}。
2. gyrocope阅读的整合也会带来一些麻烦。我应该将+PI/2添加到方向还是减去dR。

在给出确认答案之前，请先参阅Android Documentations。

估计的答案无济于事。

Answer 1

  

方位传感器实际上从真实的磁力计和加速度计中获取读数。

我想也许这就是混乱的根源。文档中说明了哪些内容？更重要的是，某处的文档是否明确说明陀螺仪读数被忽略了？据我所知，本视频中描述的方法已实施：

Sensor Fusion on Android Devices: A Revolution in Motion Processing

此方法使用陀螺仪并整合其读数。这几乎使问题的其余部分没有实际意义;尽管如此，我会尽力回答。

---

方向传感器已经为您整合了陀螺仪读数，这就是您获得方向的方法。我不明白为什么你自己这样做。

你没有正确地整合陀螺仪读数，它比CurrentOrientation += dR更复杂（这是不正确的）。如果您需要集成陀螺仪读数（我不明白为什么，SensorManager已经为您做了），请阅读Direction Cosine Matrix IMU: Theory如何正确完成（公式17）。

不要尝试与欧拉角集成（又名方位角，俯仰，滚动），没有什么好处会出来。

请在计算中使用四元数或旋转矩阵而不是欧拉角。如果使用旋转矩阵，可以始终将它们转换为欧拉角，请参阅

Computing Euler angles from a rotation matrix by Gregory G. Slabaugh

（对于四元数也是如此。）（在非退化情况下）有两种表示旋转的方法，即你将得到两个欧拉角。选择你所需范围内的那个。（在gimbal lock的情况下，有无限多的欧拉角，见上面的PDF）。只是承诺在旋转矩阵到欧拉角度转换后，你不会再在计算中再次使用欧拉角。

目前还不清楚你在使用互补过滤器做了什么。你可以根据Direction Cosine Matrix IMU: Theory手稿实现一个非常好的传感器融合，这基本上是一个教程。这样做并非易事，但我认为你不会找到比这份手稿更好，更易理解的教程。

当我根据这份手稿实现传感器融合时，我必须发现自己的一件事是所谓的integral windup可能会发生。我通过限制TotalCorrection（第27页）来处理它。如果你实现这种传感器融合，你会明白我在说什么。

---

---

更新：在接受答案后，我会回答您在评论中发布的问题。

  

我认为指南针通过使用重力和磁场给我当前的方向，对吧？陀螺仪是否在指南针中使用？

是的，如果手机静止至少半秒，您可以通过仅使用重力和指南针来获得良好的方向估计。以下是如何操作：Can anyone tell me whether gravity sensor is as a tilt sensor to improve heading accuracy?

不，指南针中没有使用陀螺仪。

  

请您解释为什么我所做的整合是错误的？据我所知，如果我的手机音高指向上，则欧拉角失败。但是我的整合还有其他问题吗？

有两个不相关的东西：（i）整合应该以不同的方式进行，（ii）由于万向节锁定，欧拉角是麻烦的。我再说一遍，这两者是无关的。

至于集成：这里有一个简单的例子，说明如何看集成有什么问题。设x和y为房间中水平面的轴。拿到手机。将手机围绕x轴（房间的）旋转45度，然后围绕y轴（房间的）旋转45度。然后，从头开始重复这些步骤，但现在先绕y轴旋转，然后绕x轴旋转。手机的方向完全不同。如果您根据CurrentOrientation += dR进行集成，您将看到没有区别！如果你想正确地进行整合，请阅读上面链接的方向余弦矩阵IMU：理论手稿。

至于欧拉角：他们搞砸了应用程序的稳定性，这足以让我不用它来进行3D任意旋转。

我仍然不明白你为什么要自己尝试，为什么你不想使用平台提供的方向估计。机会是，你不能做得更好。

Answer 2

我认为你应该避免折旧的“方向传感器”，并使用传感器融合方法，如getRotationVector，getRotationMatrix，它们已经实现了已经使用陀螺仪数据的Invensense的融合算法。

如果您想要一种称为平衡滤波器的简单传感器融合算法 （可以使用http://www.filedump.net/dumped/filter1285099462.pdf）。方法如

  

<强> http://postimg.org/image/9cu9dwn8z/

这将陀螺仪集成到角度，然后通过高通滤波器去除结果 漂移，并将其添加到平滑的加速度计和指南针结果。集成的高通滤波数据和加速度计/指南针数据以这两个部分相加的方式添加 为了使输出是一个有意义的单位的准确估计。 对于平衡滤波器，可以调整时间常数以调整响应。时间越短 恒定，响应越好，但允许通过的加速度噪声越大。

要了解它是如何工作的，想象一下你有最新的陀螺数据点（以rad / s为单位）存储在陀螺仪中， 来自加速度计的最新角度测量值存储在angle_acc中，而dt是时间 最后的陀螺仪数据到现在为止。然后使用

计算新的角度

angle = b *（angle + gyro * dt）+（1 - b）*（angle_acc）;

例如，您可以尝试b = 0.98。您可能还希望使用快速陀螺仪测量时间dt，以便陀螺仪在下一次测量之前不会漂移超过几度。平衡滤波器非常有用且易于实现，但不是理想的传感器融合方法。 Invensense的方法涉及一些聪明的算法，可能还有某种形式的卡尔曼滤波器。

来源：专业Android传感器编程，Adam Stroud。

Answer 3

如果由于磁干扰导致方位角值不准确，那么就我所知，没有什么可以消除它的。要获得稳定的方位角读数，如果TYPE_GRAVITY不可用，则需要过滤加速度计值。如果TYPE_GRAVITY不可用，那么我很确定该设备没有陀螺仪，因此您可以使用的唯一滤波器是低通滤波器。以下代码是使用TYPE_GRAVITY和TYPE_MAGNETIC_FIELD的稳定指南针的实现。

public class Compass  implements SensorEventListener
{
    public static final float TWENTY_FIVE_DEGREE_IN_RADIAN = 0.436332313f;
    public static final float ONE_FIFTY_FIVE_DEGREE_IN_RADIAN = 2.7052603f;

    private SensorManager mSensorManager;
    private float[] mGravity;
    private float[] mMagnetic;
    // If the device is flat mOrientation[0] = azimuth, mOrientation[1] = pitch
    // and mOrientation[2] = roll, otherwise mOrientation[0] is equal to Float.NAN
    private float[] mOrientation = new float[3];
    private LinkedList<Float> mCompassHist = new LinkedList<Float>();
    private float[] mCompassHistSum = new float[]{0.0f, 0.0f};
    private int mHistoryMaxLength;

    public Compass(Context context)
    {
         mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
         // Adjust the history length to fit your need, the faster the sensor rate
         // the larger value is needed for stable result.
         mHistoryMaxLength = 20;
    }

    public void registerListener(int sensorRate)
    {
        Sensor magneticSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        if (magneticSensor != null)
        {
            mSensorManager.registerListener(this, magneticSensor, sensorRate);
        }
        Sensor gravitySensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        if (gravitySensor != null)
        {
            mSensorManager.registerListener(this, gravitySensor, sensorRate);
        }
    }

    public void unregisterListener()
    {
         mSensorManager.unregisterListener(this);
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy)
    {

    }

    @Override
    public void onSensorChanged(SensorEvent event)
    {
        if (event.sensor.getType() == Sensor.TYPE_GRAVITY)
        {
            mGravity = event.values.clone();
        }
        else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        {
            mMagnetic = event.values.clone();
        }
        if (!(mGravity == null || mMagnetic == null))
        {
            mOrientation = getOrientation();
        } 
    }

    private void getOrientation()
    {
        float[] rotMatrix = new float[9];
        if (SensorManager.getRotationMatrix(rotMatrix, null, 
            mGravity, mMagnetic))
        {
            float inclination = (float) Math.acos(rotMatrix[8]);
            // device is flat
            if (inclination < TWENTY_FIVE_DEGREE_IN_RADIAN 
                || inclination > ONE_FIFTY_FIVE_DEGREE_IN_RADIAN)
            {
                float[] orientation = sensorManager.getOrientation(rotMatrix, mOrientation);
                mCompassHist.add(orientation[0]);
                mOrientation[0] = averageAngle();
            }
            else
            {
                mOrientation[0] = Float.NAN;
                clearCompassHist();
            }
        }
    }

    private void clearCompassHist()
    {
        mCompassHistSum[0] = 0;
        mCompassHistSum[1] = 0;
        mCompassHist.clear();
    }

    public float averageAngle()
    {
        int totalTerms = mCompassHist.size();
        if (totalTerms > mHistoryMaxLength)
        {
            float firstTerm = mCompassHist.removeFirst();
            mCompassHistSum[0] -= Math.sin(firstTerm);
            mCompassHistSum[1] -= Math.cos(firstTerm);
            totalTerms -= 1;
        }
        float lastTerm = mCompassHist.getLast();
        mCompassHistSum[0] += Math.sin(lastTerm);
        mCompassHistSum[1] += Math.cos(lastTerm);
        float angle = (float) Math.atan2(mCompassHistSum[0] / totalTerms, mCompassHistSum[1] / totalTerms);

        return angle;
    }
}

在你的活动中，在onCreate中实例化一个Compass对象，onResume中的registerListener和onPause中的unregisterListener

private Compass mCompass;

@Override
protected void onCreate(Bundle savedInstanceState)
{
    super.onCreate(savedInstanceState);

    mCompass = new Compass(this);
}

@Override
protected void onPause()
{
    super.onPause();

    mCompass.unregisterListener();
}

@Override
protected void onResume()
{
    super.onResume();

    mCompass.registerListener(SensorManager.SENSOR_DELAY_NORMAL);
}

Answer 4

最好让android的Orientation检测实现它。现在，您得到的值是-PI到PI，您可以将它们转换为度数（0-360）。一些相关部分：

保存要处理的数据：

@Override
public void onSensorChanged(SensorEvent sensorEvent) {
    switch (sensorEvent.sensor.getType()) {
        case Sensor.TYPE_ACCELEROMETER:
            mAccValues[0] = sensorEvent.values[0];
            mAccValues[1] = sensorEvent.values[1];
            mAccValues[2] = sensorEvent.values[2];
            break;
        case Sensor.TYPE_MAGNETIC_FIELD:
            mMagValues[0] = sensorEvent.values[0];
            mMagValues[1] = sensorEvent.values[1];
            mMagValues[2] = sensorEvent.values[2];
            break;
    }

}

计算横滚，俯仰和偏航（方位角）。mR和mI是arrys来保持旋转和倾斜矩阵，mO是一个临时数组。数组mResults的最大值为度数：

    private void updateData() {
    SensorManager.getRotationMatrix(mR, mI, mAccValues, mMagValues);

    /**
     * arg 2: what world(according to app) axis , device's x axis aligns with
     * arg 3: what world(according to app) axis , device's y axis aligns with
     * world x = app's x = app's east
     * world y = app's y = app's north
     * device x = device's left side = device's east
     * device y = device's top side  = device's north
     */

    switch (mDispRotation) {
        case Surface.ROTATION_90:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, mR2);
            break;
        case Surface.ROTATION_270:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_Y, SensorManager.AXIS_X, mR2);
            break;
        case Surface.ROTATION_180:
            SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_X, SensorManager.AXIS_MINUS_Y, mR2);
            break;
        case Surface.ROTATION_0:
        default:
            mR2 = mR;
    }

    SensorManager.getOrientation(mR2, mO);


    //--upside down when abs roll > 90--
    if (Math.abs(mO[2]) > PI_BY_TWO) {
        //--fix, azimuth always to true north, even when device upside down, realistic --
        mO[0] = -mO[0];

        //--fix, roll never upside down, even when device upside down, unrealistic --
        //mO[2] = mO[2] > 0 ? PI - mO[2] : - (PI - Math.abs(mO[2]));

        //--fix, pitch comes from opposite , when device goes upside down, realistic --
        mO[1] = -mO[1];
    }

    CircleUtils.convertRadToDegrees(mO, mOut);
    CircleUtils.normalize(mOut);

    //--write--
    mResults[0] = mOut[0];
    mResults[1] = mOut[1];
    mResults[2] = mOut[2];
}