Найти кватернион по данным гироскопа? - PullRequest
Новая
       18

Найти кватернион по данным гироскопа?

3 голосов
/ 15 октября 2011

Я пытался создать фильтр, который может успешно комбинировать компасные, геомагнитные и гироскопические данные для получения плавного опыта дополненной реальности. Прочитав этот пост вместе с многочисленными обсуждениями, я наконец-то нашел хороший алгоритм для исправления данных моего датчика. Большинство примеров, которые я прочитал, показывают, как исправить акселерометры с помощью гироскопов, но не исправить данные компаса + акселерометр с помощью гироскопа. Это алгоритм, на котором я остановился, который прекрасно работает, за исключением того, что я сталкиваюсь с блокировкой карданного подвеса, если я пытаюсь посмотреть на сцену, если я не смотрю на север. Этот алгоритм Balance Filter , только вместо того, чтобы реализовываться только в 3D

Шаг инициализации:

  • Инициализация матрицы вращения мира с использованием (шумного) акселерометра и данных датчика компаса (это уже предоставляется Android)

Обновление шагов:

  • Интегрирование показаний гироскопа (считывание времени_дельта *) для каждой оси (x, y, z)

  • Вращать мировую матрицу вращения, используя углы Эйлера, представленные интегрированием

  • Найдите кватернион из вновь повернутой матрицы

  • Найти матрицу вращения по нефильтрованному акселерометру + данные компаса (используя функцию ОС, я думаю, что она использует расчет угла / оси)

  • Получить кватернион из матрицы, сгенерированной на предыдущем шаге.

  • Проскальзывание между кватернионом, сгенерированным на шаге 2 (из гироскопа), и данными акселерометра с использованием коэффициента, основанного на некоторой экспериментальной магии

  • Конвертировать обратно в матрицу и использовать ее для рисования сцены.

Моя проблема в том, что когда я смотрю на север, а затем пытаюсь посмотреть на юг, все взрывается, и кажется, что это карданный замок. После нескольких блокировок карданного подвеса весь фильтр находится в неопределенном состоянии. Осматривая вокруг, я слышу, как все говорят: «Просто используйте кватернионы», но я боюсь, что это не так просто (по крайней мере, для меня), и я знаю, что кое-что мне просто не хватает. Любая помощь будет принята с благодарностью.

Ответы [ 2 ]

4 голосов
/ 09 января 2013

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

2 голосов
/ 16 октября 2013

Многие извинения, если информация задерживается или не используется конкретно, но может быть полезна для других, как я нашел после некоторого исследования :::

a.Используя фильтр Калмана (линейный или нелинейный), вы выполняете следующие действия: *

Гироскоп, чтобы интегрировать угол дельты, в то время как акселерометры сообщают вам внешний предел.

b.Скорости Эйлера отличаются от скорости изменения угла гироскопа, поэтому вам понадобится кватернион или представление Эйлера ::

Кватернион не тривиален, но два основных шага: ---- 

1. For Roll, pitch,yaw   you get three quaternions as cos(w) +sin(v) where w is scalar part and v is vector part (or when coding just another variable) 
 Then simply multiply all 3 quat. to get a delta quaternion
 i.e   quatDelta[0] =c1c2*c3 - s1s2*s3;
  quatDelta[1] =c1c2*s3 + s1s2*c3;
  quatDelta[2] =s1*c2*c3 + c1*s2*s3;
  quatDelta[3] =c1*s2*c3 - s1*c2*s3;
where c1,c2,c3 are cos of roll,pitch,yaw and s stands for sin of the same actually half of those gyro pre integrated angles.

2. Then just multiply by old quaternion you had

  newQuat[0]=(quaternion[0]*quatDelta[0] - quaternion[1]*quatDelta[1] - quaternion[2]*quatDelta[2] - quaternion[3]*quatDelta[3]);
  newQuat[1]=(quaternion[0]*quatDelta[1] + quaternion[1]*quatDelta[0] + quaternion[2]*quatDelta[3] - quaternion[3]*quatDelta[2]);
  newQuat[2]=(quaternion[0]*quatDelta[2] - quaternion[1]*quatDelta[3] + quaternion[2]*quatDelta[0] + quaternion[3]*quatDelta[1]);
  newQuat[3]=(quaternion[0]*quatDelta[3] + quaternion[1]*quatDelta[2] - quaternion[2]*quatDelta[1] + quaternion[3]*quatDelta[0]);

Как выцикл по коду, который он обновляет, так что только кватинирование - это глобальные переменные, а не остальные 

3. Lastly if you want Euler angles from them then do the following:


`euler[2]=atan2(2.0*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]), 1-2.0*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]))euler[1]=safe_asin(2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]))euler[0]=atan2(2.0*(quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2]), 1-2.0*(quaternion[2] *quaternion[2]+quaternion[3]*quaternion[3]))`

euler[1] is pitch and so on..

Я просто хотел наметить общие шаги реализации кватернионов.Могут быть небольшие ошибки, но я попробовал это сам, и это работает.Обратите внимание, что при переходе на углы Эйлера вы получите особенности, также называемые «Замок карданного подвеса»

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

...