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()