Кватернион - вычисление углов с использованием акселерометра и гироскопа

Question

Я использовал 6dof Блок LSM6DS0 IMU (с акселерометром и гироскопом). И я пытаюсь вычислить угол поворота вокруг всех трех осей. Я пробовал методы, но не получал ожидаемых результатов.

Методы пытались:

фильтр подход (я) Complementatry - Я могу получить углы, используя формулу, представленную в ссылке Angle comutation method.

Но проблема в том, что углы совсем не согласуются и много дрейфуют. Более того, когда ИДУ вращается вокруг одной оси, углы, рассчитанные по другой оси, слишком сильно раскачиваются.

(ii) Расчет угла наклона по кватерниту: было достаточно ресурсов, требующих, чтобы углы были очень хорошо вычислены с использованием подхода кватерниона, но ни у кого не было четкого объяснения. Я использовал this method, чтобы обновить кватернион для каждого значения, взятого из блока IMU. Но ссылка поясняет, как рассчитать углы от кватерниона.

Я использовал GLM математике библиотека Inorder конвертировать кватернион в эйлеровых углов, а также попробовали формулу, указанную в wiki link. С помощью этого метода, поскольку в расчете шага asin возвращает только -90 до +90 градусов, я не могу повернуть объект в 3D как тот, который они делали в упомянутой ссылке.

Кто-нибудь пробовал преобразовать кватернион в угол раньше? Мне нужно рассчитать углы вокруг всех трех осей в диапазоне от 0 до 360 градусов или от -180 до +180 градусов.

Любая помощь может быть действительно оценена. Заранее спасибо.

Answer 1

http://davidegironi.blogspot.com.ar/2013/02/avr-atmega-mpu6050-gyroscope-and.html#.VzjDKfnhCUk

Sq = [q0, q1, q2, q3] 
    //sensor quaternion can be translated to aerospace sequence of yaw/pitch/roll angles 
    yaw = atan2(2*q1*q2 - 2*q0*q3, 2*(q0^2) + 2*(q1^2) - 1) 
    pitch = -asin(2*q1*q3 + 2*q0*q2) 
    roll = atan2(2*q2*q3 - 2*q0*q1, 2*(q0^2) + 2*(q3^2) - 1)