MPU9250 IMU Auswertung

Hallo,

ich habe es endlich geschafft von den MPU9250 kontinuierlich Daten abzurufen. Dies funktioniert auch gut. Verwendeter µC ist ein ATXmega256a3BU.
Nur mit der Auswertung klappt es nicht ganz so wie gedacht. Aufgrund beschränkter Rechenleistung will ich mich auf einen Komplementär Filter beschränken. Bzw. mit der Notation komme ich nicht so richtig klar.
Prinzipiell werden alle Messwerte im Sensor in einem FIFO abgelegt, und dort auch "unterbrechungsfrei" abgerufen. "Unterbrechungsfrei" heißt das keine Messwerte verloren gehen.
Es werden auch keine Messbereiche überschritten, ich frage im Programm auf Überlauf der Messwerte ab.

Die Gyrometer Werte werden Aufsummiert. Die Werte des Accelometer über die "atan2" Funktion verarbeitet.

Wird im Stillstand der Sensor um die Achsen gedreht, stimmen die errechneten Winkelwerte. Aber sobald mein Roboter in Bewegung ist, können die errechneten Winkel deutlich abweichen (+-10°).

Die Frage ist nun, ob ein Komplementär Filter ausreichend ist?
Ist der, Rechenleistung sparende, Ablauf nur die Gyrometer Werte zu integrieren und nur den letzten ACCEL Wert zu verwenden der Fehler? Müssten/können die ACCEL Werte auch integriert werden? Ich möchte gern wenige "atan2" Funktionen nutzen, da diese erheblich Rechenleistung benötigen.

Anbei die entscheidenden Code-Zeilen.

Code:

---

//Euler WINKEL
// +- M_PI (Pi/2) um Winkelanzeige auf 0° zu setzen                       
        ACC_roll        = (((atan2(ACCEL_Wert[2],ACCEL_Wert[1])) - M_PI_2) * 180/M_PI);        //Drehung um X-Achse                                                       
        ACC_pitch  = ((atan2(ACCEL_Wert[0],ACCEL_Wert[2])) * 180/M_PI);                                //Drehung um Y-Achse                                               
        //ACC_yaw = ( ((atan2(ACCEL_Wert[0],ACCEL_Wert[1])) + M_PI) * 180/M_PI);        //Drehung um Z-Achse

        //Komplementär Filter
                X_Achse[0] = ((X_Achse[0] - GYRO_Wert[0]) * 0.98) + (ACC_roll * 0.02);
                Y_Achse[0] = ((Y_Achse[0] - GYRO_Wert[1]) * 0.98) + (ACC_pitch * 0.02);
                //Z_Achse  = ((Z_Achse + GYRO_Wert[2]) * 0.98) + (ACC_yaw * 0.02);

---

Edit:
Die Werte des Accelometer zeitgleich mit dem Gyrometer zu verrechnen, bringt leider einen gegenteiligen Effekt. Zuvor wurde nur alle Gyro Werte aufsummiert, und der letzte ACCEL Wert mittels "atan2" genutz.
Aber die Messwerte des Accelometer über einen gleitenden Mittelwert zu stabilisieren, verbessert die Winkelausgabe. Dies muss ich aber noch weiter bearbeiten. Zur Zeit mittle ich 10 Werte, bei 25 Werten war mir die Anzeige bereits zu träge.