PF.predict(u, f, f_args=(w, delta_t)) # plot progress if plot_prediction: plot_final_slam_result("Progress for localization using an extended Kalman filter", folder + "pf_progress/step_%d_pred.png" % i, position_at_i[:i + 1], pf_prediction[:i] + [PF.get_prediction()], landmarks, x_min, x_max, PF.particles, None, do_particles=True, do_ensemble=False, path=True, show=False, save=True, slam=False) # after prediction, parse each measurement to update for j in range(len(z)): mes, according_landmark = z[j], landmarks[j] if mes is None: continue PF.update(mes, h, according_landmark) PF.resample() pf_prediction += [PF.get_prediction()] # plot progress if plot_update: plot_final_slam_result("Progress for localization using an extended Kalman filter", folder + "pf_progress/step_%d_up.png" % i, position_at_i[:i+1], pf_prediction[:i+1], landmarks, x_min, x_max, PF.particles, None, do_particles=True, do_ensemble=False, path=True, show=False, save=True, slam=False) if plot_result and do_ex: plot_final_slam_result("Localization using an extended Kalman filter", folder + "exkf_localization.pdf", position_at_i, ex_prediction, landmarks, x_min, x_max, None, None, do_particles=False, do_ensemble=False, path=True, show=False, save=True, slam=False)