for i in range(t.size):
#stuff for plotting
x_hist.append(state)
mu_hist.append(mu)
err = state - mu
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(Sigma[0, 0])
y_covar_hist.append(Sigma[1, 1])
psi_covar_hist.append(Sigma[2, 2])
Car.animateCar(state, mu, dead_reckon, Chi)
plt.pause(0.02)
state = filter.propagateState(state, v[i], w[i])
zt = getMeasurements(state)
mu, Sigma, Chi = filter.update(mu, Sigma, Chi, zt, vc[i], wc[i])
dead_reckon = filter.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()
