Android罗盘，可补偿倾斜和俯仰

我正在尝试在我的Android手机（Nexus4）上制作一个应用程序，它将用于船模。我已经添加了低通滤波器来滤除传感器中的gitter

然而，指南针只有在手机平躺时才稳定。如果我把它向上倾斜（比如翻开一页书），那么指南针的航向就会偏离——高达50*

我用Sensor.TYPE\u磁场和Sensor.TYPE\u重力和Sensor.TYPE\u加速计试过，效果是一样的

我已经使用了上面提到的解决方案，以及许多其他地方。我的数学不是很好，但这一定是一个常见的问题，我发现它令人沮丧的是，没有一个API来处理它

我已经研究这个问题3天了，仍然没有找到任何解决方案，但是当我使用手机时，无论手机倾斜多大，他们的手机都会保持稳定。所以我知道这一定是可能的

我想做的就是创建一个指南针，如果手机指向北方，那么指南针的读数将是北方，当手机移动到任何其他轴（滚动或俯仰）时，指南针不会跳跃

在我不得不放弃我的项目之前，有谁能帮我一下吗

谢谢，

---

亚当，你的问题可能是。如果你想一想，当手机直立时，俯仰角是正负90度，那么方位角和滚动角是一样的。如果你仔细研究数学，你会发现在这种情况下，方位角+横滚或方位角横滚都有很好的定义，但它们不是单独定义的。因此，当螺距接近正负90度时，读数变得不稳定。有些人选择重新映射坐标系，汤姆尝试绕过这个问题，例如，这样可能对你有用。

好的，我想我解决了它

我没有使用Sensor.TYPE_加速计（或TYPE_重力）和Sensor.TYPE_磁场，而是使用Sensor.TYPE_旋转向量：

float[] roationV = new float[16];
SensorManager.getRotationMatrixFromVector(roationV, rotationVector);

float[] orientationValuesV = new float[3];
SensorManager.getOrientation(roationV, orientationValuesV);

这返回了一个稳定的方位角，无论手机的滚动或俯仰

如果你看一下，就在表1下面，它说旋转传感器是罗盘、增强现实等的理想选择

---

当你知道如何。。。。然而，随着时间的推移，我还没有对这个问题进行测试，看看是否引入了错误。

我已经思考这个问题好几个星期了，因为

作为一名数学家，我对我在别处看到的任何答案都不满意；而且

对于我正在开发的应用程序，我需要一个好的答案。

所以在过去的几天里，我想出了自己的方法来计算方位角值，以便在指南针中使用

我已经把我正在使用的数学，我已经粘贴了我在下面使用的代码。该代码根据原始

TYPE\u重力

和

TYPE\u磁场

传感器数据计算方位角和俯仰角，无需调用任何API，例如

SensorManager.getRotationMatrix（…）

或

SensorManager.getOrientation（…）

。如果输入结果有点不稳定，代码可能会得到改进，例如使用低通滤波器。请注意，代码通过精度更改（传感器传感器，int精度）方法记录传感器的精度，因此，如果方位角不稳定，另一件要检查的事情是每个传感器的精度。在任何情况下，由于所有计算在本代码中明确可见，如果存在不稳定性问题（当传感器精度合理时），则可以通过查看输入或方向向量中的不稳定性来解决这些问题

m_NormGravityVector[]

，

m_NormEastVector[]

或

m_NormNorthVector[]

