Hallo liebe Gemeinde!

Nachdem ich nun seit Wochen Probleme hab, ein ordentliches Ergebnis eines Kalmanfilters zu bekommen, hoffe ich dass mir hier vielleichtjemand weiterhelfen.
Ich habe mich viel in den Filter eingelesen, jedoch hab ich mich dann dazu entschieden, Beispiel zu adaptieren.
Als Grundlage verwende ich den Filter von Tom Pycke: http://tom.pycke.be/mav/92/kalman-demo-application
Nach unzähligen Anpassungen der Variablen, Zeiten, Etc kommt bei mir immer noch sowas raus:
http://youtu.be/SB921pnLwAE
1. Zu beginn schwingt er sich sehr seltsam ein.
2. Er ist sehr langsam und schwingt immer etwas über.
Wenn ich die Parameter ändere, dass er schneller ist, also ziemlich gleich dem Beschleunigungssensor ist, werde Störungen durch Wackeln und stoßen nicht mehr richtig rausgefiltert.

Noch ein paar Eckdaten:
5DOF Board von Sparkfun wo ich je die Y Kanäle benutze (IDG300, ADXL330); Sind eigentlich die Nachfolger der sehr verbreiteten Sensoren ADXL320 (allerdings mit nur +-3g) und des IDG*nummerWeißIchNichtAuswendig*
Controller ist ein STM32F103 ARM Cortex M3 Controller
Hab den Code sehr simpel gehalten, sogar ADC ohne DMA um Fehlerquellen ausschließen zu können.
Bei Bedarf gibts noch Code, jedoch denke ich dass es irgendwelche Parameter Probleme sind, da ich ein sehr ähnliches Problem hatte als ich noch einen PIC18 zur Auswertung verwendet hatte.
Habe auch mittlerweile die Serielle Bluetooth Verbindung gegen Kabel getauscht, was aber auch keine Änderung gebracht hatte.

Bin für jeden Tipp dankbar!