Как получить азимут телефона с показаниями компаса и показаниями гироскопа?

Датчик ориентации фактически получает показания от реального магнитометра и акселерометра.

Я думаю, может быть, это и есть источник путаницы. Где это указано в документации? Что еще более важно, в документации где-то явно указано, что показания гироскопа игнорируются? Насколько я знаю реализован метод описанный в этом видео:

Sensor Fusion на устройствах Android: революция в обработке движения

Этот метод использует гироскопы и объединяет их показания. Это в значительной степени делает остальную часть вопроса спорной; тем не менее я постараюсь ответить на него.

Датчик ориентации уже интегрирует для вас показания гироскопа, так вы получаете ориентацию. Я не понимаю, почему вы делаете это сами.

Вы неправильно выполняете интеграцию показаний гироскопа, это сложнее, чем CurrentOrientation += dR (что неверно). Если вам нужно интегрировать показания гироскопа (не понимаю, почему, SensorManager уже делает это за вас), пожалуйста, прочтите Матрица косинуса направления IMU: теория, как это сделать правильно (уравнение 17).

Не пытайтесь интегрировать углы Эйлера (азимут, тангаж, крен), ничего хорошего не выйдет.

Используйте в своих вычислениях либо кватернионы, либо матрицы вращения вместо углов Эйлера. Если вы работаете с матрицами вращения, вы всегда можете преобразовать их в углы Эйлера, см.

Вычисление углов Эйлера по матрице вращения, Грегори Г. Слабо

(То же верно и для кватернионов.) Есть (в невырожденном случае) два способа представить вращение, то есть вы получите два угла Эйлера. Выберите тот, который находится в нужном вам диапазоне. (В случае карданного замка, существует бесконечно много углов Эйлера, см. PDF-файл выше). Просто пообещайте, что вы больше не будете использовать углы Эйлера в своих вычислениях после преобразования матрицы вращения в углы Эйлера.

Непонятно, что вы делаете с дополнительным фильтром. Вы можете реализовать чертовски хорошее слияние датчиков на основе Матрица направленного косинуса IMU: теория, которая по сути является учебным пособием. Это нетривиально, но я не думаю, что вы найдете лучший и более понятный учебник, чем эта рукопись.

Одна вещь, которую мне пришлось открыть для себя, когда я реализовал слияние датчиков на основе этой рукописи, заключалась в том, что так называемый интегральный может произойти зависание. Я позаботился об этом, ограничив TotalCorrection (стр. 27). Вы поймете, о чем я говорю, если реализуете этот сплав датчиков.

ОБНОВЛЕНИЕ: Здесь я отвечаю на ваши вопросы, которые вы разместили в комментариях после принятия ответа.

Я думаю, что компас дает мне мою текущую ориентацию, используя гравитацию и магнитное поле, верно? Используется ли гироскоп в компасе?

Да, если телефон более или менее неподвижен хотя бы полсекунды, вы можете получить хорошую оценку ориентации, используя только гравитацию и компас. Вот как это сделать: Может ли кто-нибудь сказать мне, используется ли датчик силы тяжести как датчик наклона для повышения точности курса?

Нет, гироскопы в компасе не используются.

Не могли бы вы объяснить, почему интеграция, сделанная мной, неверна? Я понимаю, что если шаг моего телефона указывает вверх, угол Эйлера не работает. Но что еще не так с моей интеграцией?

Есть две несвязанные вещи: (i) интегрирование должно быть выполнено по-другому, (ii) углы Эйлера вызывают проблемы из-за блокировки карданного подвеса. Повторяю, эти двое не связаны.

Что касается интеграции: вот простой пример того, как вы на самом деле можете увидеть, что не так с вашей интеграцией. Пусть x и y — оси горизонтальной плоскости комнаты. Возьмите телефон в руки. Поверните телефон вокруг оси x (комнаты) на 45 градусов, затем вокруг оси y (комнаты) на 45 градусов. Затем повторите эти шаги с самого начала, но теперь вращайте сначала вокруг оси Y, а затем вокруг оси X. Телефон оказывается в совершенно другой ориентации. Если сделать интеграцию по CurrentOrientation += dR разницы не увидите! Пожалуйста, прочтите связанную выше матрицу косинуса направления IMU: рукопись Theory, если вы хотите правильно выполнить интеграцию.

Насчет углов Эйлера: они портят стабильность приложения и мне достаточно их не использовать для произвольных поворотов в 3D.

Я до сих пор не понимаю, почему вы пытаетесь сделать это сами, почему вы не хотите использовать оценку ориентации, предоставленную платформой. Скорее всего, вы не можете сделать лучше, чем это.

Я думаю, вам следует избегать устаревшего «датчика ориентации» и использовать методы слияния датчиков, такие как getRotationVector, getRotationMatrix, которые уже реализуют алгоритмы слияния, особенно Invensense, которые уже используют данные гироскопа.

Если вам нужен простой алгоритм объединения датчиков, называемый фильтром баланса (см. http://www.filedump.net/dumped/filter1285099462.pdf). Подход как в

http://postimg.org/image/9cu9dwn8z/

Это интегрирует гироскоп для получения угла, затем фильтр верхних частот результат для устранения дрейфа и добавляет его к сглаженным результатам акселерометра и компаса. Интегрированные данные гироскопа, прошедшие фильтрацию верхних частот, и данные акселерометра/компаса добавляются таким образом, что две части складываются в одну, так что на выходе получается точная оценка в разумных единицах. Для фильтра баланса постоянная времени может быть настроена для настройки отклика. Чем короче постоянная времени, тем лучше отклик, но тем больше шума ускорения будет пропущено.

Чтобы увидеть, как это работает, представьте, что у вас есть самая новая точка данных гироскопа (в рад/с), сохраненная в gyro, новейшее измерение угла от акселерометра сохранено в angle_acc, и d это время от последних данных гироскопа до настоящего момента. Тогда ваш новый угол будет рассчитан с использованием

угол = b * (угол + гироскоп*dt) + (1 - b) *(угол_акк);

Вы можете начать с попытки, например, b = 0,98. Вы также, вероятно, захотите использовать быстрое время измерения гироскопа dt, чтобы гироскоп не отклонялся более чем на пару градусов перед выполнением следующего измерения. Балансовый фильтр полезен и прост в реализации, но не является идеальным подходом к объединению датчиков. Подход Invensense включает несколько умных алгоритмов и, возможно, некую форму фильтра Калмана.

Источник: Professional Android Sensor Programming, Adam Stroud.

Насколько я знаю, если значение азимута неточное из-за магнитных помех, вы ничего не можете сделать, чтобы устранить это. Чтобы получить стабильные показания азимута, вам необходимо отфильтровать значения акселерометра, если 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;
    }
}

В вашей деятельности создайте экземпляр объекта Compass, скажем, в onCreate, registerListener в onResume и unregisterListener в onPause

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

Лучше позволить реализации Android обнаружения ориентации справиться с этим. Теперь вы получаете значения от -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 — это массивы для хранения матриц поворота и наклона, 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];
}