Vergleiche doch mal mit diesem Teil:

http://www.siliconsensing.com/DMU02

Das mit dem Kalman Filter ist natürlich auch ein wichtiges Argument. Den mußt Du Dir sonst selber schreiben. Dafür diese Quelle als BasisCode:

http://home.arcor.de/uffmann/ARTIST3.htm

http://home.arcor.de/uffmann/ARTIST-Dateien/artist.zip

Look for the files kalman.h and kalman.c