Изменение отношения - углы и ось - кватернионная математика

У меня есть приложение, которое фиксирует углы, когда пользователь ходит по объекту, а указательное устройство (предпочтительно) находится в центре объекта. Угол получает reset по команде пользователя. - так ссылки отношение получает reset

Использование углов Эйлера приводит к блокировке каркаса, поэтому в настоящее время я использую кватернионы и расчетные углы следующим образом:

double angleFromLastPosition = acos(fromLastPositionAttitude.quaternion.w) * 2.0f;

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

Задача 1: если рывок рыскания и высота тона слегка меняются (пользователь не указывает прямо в центр объекта), то и angleFromLastPosition. Я понимаю эту часть, так как моя угловая формула просто показывает угол между двумя отношениями устройств в трехмерном пространстве.

Сценарий:

Я отмечаю начало поворота и начинаю двигаться по кругу вокруг объекта, указывая на центр.
Я останавливаюсь, скажем, на 45 градусов и меняю шаг устройства, указывая его выше или ниже. Угол изменяется соответственно.
Мне хотелось бы видеть: угол остается на уровне 45 градусов, даже если изменяется шаг или рыскание.

Вопрос 1: как я могу рассчитать только рулон устройства с использованием кватернионов и игнорировать изменения в других двух осях (по крайней мере, в пределах некоторого разумного количества градусов).

Проблема 2: если я немного повернусь, а затем полностью заморожу устройство (на штативе, чтобы его вообще не трясло), angleFromLastPosition дрейфует со скоростью 1 градус за 10-20 секунд, и, похоже, это не так быть линейным. Другими словами, он сначала сползает, а затем значительно замедляется. Иногда я вообще не дрейфую - угол твердый, если устройство неподвижно. И это заставляет меня потерять понимание того, что происходит.

Вопрос 2, что здесь происходит, и как я могу позаботиться о дрейфе?

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

Ответ 1

Я тестировал это и, похоже, работал в соответствии с тем, что вы ищете в вопросе 1, Андрей.

Я устанавливаю homeangle изначально 0, и сразу после первого прохода я сохраняю угол, возвращаемый из walkaroundAngleFromAttitude: fromHomeAngle: в homeangle, для будущего использования.

Мой тестировании включены начиная обновления устройств с использованием опорного кадра:

[_motionManager 
startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryZVertical 
toQueue:operationQueue 
withHandler:handler];

и используя следующие методы, вызываемые в обработчике:

- (CMQuaternion) multiplyQuanternion:(CMQuaternion)left withRight:(CMQuaternion)right {

    CMQuaternion newQ;
    newQ.w = left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
    newQ.x = left.w*right.x + left.x*right.w + left.y*right.z - left.z*right.y;
    newQ.y = left.w*right.y + left.y*right.w + left.z*right.x - left.x*right.z;
    newQ.z = left.w*right.z + left.z*right.w + left.x*right.y - left.y*right.x;

    return newQ;
}

-(float)walkaroundRawAngleFromAttitude:(CMAttitude*)attitude {

    CMQuaternion e =  (CMQuaternion){0,0,1,1};
    CMQuaternion quatConj = attitude.quaternion;
    quatConj.x *= -1; quatConj.y *= -1; quatConj.z *= -1;
    CMQuaternion quat1 = attitude.quaternion;
    CMQuaternion quat2 = [self multiplyQuanternion:quat1 withRight:e];
    CMQuaternion quat3 = [self multiplyQuanternion:quat2 withRight:quatConj];

    return atan2f(quat3.y, quat3.x);
}
-(float)walkaroundAngleFromAttitude:(CMAttitude*)attitude fromHomeAngle:(float)homeangle {

    float rawangle = [self walkaroundRawAngleFromAttitude:attitude];
    if (rawangle <0) rawangle += M_PI *2;
    if (homeangle < 0) homeangle += M_PI *2;
    float finalangle = rawangle - homeangle;
    if (finalangle < 0) finalangle += M_PI *2;

    return finalangle;
}

Это использует некоторый модифицированный и расширенный код из Поиск нормального вектора для устройства iOS

Изменить для решения вопросов 2 и 2:

Это может быть не разрешимо. Я видел это в других приложениях (например, 360 pano) и читал о ошибочных показаниях в гироскопах и т.д. Если вы попытаетесь компенсировать это, вы, конечно, столкнетесь с неудовлетворительным опытом, когда какое-то подлинное вращательное движение будет зависеть от кода компенсации. Насколько я интерпретировал последние несколько лет, это проблема аппаратного обеспечения.