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

Ответ 1

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Ответ 2

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

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

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

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

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

angle = b * (angle + gyro * dt) + (1 - b) * (angle_acc);

Вы можете начать, например, с b = 0.98. Вы также, вероятно, захотите использовать быстрое время измерения гироскопа dt, поэтому гироскоп не дрейфует более чем на пару градусов до того, как будет выполнено следующее измерение. Фильтр баланса полезен и прост в применении, но не является идеальным методом слияния датчиков. Подход Invensenses включает в себя некоторые умные алгоритмы и, возможно, некоторую форму фильтра Калмана.

Источник: Профессиональное программирование для Android, Adam Stroud.

Ответ 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;
    }
}

В вашей деятельности создайте экземпляр объекта 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);
}

Ответ 4

Лучше разрешить андроидную реализацию определения ориентации справиться с этим. Теперь, да значения, которые вы получаете, от -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 x axis aligns with
     * arg 3: what world(according to app) axis , device y axis aligns with
     * world x = app x = app east
     * world y = app y = app north
     * device x = device left side = device east
     * device y = device top side  = device 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];
}