hab ich nicht, hast mich aber auf ne Idee gebracht ...
werd mir mal diesen:
http://www.techsystemsembedded.com/Kalman.html
Quellcode zur Brust nehmen ... schaut nicht schlecht aus.