function theta = angle_difference(theta1, theta2)
	theta = theta1 + pi - theta2;
	two_pi = 2 * pi;
	if theta < 0
		theta = theta + two_pi;
	elseif theta > two_pi
		theta = theta - two_pi;
	end
	theta = theta - pi;