ADXL345, ITG3200 und der Kalmanfilter

Hallo Leute,

Nach einer kurzen Sommerpause gehts bei meinem Projekt wieder weiter (R/C Helikopter stabilisieren).

Die Hardware steht soweit, die IMU läuft sehr zufriedenstellend...Hardwarefehler hat sich schonmal keiner eingeschlichen =)

Ich hab die Sensoren soweit in Betrieb genommen, FreeRTOS und ein kleines Protokoll zur Visualisierung der Daten lauffähig gemacht. Der nächste Schritt war für mich "Kalmanfilter", mehr aus Interesse als aufgrund von Nutzen ;)

Das funktioniert auch soweit...Das heißt ich bekomm meinen Winkel korrekt raus, leider ist der Filter ziemlich träge und aus irgendeinem Grund sind die Achsen miteinandere verkoppelt.

Im Anhang ein Plot davon. Man sieht eine Änderung EINER Achse von ~23° auf 0° und wieder zurück..Neben dem Überschwingen und dem Einfluss auf die zweite Achse dauert ein Ergebnis bis zum korrekten Winkel ~6-7s was mir eigentlich zulange dauert.

Als Code verwende ich einen abgewandelten Kalmanfilter von rotomotion, der gleich beide Achsen miteinander verschmilzt. Code im Anhang.

Zum Timing( dank FreeRTOS kann ich das relativ genau sagen):

Höchste Prioität (3): 10ms.... ADXL/ITG3200 lesen und Winkel neu berechnen
(2): 100ms.... Protokollhandling (noch sehr einfach)
(1): 1sek.... LED blinken lassen

Es macht aber keinen Unterschied ob ich den Kalman alle 10ms oder jede ms mit neuen Werten füttere...das Ergebnis im Plot ist das selbe. Ein kompletter Taskzyklus dauert 890µs..also Datenlesen und Kalman ausrechnen

Wo kann ich jetzt ansetzen um den Filter zu tunen?
Da ich ihn leider garnicht verstehe, tu ich mir sehr sehr schwer :(
Hat jemand einen Tipp?

€dit gerade bemerkt, da ist ein kommentierter Teil in der kalman lieg für die Gyro und Acc werte...
/*
float Q_angle = 0.001;
float Q_gyro = 0.003;
*/
float Q_angle = 0.001;
float Q_gyro = 0.001;

Das ist ein Rest von einem Versuch ob sich was ändert wenn ich was verstelle...das ist/war aber die EINZIGE Änderung in dem Code

Ich häng noch ein Bild an welches die jeweiligen Gyrowerte in rad/s anzeigt damit man sieht das die Änderung vom Filter kommt und nicht von den Gyros kommt...hier sind die Achsen noch unabhängig!

LG
Michael