x_vel = np.array(x_vel + np.random.randn(m) * 0.1)
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')
