Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement