Added yaw gyro correction