Ist halt sehr schwierig den Fehler zu finden, wenn man nicht weiß wo man anfangen soll.

In Deinem ersten Post hast Du ja ein kurzes Video verlinkt. Dort sehen die Anzeigen für Beschleunigung und Drehrate recht plausibel aus. Deshalb vermute ich das Problem im nachgeschalteten Algoritmus.