Fixed error in pose prediction when going from theta +pi -> -pi