Compass now using a similar algorithm and works with minor error