import numpy as np
import matplotlib.pyplot as plt

def mi_unwrap(yaw, yaw_prev):
    yaw += (2*np.pi) * np.ceil(((yaw_prev - yaw) // np.pi) / 2.)
    return yaw

yaw_prev = -10000.
yaw_reales = np.arange(yaw_prev, -yaw_prev, 1)
yaw_arreglados = []

for yaw_real in yaw_reales:
    yaw_calculado = np.arctan2(np.sin(yaw_real), np.cos(yaw_real)) # el resultado va a estar entre -pi y pi
    yaw_arreglado = mi_unwrap(yaw_calculado, yaw_prev)
    yaw_arreglados.append(yaw_arreglado)
    print yaw_prev, yaw_calculado, yaw_arreglado
    yaw_prev = yaw_arreglado


plt.plot(yaw_reales, yaw_arreglados)
plt.show()