Added gyroscope drift compensation