SHARE
TWEET

Untitled

a guest Sep 18th, 2019 105 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import numpy as np
  2. import matplotlib.pyplot as plt
  3.  
  4. def mi_unwrap(yaw, yaw_prev):
  5.     yaw += (2*np.pi) * np.ceil(((yaw_prev - yaw) // np.pi) / 2.)  
  6.     return yaw
  7.  
  8. yaw_prev = -10000.
  9. yaw_reales = np.arange(yaw_prev, -yaw_prev, 1)
  10. yaw_arreglados = []
  11.  
  12. for yaw_real in yaw_reales:
  13.     yaw_calculado = np.arctan2(np.sin(yaw_real), np.cos(yaw_real)) # el resultado va a estar entre -pi y pi
  14.     yaw_arreglado = mi_unwrap(yaw_calculado, yaw_prev)
  15.     yaw_arreglados.append(yaw_arreglado)
  16.     print yaw_prev, yaw_calculado, yaw_arreglado
  17.     yaw_prev = yaw_arreglado
  18.    
  19.  
  20. plt.plot(yaw_reales, yaw_arreglados)
  21. plt.show()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top