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()