Przejście z kwaternionów na kąty eulera

[karol9966]

Cześć, testuje różne sposoby wyznaczania orientacji w przestrzeni za pomocą akcelerometru i żyroskopu i trafiłem na filtr Madgwicka, mam wyznaczone kwaterniony i przechodzę z nich na kąty eulera. Nie mam problemów z wyliczeniem Yaw, ale mam problem, gdy Pitch osiągnie wartość +90* lub - 90*, wtedy przy dalszym wychylaniu wartość ta maleje a Yaw skacze do 180*, jak temu zapobiec, tak bym mógł osiągnąć pełną swobodę zakresów?

Pitch = atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1);
Roll = -1 * asin(2 * q1 * q3 + 2 * q0 * q2);
Yaw = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 - 1);

[Treker (Damian Szymański)]

karol9966, w czym piszesz program? Jeśli chcesz działać na Arduino, to jest tam do tego bardzo fajna biblioteka. Wykorzystałem ją np. w poniższym artykule:

Pierwsze starcie z Genuino 101 – wizualizacja położenia