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

В некоторых источниках говорится о важности порядка применения крена, тангажа и рыскания. Но я не могу понять, почему это связано.

Возьмите правую руку: направьте большой палец вверх, указательный палец от себя, а средний палец влево. Теперь у вас есть правосторонняя система координат: большой палец — это ось X, указательный палец — ось Y, а средний палец — ось Z.

Сначала поверните на +90° вокруг оси X (большой палец). Теперь ваш указательный палец указывает налево, а средний палец — на вас. Затем поверните на +90° вокруг оси Y (указательный палец). Теперь ваш большой палец направлен от вас, указательный — влево, а средний — вверх.

Теперь вернитесь в исходное положение и примените те же два поворота в другом порядке, сначала вокруг оси Y, а затем вокруг оси X: Повернуть на +90° вокруг оси Y (указательный палец). Ваш большой палец теперь указывает вправо, указательный палец от вас, а средний палец вверх. Затем поверните на +90° вокруг оси x (большой палец). Теперь ваш большой палец указывает вправо, указательный палец вверх, а средний палец к вам.

Это отличается от предыдущего результата: порядок ротации имеет значение.

Я использовал значения акселерометра, введя их в некоторые формулы в Интернете (те самые формулы арктангенса, которые все используют, но никто толком не объясняет)

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

Когда датчик наклонен, измеренный вектор также имеет компоненты x и y.
Для простоты представьте случай, когда датчик наклонен вокруг положительной оси Y.
Теперь датчик измеряет компонент x, а также компонент az: (x, 0, -z).

Как видно из следующего изображения, угол поворота θ = atan2(x, z).

Черная стрелка – ускорение свободного падения, а красная и синяя – оси X и Z системы координат (наклоненного) датчика.

---

Однако измерения акселерометра бесполезны для определения ориентации, когда датчик ускоряется, потому что тогда измеренный вектор больше не указывает точно вниз (в глобальной системе координат).

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

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

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

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

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

При желании вы также можете получить кватернион напрямую из вектора ускорения:

    Quaternion quaternionFromDirection(Vec3f v) {
/*
* Формула:
* q = cos(ϑ/2) + sin(ϑ/2)·(x·i + y·j + z·k)
* где (xyz) — единичный вектор, представляющий ось, вокруг которой
* корпус поворачивается; ϑ - угол, на который он повернут.
*
* Источник:
* https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotations
*
* Ось вращения (xyz) можно рассчитать, взяв нормализованную
* перекрестное произведение (0 0 1) и заданного вектора. Угол поворота
* ϑ можно найти, используя |A×B| = |A||B|·sin(ϑ).
*/

        // Сначала проверяем крайний случай, когда v == (0 0 z), т.е. вертикальный
        if (v.x == 0 && v.y == 0)
            return {1, 0, 0, 0};

        // Вычислить векторное произведение и его норму.
        Vec3f cross     = {v.y, -v.x, 0};
        float crossNorm = cross.norm();
        cross /= crossNorm;

        // Вычислить угол ϑ.
        float angle = std::asin(crossNorm / v.norm());

        // Вычислить полученный кватернион.
        return {
            std::cos(angle / 2),            //
            std::sin(angle / 2) * cross.x,  //
            std::sin(angle / 2) * cross.y,  //
            std::sin(angle / 2) * cross.z,  //
        };
    }