Debugged yaw calibration algorithm. Tested and proven working.