EKF Code Problem

Hallo Zusammen,

ich arbeite momentan an einem EKF (Extendet Kalman Filter) mit 6 
Zuständen. Der EKF erhält Daten vom Acc, Gyro und Mag. Neben den 4 
Quaternionen wird der Gyro Offset geschätzt.
Leider klappt die Schätzung des Gierwinkels nicht sonderbar gut. Sobald 
ich das Sensorboard um die x- bzw. y-Achse drehe, ändert sich 
fälschlicherweise der Gierwinkel (Drehung um die z-Achse). Der 
geschätzte z-earth vector wird allerdings korrekt ausgegeben, der Fehler 
müsste also bei der Berechnung des Gierwinkels durch den Mag entstehen. 
Und da weiß ich momentan nicht weiter, hab schon alles mehrfach 
überprüft.

Sicherheitshalber habe ich die Sensorwerte mal angehängt. Die Mag Werte 
finde ich allerdings nicht ganz plausibel!?!?

Auch die EKF C Files liegen bei und stehen zur freien Verfügung.

Hoffe jemand kann mir dabei helfen,
Sam