normalize_angle function

Wrap angle measurements to the interval (-pi, pi).