for i in range(t.size):
#stuff for plotting
x_hist.append(state)
mu_hist.append(ekf.mu[:3])
err = state - ekf.mu[:3]
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(ekf.Sigma[0, 0])
y_covar_hist.append(ekf.Sigma[1, 1])
psi_covar_hist.append(ekf.Sigma[2, 2])
Car.animateCar(state, ekf.mu[:2], dead_reckon, ekf.mu[3:],
ekf.Sigma[3:, 3:], ekf.lms_found)
plt.pause(0.02)
state = ekf.propagateState(state, v[i], w[i])
zt, lm_ind = getMeasurements(state)
ekf.update(zt, lm_ind, vc[i], wc[i])
dead_reckon = ekf.propagateState(dead_reckon, vc[i], wc[i])
fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True)
x_hist = np.array(x_hist).T
mu_hist = np.array(mu_hist).T
ax1[0].plot(t, x_hist[0, :], label="Truth")
ax1[0].plot(t, mu_hist[0, :], label="Est")
ax1[0].set_ylabel("x (m)")
ax1[0].legend()
ax1[1].plot(t, x_hist[1, :], label="Truth")
ax1[1].plot(t, mu_hist[1, :], label="Est")
ax1[1].set_ylabel("y (m)")
ax1[1].legend()