我对任何人对我的反馈都很感兴趣。我发现，在我自己的应用程序中，只要设备是平面朝上、垂直或介于两者之间的某个位置，它就会像梦一样工作。然而，正如我在math.stackexchange.com的文章中提到的，当设备接近翻转时，会出现一些问题。在这种情况下，人们需要仔细定义自己想要的行为

    import android.app.Activity;
    import android.hardware.Sensor;
    import android.hardware.SensorEvent;
    import android.hardware.SensorEventListener;
    import android.hardware.SensorManager;
    import android.view.Surface;

    public static class OrientationSensor implements  SensorEventListener {

    public final static int SENSOR_UNAVAILABLE = -1;

    // references to other objects
    SensorManager m_sm;
    SensorEventListener m_parent;   // non-null if this class should call its parent after onSensorChanged(...) and onAccuracyChanged(...) notifications
    Activity m_activity;            // current activity for call to getWindowManager().getDefaultDisplay().getRotation()

    // raw inputs from Android sensors
    float m_Norm_Gravity;           // length of raw gravity vector received in onSensorChanged(...).  NB: should be about 10
    float[] m_NormGravityVector;    // Normalised gravity vector, (i.e. length of this vector is 1), which points straight up into space
    float m_Norm_MagField;          // length of raw magnetic field vector received in onSensorChanged(...). 
    float[] m_NormMagFieldValues;   // Normalised magnetic field vector, (i.e. length of this vector is 1)

    // accuracy specifications. SENSOR_UNAVAILABLE if unknown, otherwise SensorManager.SENSOR_STATUS_UNRELIABLE, SENSOR_STATUS_ACCURACY_LOW, SENSOR_STATUS_ACCURACY_MEDIUM or SENSOR_STATUS_ACCURACY_HIGH
    int m_GravityAccuracy;          // accuracy of gravity sensor
    int m_MagneticFieldAccuracy;    // accuracy of magnetic field sensor

    // values calculated once gravity and magnetic field vectors are available
    float[] m_NormEastVector;       // normalised cross product of raw gravity vector with magnetic field values, points east
    float[] m_NormNorthVector;      // Normalised vector pointing to magnetic north
    boolean m_OrientationOK;        // set true if m_azimuth_radians and m_pitch_radians have successfully been calculated following a call to onSensorChanged(...)
    float m_azimuth_radians;        // angle of the device from magnetic north
    float m_pitch_radians;          // tilt angle of the device from the horizontal.  m_pitch_radians = 0 if the device if flat, m_pitch_radians = Math.PI/2 means the device is upright.
    float m_pitch_axis_radians;     // angle which defines the axis for the rotation m_pitch_radians

    public OrientationSensor(SensorManager sm, SensorEventListener parent) {
        m_sm = sm;
        m_parent = parent;
        m_activity = null;
        m_NormGravityVector = m_NormMagFieldValues = null;
        m_NormEastVector = new float[3];
        m_NormNorthVector = new float[3];
        m_OrientationOK = false;
    }

    public int Register(Activity activity, int sensorSpeed) {
        m_activity = activity;  // current activity required for call to getWindowManager().getDefaultDisplay().getRotation()
        m_NormGravityVector = new float[3];
        m_NormMagFieldValues = new float[3];
        m_OrientationOK = false;
        int count = 0;
        Sensor SensorGravity = m_sm.getDefaultSensor(Sensor.TYPE_GRAVITY);
        if (SensorGravity != null) {
            m_sm.registerListener(this, SensorGravity, sensorSpeed);
            m_GravityAccuracy = SensorManager.SENSOR_STATUS_ACCURACY_HIGH;
            count++;
        } else {
            m_GravityAccuracy = SENSOR_UNAVAILABLE;
        }
        Sensor SensorMagField = m_sm.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        if (SensorMagField != null) {
            m_sm.registerListener(this, SensorMagField, sensorSpeed);
            m_MagneticFieldAccuracy = SensorManager.SENSOR_STATUS_ACCURACY_HIGH;     
            count++;
        } else {
            m_MagneticFieldAccuracy = SENSOR_UNAVAILABLE;
        }
        return count;
    }

    public void Unregister() {
        m_activity = null;
        m_NormGravityVector = m_NormMagFieldValues = null;
        m_OrientationOK = false;
        m_sm.unregisterListener(this);
    }

    @Override
    public void onSensorChanged(SensorEvent evnt) {
        int SensorType = evnt.sensor.getType();
        switch(SensorType) {
            case Sensor.TYPE_GRAVITY:
                if (m_NormGravityVector == null) m_NormGravityVector = new float[3];
                System.arraycopy(evnt.values, 0, m_NormGravityVector, 0, m_NormGravityVector.length);                   
                m_Norm_Gravity = (float)Math.sqrt(m_NormGravityVector[0]*m_NormGravityVector[0] + m_NormGravityVector[1]*m_NormGravityVector[1] + m_NormGravityVector[2]*m_NormGravityVector[2]);
                for(int i=0; i < m_NormGravityVector.length; i++) m_NormGravityVector[i] /= m_Norm_Gravity;
                break;
            case Sensor.TYPE_MAGNETIC_FIELD:
                if (m_NormMagFieldValues == null) m_NormMagFieldValues = new float[3];
                System.arraycopy(evnt.values, 0, m_NormMagFieldValues, 0, m_NormMagFieldValues.length);
                m_Norm_MagField = (float)Math.sqrt(m_NormMagFieldValues[0]*m_NormMagFieldValues[0] + m_NormMagFieldValues[1]*m_NormMagFieldValues[1] + m_NormMagFieldValues[2]*m_NormMagFieldValues[2]);
                for(int i=0; i < m_NormMagFieldValues.length; i++) m_NormMagFieldValues[i] /= m_Norm_MagField;  
                break;
        }
        if (m_NormGravityVector != null && m_NormMagFieldValues != null) {
            // first calculate the horizontal vector that points due east
            float East_x = m_NormMagFieldValues[1]*m_NormGravityVector[2] - m_NormMagFieldValues[2]*m_NormGravityVector[1];
            float East_y = m_NormMagFieldValues[2]*m_NormGravityVector[0] - m_NormMagFieldValues[0]*m_NormGravityVector[2];
            float East_z = m_NormMagFieldValues[0]*m_NormGravityVector[1] - m_NormMagFieldValues[1]*m_NormGravityVector[0];
            float norm_East = (float)Math.sqrt(East_x * East_x + East_y * East_y + East_z * East_z);
            if (m_Norm_Gravity * m_Norm_MagField * norm_East < 0.1f) {  // Typical values are  > 100.
                m_OrientationOK = false; // device is close to free fall (or in space?), or close to magnetic north pole.
            } else {
                m_NormEastVector[0] = East_x / norm_East; m_NormEastVector[1] = East_y / norm_East; m_NormEastVector[2] = East_z / norm_East;

                // next calculate the horizontal vector that points due north                   
                float M_dot_G = (m_NormGravityVector[0] *m_NormMagFieldValues[0] + m_NormGravityVector[1]*m_NormMagFieldValues[1] + m_NormGravityVector[2]*m_NormMagFieldValues[2]);
                float North_x = m_NormMagFieldValues[0] - m_NormGravityVector[0] * M_dot_G;
                float North_y = m_NormMagFieldValues[1] - m_NormGravityVector[1] * M_dot_G;
                float North_z = m_NormMagFieldValues[2] - m_NormGravityVector[2] * M_dot_G;
                float norm_North = (float)Math.sqrt(North_x * North_x + North_y * North_y + North_z * North_z);
                m_NormNorthVector[0] = North_x / norm_North; m_NormNorthVector[1] = North_y / norm_North; m_NormNorthVector[2] = North_z / norm_North;

                // take account of screen rotation away from its natural rotation
                int rotation = m_activity.getWindowManager().getDefaultDisplay().getRotation();
                float screen_adjustment = 0;
                switch(rotation) {
                    case Surface.ROTATION_0:   screen_adjustment =          0;         break;
                    case Surface.ROTATION_90:  screen_adjustment =   (float)Math.PI/2; break;
                    case Surface.ROTATION_180: screen_adjustment =   (float)Math.PI;   break;
                    case Surface.ROTATION_270: screen_adjustment = 3*(float)Math.PI/2; break;
                }
                // NB: the rotation matrix has now effectively been calculated. It consists of the three vectors m_NormEastVector[], m_NormNorthVector[] and m_NormGravityVector[]

                // calculate all the required angles from the rotation matrix
                // NB: see https://math.stackexchange.com/questions/381649/whats-the-best-3d-angular-co-ordinate-system-for-working-with-smartfone-apps
                float sin = m_NormEastVector[1] -  m_NormNorthVector[0], cos = m_NormEastVector[0] +  m_NormNorthVector[1];
                m_azimuth_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
                m_pitch_radians = (float) Math.acos(m_NormGravityVector[2]);
                sin = -m_NormEastVector[1] -  m_NormNorthVector[0]; cos = m_NormEastVector[0] -  m_NormNorthVector[1];
                float aximuth_plus_two_pitch_axis_radians = (float)(sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
                m_pitch_axis_radians = (float)(aximuth_plus_two_pitch_axis_radians - m_azimuth_radians) / 2;
                m_azimuth_radians += screen_adjustment;
                m_pitch_axis_radians += screen_adjustment;
                m_OrientationOK = true;                                 
            }
        }
        if (m_parent != null) m_parent.onSensorChanged(evnt);
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
        int SensorType = sensor.getType();
        switch(SensorType) {
            case Sensor.TYPE_GRAVITY: m_GravityAccuracy = accuracy; break;
            case Sensor.TYPE_MAGNETIC_FIELD: m_MagneticFieldAccuracy = accuracy; break;
        }
        if (m_parent != null) m_parent.onAccuracyChanged(sensor, accuracy);
    }
}

导入android.app.Activity；
导入android.hardware.Sensor；
导入android.hardware.SensorEvent；
导入android.hardware.SensorEventListener；
导入android.hardware.SensorManager；
导入android.view.Surface；
公共静态类定向传感器实现SensorEventListener{
公共最终静态int传感器_不可用=-1；
//对其他对象的引用
传感器管理器m_sm；
SensorEventListener m_parent；//如果此类在onSensorChanged（…）和onAccuracyChanged（…）通知后调用其父类，则为非null
活动m_Activity；//调用getWindowManager（）的当前活动。getDefaultDisplay（）getRotation（）
//来自Android传感器的原始输入
float m_Norm_Gravity；//在onSensorChanged（…）中接收到的原始重力向量的长度。注意：应该是10左右
float[]m_NormGravityVector；//归一化重力向量（即该向量的长度为1），它直接指向空间
float m_Norm_MagField；//在onSensorChanged（…）中接收到的原始磁场矢量的长度。
float[]m_NormMagFieldValues；//归一化磁场矢量（即该矢量的长度为1）
//精度规格。未知时传感器不可用，否则传感器管理器。传感器状态不可靠、传感器状态精度低、传感器状态精度中等或传感器状态精度高
int m_重力精度；//重力传感器的精度
int m_磁场精度；//磁场传感器的精度
//重力和磁场矢量可用后计算的值
float[]m_normaleastVector；//原始重力向量与磁场值的归一化叉积，指向东方
float[]m_northvector；//指向磁北的归一化向量
布尔m
private final static double PI = Math.PI;
private final static double TWO_PI = PI*2;

 case Sensor.TYPE_ROTATION_VECTOR:
                float[] orientation = new float[3];
                float[] rotationMatrix = new float[9];

                SensorManager.getRotationMatrixFromVector(rotationMatrix, rawValues);
                SensorManager.getOrientation(rotationMatrix, orientation);

                float heading = mod(orientation[0] + TWO_PI,TWO_PI);//important
                //do something with the heading
                break;



private double mod(double a, double b){
        return a % b;
    }