Added yaw drift compensation

Added yaw drift compensation and changed clipping code