DataModerated 9DOF IMU Driver Test

Latest Video http://www.youtube.com/watch?v=DK-bKWB47-M&feature=related Code at http://www.datamoderated.com/IMU/ to the link. 3d Visualization and driver tests with SparkFun 9D0f Razor. I've got the kalman filter working well, and the input without so much noise as well. What I'm looking for is some help getting the displacement algorithm going. So far I've tried double integration of the accelerometer with no hope. Any Ideas? Thanks, -C