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)
if plot_result and do_ens:
