ST LPR530AL + Kalman Filter

[Pepade]

Hallo,

ich moechte gern ueber den Gyro "LPR530AL" (Datenblatt http://www.st.com/stonline/products/...s/lpr530al.pdf)gern meinen Pitch erhalten. Da dieser ja ziemlich am driften ist, muss ich den Kalman-Filter verwenden.
Durch die Suchfunktion in diesem Forum hab ich das "ARTIST: Two-wheel balancing robot built from fischertechnik components
" Projekt gefunden und einen entsprechenden Code unter http://home.arcor.de/uffmann/ARTIST-Dateien/artist.zip.
Ich hab diesen Code jetzt mal bei mir in C# implementiert und mir die Daten ausgeben lassen, allerdings kommt bei mir nicht sowas tolles raus, wie erwartet

Aufrufen werden die Methoden direkt nacheinander mit:

myFilter.state_update(_data.Gyroscope.Y);
int angle = myFilter.kalman_update(_data.Accelometer.X, _data.Accelometer.Z);

Weiß einer, was ich noch alles anpassen muesste?
Ich erhalte die Werte mit 1kHz. Evtl hab ich auch einen Denkfehler drin.

Lieben Gruß
Pete