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

Question

Я хочу узнать текущую ориентацию моего телефона следующим способом:

1. Сначала получите начальную ориентацию (азимут) с помощью getRotationMatrix() а также getOrientation(),
2. Добавьте интеграцию показаний гироскопа с течением времени, чтобы получить текущую ориентацию.

Ориентация телефона:

Плоскость xy телефона зафиксирована параллельно плоскости земли. т. е. находится в ориентации "текстовые сообщения при ходьбе".

" getOrientation() "Возвращения:

Android API позволяет мне легко получить ориентацию, то есть азимут, тангаж, крен, из getOrientation(),

Обратите внимание, что этот метод всегда возвращает свое значение в диапазоне: [0, -PI] а также [o, PI],

Моя проблема:

Поскольку интеграция показаний гироскопа, обозначается dR может быть довольно большим, поэтому, когда я делаю CurrentOrientation += dR, CurrentOrientation может превышать [0, -PI] а также [o, PI] диапазоны.

Какие манипуляции необходимы, чтобы я ВСЕГДА мог получить текущую ориентацию в пределах [0, -PI] а также [o, PI] диапазоны?

Я попробовал следующее в Python, но я очень сомневаюсь в его правильности.

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

---

замечания

Это НЕ так просто, потому что в нем участвуют следующие нарушители:

1. Показание датчика ориентации идет от 0 в -PI, а затем ПРЯМО Прыгает +PI и постепенно возвращается к 0 с помощью +PI/2,
2. Интеграция показаний гироскопа также приводит к некоторым проблемам. Должен ли я добавить dR к ориентации или вычитать dR,

Пожалуйста, сначала обратитесь к документации Android, прежде чем дать подтвержденный ответ.

Расчетные ответы не помогут.

Answer 1

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

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

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

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

---

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

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

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

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

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

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

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

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

---

---

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

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

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

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

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

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

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

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

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

Answer 2

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

Answer 3

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

Answer 4

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

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

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

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

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

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

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

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