pos_truth_se2[:, 0],
chi0,
mu0,
sigma0,
landmarks,
limits=lims,
live=animate)
for i in range(len(odom_t)):
if i == 0:
continue # skip first step because visualizer already has data
t = odom_t[i]
dt = t - odom_t[i - 1]
u_c = vel_odom[:, i:i + 1]
x, z, got_meas = turtlebot.step(t)
pf.predictionStep(u_c, dt)
# skip measurement updates when no commands given to wheels to avoid false convergence
if all(i == 0 for i in u_c):
continue
else:
if got_meas:
pf.correctionStep(z)
viz.update(t, x, pf.chi, pf.mu, pf.sigma, pf.z, got_meas)
viz.plotHistory()
