Such mal bei Sourceforge nach dem Autopilot Projekt. Die machen genau das was du suchst. -> Kalman Filter mit Gyro und B-Messern.