Get angle value in [-pi, pi] space
correct_angle_rad(x)
x
angle
angle in [-pi, pi] space
correct_angle_rad(4 * pi)