xddot[:, t] = [[v[t] * math.cos(xd[2, t])],
                   [v[t] * math.sin(xd[2, t])],
                   [w[t]]]
    xd[:, t + 1] = xd[:, t] + dt * xddot[:, t]

  pyplot.figure(1)
  pyplot.clf()
  pyplot.hold(True)
  pyplot.plot(T, xd[:, 0:-1].T)

  pyplot.figure(2)
  pyplot.clf()
  pyplot.hold(True)
  pyplot.plot(xd[0, :].T, xd[1, :].T)
  for t in xrange(0, len(T), 3):
    draw.drawcar(xd[0, t], xd[1, t], xd[2, t], .05, 2)
  pyplot.title('Desired Trajectory');
  pyplot.axis('equal')

  # Squiggle
  x0 = np.mat([1, 1, 1]).T
  xddot = np.mat(np.zeros((3, len(T))))
  xd = np.mat(np.zeros((3, len(T) + 1)))
  xd[:, 0] = x0
  for t in xrange(len(T)):
    xddot[:, t] = np.mat([[2 * np.cos(xd[2, t])],
                          [1 * np.sin(xd[2, t])],
                          [(xd[0, t])]])
    xd[:, t + 1] = xd[:, t] + dt * xddot[:, t]

  pyplot.figure(3)
  ekf_est_data = []
  gps_data = []
  path_wave_data = []

  for x, y, theta in ekf_est_reader:
    ekf_est_data.append((float(x), float(y), float(theta)))

  for t, lat, long, alt, track, err_track, speed, vel_cmd, turn_cmd in gps_reader:
    gps_data.append((float(t) / 1e9, float(lat), float(long), float(track) / 180 * math.pi, float(turn_cmd)))

  for x, y in path_wave_reader:
    path_wave_data.append((float(x), float(y)))

  pyplot.figure(1)
  print 'PLOT EKF EST'
  print 'Plotting path'
  pyplot.plot([r[0] for r in ekf_est_data], [r[1] for r in ekf_est_data], 'g-')
  pyplot.plot([r[0] for r in path_wave_data], [r[1] for r in path_wave_data], 'r-')
  print 'Plotting cars'
  for x, y, theta in ekf_est_data[0:-1:10]:
    draw.drawcar(x, y, theta, .5 * 0.257, 1)
  
  pyplot.figure(2)
  print 'PLOT GPS'
  print 'Plotting path'
  pyplot.plot([r[1] for r in gps_data], [r[2] for r in gps_data], 'g-')
  print 'Plotting cars'
  for r in gps_data[0:-1:5]:
    draw.drawcar(r[1], r[2], r[4], .5 * 0.257 / 1e5, 2)
  pyplot.show()
                   np.array([u[:, i]]).T,
                   R,
                   G,
                   g,
                   meas_model['H'],
                   meas_model['h'],
                   T[i],
                   T[i - 1])
    mu[:, i] = ekf_data['mu'].flatten()
    S[:, :, i] = ekf_data['S']

  plt.figure(1)
  # Render cones
  patches = []
  for i in xrange(len(cones)):
    patches.append(Circle((cones[i, 0], cones[i, 1]), d_cone / 2))
  p = PatchCollection(patches, alpha=0.4)
  plt.gca().add_collection(p)
  # Render start position
  plt.plot(x[0, 0], x[1, 0], 'bx', markersize=10, linewidth=2)
  # Render car
  for i, pnt in enumerate(mu.T):
    plt.plot(pnt[0], pnt[1], 'rx', markersize=10, linewidth=2)
    draw.drawcar(pnt[0], pnt[1], pnt[2], 0.25, fig=1, style='k')
    draw.error_ellipse(S[0:2, 0:2, i], (pnt[0], pnt[1]), 0.95, style='g')
  for pnt in x.T:
    draw.drawcar(pnt[0], pnt[1], pnt[2], 0.25, fig=1)
  plt.axis('equal')
  plt.axis([0, 20, -2, 12])
  plt.show()