y_vel = np.array(y_vel + np.random.randn(m) * 0.1) measurements = np.vstack((x, y, x_vel, y_vel)) print(measurements.shape) print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(x)) print('You assumed %.2f in R.' % obst.R[0, 0]) # Preallocation for Plotting xt = [] yt = [] for n in range(len(measurements[0])): obst.predict(dt) obst.correct(np.asarray([x[n], y[n], x_vel[n], y_vel[n]]).reshape(4, 1)) x_tmp = obst.x[0] y_tmp = obst.x[1] # Save states for Plotting xt.append(x_tmp) yt.append(y_tmp) fig = plt.figure(figsize=(16, 16)) # plt.scatter(xpd, ypd, s=20, label='predict', c='y') plt.scatter(x, y, s=20, label='Real', c='b') plt.scatter(xt, yt, s=20, label='State', c='k') plt.scatter(xt[0], yt[0], s=100, label='Start', c='g') plt.scatter(xt[-1], yt[-1], s=100, label='Goal', c='r') plt.xlabel('X')
my = np.array(vy + np.random.randn(m)) measurements = np.vstack((x_p, y_p, mx, my)) print(measurements.shape) print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx)) print('You assumed %.2f in R.' % obst.R[0, 0]) # Preallocation for Plotting xt = [] yt = [] for n in range(len(measurements[0])): obst.predict(dt) obst.correct(np.asarray([x_p[n], y_p[n], mx[n], my[n]]).reshape(4, 1)) x = obst.x[0] y = obst.x[1] print obst.x # Save states for Plotting xt.append(x) yt.append(y) fig = plt.figure(figsize=(16, 16)) # plt.scatter(xpd, ypd, s=20, label='predict', c='y') plt.scatter(x_p, y_p, s=20, label='Real', c='b') plt.scatter(xt, yt, s=20, label='State', c='k') plt.scatter(xt[0], yt[0], s=100, label='Start', c='g') plt.scatter(xt[-1], yt[-1], s=100, label='Goal', c='r')