def main():
args = get_cli_args()
validate_cli_args(args)
# weights for covariance action noise R and observation noise Q
alphas = np.array(args.alphas) **2 # variance of noise R proportional to alphas, see tools/tasks@get_motion_noise_covariance()
beta = np.deg2rad(args.beta) # see also filters/localization_filter.py
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps, alphas, beta, args.dt)
else:
raise RuntimeError('')
store_sim_data = True if args.output_dir else False
show_plots = True if args.animate else False
write_movie = True if args.movie_file else False
show_trajectory = True if args.animate and args.show_trajectory else False
show_particles = args.show_particles and args.animate and args.filter_name == 'pf'
update_mean_trajectory = True if show_trajectory or store_sim_data else False
update_plots = True if show_plots or write_movie else False
one_trajectory_per_particle = True if show_particles and not store_sim_data else False
if store_sim_data:
if not os.path.exists(args.output_dir):
os.makedirs(args.output_dir)
save_input_data(data, os.path.join(args.output_dir, 'input_data.npy'))
# ---------------------------------------------------------------------------------------------------
# Student's task: You will fill these function inside 'filters/.py'
# ---------------------------------------------------------------------------------------------------
localization_filter = None
if args.filter_name == 'ekf':
localization_filter = EKF(initial_state, alphas, beta)
elif args.filter_name == 'pf':
localization_filter = PF(initial_state, alphas, beta, args.num_particles, args.global_localization)
fig = None
if show_plots or write_movie:
fig = plt.figure(1)
if show_plots:
plt.ion()
# Initialize the trajectory if user opted-in to display.
sim_trajectory = None
if update_mean_trajectory:
if one_trajectory_per_particle:
mean_trajectory = np.zeros((data.num_steps, localization_filter.state_dim, args.num_particles))
else:
mean_trajectory = np.zeros((data.num_steps, localization_filter.state_dim))
sim_trajectory = FilterTrajectory(mean_trajectory)
if store_sim_data:
# Pre-allocate the memory to store the covariance matrix of the trajectory at each time step.
sim_trajectory.covariance = np.zeros((localization_filter.state_dim,
localization_filter.state_dim,
data.num_steps))
# Initialize the movie writer if `--movie-file` was present in the CL args.
movie_writer = None
if write_movie:
get_ff_mpeg_writer = anim.writers['ffmpeg']
metadata = dict(title='Localization Filter', artist='matplotlib', comment='PS2')
movie_fps = min(args.movie_fps, float(1. / args.plot_pause_len))
movie_writer = get_ff_mpeg_writer(fps=movie_fps, metadata=metadata)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
with movie_writer.saving(fig, args.movie_file, data.num_steps) if write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
localization_filter.predict(u)
localization_filter.update(z)
if update_mean_trajectory:
if one_trajectory_per_particle:
sim_trajectory.mean[t, :, :] = localization_filter.X.T
else:
sim_trajectory.mean[t] = localization_filter.mu
if store_sim_data:
sim_trajectory.covariance[:, :, t] = localization_filter.Sigma
progress_bar.next()
if not update_plots:
continue
plt.cla()
plot_field(z[1])
plot_robot(data.debug.real_robot_path[t])
plot_observation(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'g')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'm')
#plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*g')
plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*m')
if show_particles:
samples = localization_filter.X.T
plt.scatter(samples[0], samples[1], s=2)
else:
plot2dcov(localization_filter.mu_bar[:-1],
localization_filter.Sigma_bar[:-1, :-1],
'red', 3,
legend='{} -'.format(args.filter_name.upper()))
plot2dcov(localization_filter.mu[:-1],
localization_filter.Sigma[:-1, :-1],
'blue', 3,
legend='{} +'.format(args.filter_name.upper()))
plt.legend()
if show_trajectory:
if len(sim_trajectory.mean.shape) > 2:
# This means that we probably intend to show the trajectory for ever particle.
x = np.squeeze(sim_trajectory.mean[0:t, 0, :])
y = np.squeeze(sim_trajectory.mean[0:t, 1, :])
plt.plot(x, y)
else:
plt.plot(sim_trajectory.mean[0:t, 0], sim_trajectory.mean[0:t, 1], 'blue')
if show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if write_movie:
movie_writer.grab_frame()
progress_bar.finish()
if show_plots:
plt.show(block=True)
if store_sim_data:
file_path = os.path.join(args.output_dir, 'output_data.npy')
with open(file_path, 'wb') as data_file:
np.savez(data_file,
mean_trajectory=sim_trajectory.mean,
covariance_trajectory=sim_trajectory.covariance)
def main():
args = get_cli_args()
validate_cli_args(args)
alphas = np.array(args.alphas)
beta = np.array(args.beta)
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps,
args.num_landmarks_per_side,
args.max_obs_per_time_step, alphas, beta,
args.dt)
else:
raise RuntimeError('')
store_sim_data = True if args.output_dir else False
should_show_plots = True if args.animate else False
should_write_movie = True if args.movie_file else False
should_update_plots = True if should_show_plots or should_write_movie else False
field_map = FieldMap(args.num_landmarks_per_side)
fig = get_plots_figure(should_show_plots, should_write_movie)
movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM',
args.movie_fps, args.plot_pause_len)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
if store_sim_data:
if not os.path.exists(args.output_dir):
os.makedirs(args.output_dir)
save_input_data(data, os.path.join(args.output_dir, 'input_data.npy'))
# slam object initialization
slam = EKF_SLAM('ekf', 'known', 'batch', args, initial_state)
mu_traj = mean_prior
sigma_traj = []
theta = []
with movie_writer.saving(
fig, args.movie_file,
data.num_steps) if should_write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
# TODO SLAM predict(u)
mu, Sigma = slam.predict(u)
# TODO SLAM update
mu, Sigma = slam.update(z)
mu_traj = np.vstack((mu_traj, mu[:3]))
sigma_traj.append(Sigma[:3, :3])
theta.append(mu[2])
progress_bar.next()
if not should_update_plots:
continue
plt.cla()
plot_field(field_map, z)
plot_robot(data.debug.real_robot_path[t])
plot_observations(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0],
data.debug.real_robot_path[1:tp1, 1], 'm')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0],
data.debug.noise_free_robot_path[1:tp1, 1], 'g')
plt.plot([data.debug.real_robot_path[t, 0]],
[data.debug.real_robot_path[t, 1]], '*r')
plt.plot([data.debug.noise_free_robot_path[t, 0]],
[data.debug.noise_free_robot_path[t, 1]], '*g')
# TODO plot SLAM solution
# robot filtered trajectory and covariance
plt.plot(mu_traj[:, 0], mu_traj[:, 1], 'blue')
plot2dcov(mu[:2], Sigma[:2, :2], color='b', nSigma=3, legend=None)
# landmarks covariances and expected poses
Sm = slam.Sigma[slam.iR:slam.iR + slam.iM,
slam.iR:slam.iR + slam.iM]
mu_M = slam.mu[slam.iR:]
for c in range(0, slam.iM, 2):
Sigma_lm = Sm[c:c + 2, c:c + 2]
mu_lm = mu_M[c:c + 2]
plt.plot(mu_lm[0], mu_lm[1], 'ro')
plot2dcov(mu_lm, Sigma_lm, color='k', nSigma=3, legend=None)
if should_show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if should_write_movie:
movie_writer.grab_frame()
progress_bar.finish()
# plt.figure(2)
# plt.plot(theta)
plt.show(block=True)
if store_sim_data:
file_path = os.path.join(args.output_dir, 'output_data.npy')
with open(file_path, 'wb') as data_file:
np.savez(data_file,
mean_trajectory=mu_traj,
covariance_trajectory=np.array(sigma_traj))
def main():
args = get_cli_args()
validate_cli_args(args)
alphas = np.array(args.alphas)
beta = np.array(args.beta)
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps,
args.num_landmarks_per_side,
args.max_obs_per_time_step, alphas, beta,
args.dt)
else:
raise RuntimeError('')
should_show_plots = True if args.animate else False
should_write_movie = True if args.movie_file else False
should_update_plots = True if should_show_plots or should_write_movie else False
field_map = FieldMap(args.num_landmarks_per_side)
fig_robot = get_plots_figure(should_show_plots, should_write_movie)
movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM',
args.movie_fps, args.plot_pause_len)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
# sam object init:
sam = SAM(initial_state, args)
mu_traj = np.array([None, None])
theta = []
with movie_writer.saving(
fig_robot, args.movie_file,
data.num_steps) if should_write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# for t in range(50):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
# TODO SLAM predict(u)
mu, Sigma = sam.predict(u)
# TODO SLAM update
mu, Sigma = sam.update(u, z)
mu_traj = np.vstack((mu_traj, mu[:2]))
theta.append(mu[2])
progress_bar.next()
if not should_update_plots:
continue
plt.figure(1)
plt.cla()
plot_field(field_map, z)
plot_robot(data.debug.real_robot_path[t])
plot_observations(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0],
data.debug.real_robot_path[1:tp1, 1], 'm')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0],
data.debug.noise_free_robot_path[1:tp1, 1], 'g')
plt.plot([data.debug.real_robot_path[t, 0]],
[data.debug.real_robot_path[t, 1]], '*r')
plt.plot([data.debug.noise_free_robot_path[t, 0]],
[data.debug.noise_free_robot_path[t, 1]], '*g')
# TODO plot SLAM solution
# robot filtered trajectory and covariance
plt.plot(mu_traj[:, 0], mu_traj[:, 1], 'blue')
plot2dcov(mu[:2], Sigma[:2, :2], color='b', nSigma=3, legend=None)
plt.figure(2, figsize=(8, 6))
plt.cla()
plt.spy(sam.A, marker='o', markersize=5)
if should_show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if should_write_movie:
movie_writer.grab_frame()
progress_bar.finish()
plt.show()
def main():
args = get_cli_args()
validate_cli_args(args)
alphas = np.array(args.alphas)**2
beta = np.array(args.beta)
beta[1] = np.deg2rad(beta[1])
Q = np.array([[beta[0]**2, 0], [0, beta[1]**2]])
filter_name = args.filter_name
DATA_ASSOCIATION = args.data_association
UPDATE_TYPE = args.update_type
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
# print(initial_state)
SAM_MODEL = Sam(initial_state=initial_state,
alphas=alphas,
slam_type=filter_name,
data_association=DATA_ASSOCIATION,
update_type=UPDATE_TYPE,
Q=Q)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps,
args.num_landmarks_per_side,
args.max_obs_per_time_step, alphas, beta,
args.dt)
else:
raise RuntimeError('')
should_show_plots = True if args.animate else False
should_write_movie = True if args.movie_file else False
should_update_plots = True if should_show_plots or should_write_movie else False
field_map = FieldMap(args.num_landmarks_per_side)
fig = get_plots_figure(should_show_plots, should_write_movie)
movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM',
args.movie_fps, args.plot_pause_len)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
with movie_writer.saving(
fig, args.movie_file,
data.num_steps) if should_write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
# TODO SLAM predict(u)
SAM_MODEL.predict(u)
# TODO SLAM update
SAM_MODEL.update(z)
# SAM_MODEL.solve()
progress_bar.next()
if not should_update_plots:
continue
plt.cla()
plot_field(field_map, z)
plot_robot(data.debug.real_robot_path[t])
plot_observations(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0],
data.debug.real_robot_path[1:tp1, 1], 'm')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0],
data.debug.noise_free_robot_path[1:tp1, 1], 'g')
plt.plot([data.debug.real_robot_path[t, 0]],
[data.debug.real_robot_path[t, 1]], '*r')
plt.plot([data.debug.noise_free_robot_path[t, 0]],
[data.debug.noise_free_robot_path[t, 1]], '*g')
# TODO plot SLAM solution
for i in SAM_MODEL.LEHRBUCH.keys():
Coord = SAM_MODEL.graph.get_estimated_state()[
SAM_MODEL.LEHRBUCH[i]]
plt.plot(Coord[0], Coord[1], 'g*', markersize=7.0)
S = SAM_MODEL.graph.get_estimated_state()
states_results_x = []
states_results_y = []
for i in range(len(S)):
if i not in SAM_MODEL.LEHRBUCH.values():
states_results_x.append(S[i][0][0])
states_results_y.append(S[i][1][0])
plt.plot(states_results_x, states_results_y, 'b')
plt.plot(states_results_x[-1],
states_results_y[-1],
'bo',
markersize=3.0)
if should_show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if should_write_movie:
movie_writer.grab_frame()
# chi2var = SAM_MODEL.graph.chi2()
# i = 0
# error_var = 1
# print('\n')
# while error_var >= 0.5 and i <= 100:
# # print('Error equals ={}, for {} iteration'.format(chi2var,i))
# SAM_MODEL.graph.solve(mrob.GN)
# chi4var = SAM_MODEL.graph.chi2()
# error_var = abs(chi4var - chi2var)
# chi2var = chi4var
# i += 1
# print('Error ={}, Iter = {}'.format(chi2var,i))
#______________________________________________________________________
SAM_MODEL.graph.solve(mrob.LM)
print(SAM_MODEL.graph.chi2())
progress_bar.finish()
COV = inv(SAM_MODEL.graph.get_information_matrix())[-3:-1, -3:-1]
plot2dcov(np.array([states_results_x[-1], states_results_y[-1]]).T,
COV.A,
'k',
nSigma=3)
plt.show(block=True)
# plt.figure(figsize=(10,10))
# plt.plot(SAM_MODEL.ci2)
# plt.grid('on')
# plt.xlabel('T')
# plt.ylabel('Estimation')
# plt.title('Plot chi2')
# plt.show(block=True)
plt.figure(figsize=(8, 8))
plt.spy(SAM_MODEL.graph.get_adjacency_matrix(),
marker='o',
markersize=2.0,
color='g')
plt.title('GAM')
plt.show(block=True)
plt.figure(figsize=(8, 8))
plt.spy(SAM_MODEL.graph.get_information_matrix(),
marker='o',
markersize=2.0,
color='g')
plt.title('GIM')
plt.show(block=True)
def main():
args = get_cli_args()
validate_cli_args(args)
alphas = np.array(args.alphas)**2 # should the square operation be done?
beta = np.array(args.beta)
beta[1] = np.deg2rad(beta[1])
Q = np.diag([*(beta**2)])
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps,
args.num_landmarks_per_side,
args.max_obs_per_time_step, alphas, beta,
args.dt)
else:
raise RuntimeError('')
# SAM filtering set
slam_filter = None
if args.filter_name == 'sam':
slam_filter = SAM('sam', 'known', 'batch', initial_state, alphas, Q)
should_show_plots = True if args.animate else False
should_write_movie = True if args.movie_file else False
should_update_plots = True if should_show_plots or should_write_movie else False
field_map = FieldMap(args.num_landmarks_per_side)
fig = get_plots_figure(should_show_plots, should_write_movie)
movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM',
args.movie_fps, args.plot_pause_len)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
with movie_writer.saving(
fig, args.movie_file,
data.num_steps) if should_write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
# TODO SLAM predict(u)
slam_filter.predict(u)
# TODO SLAM update
slam_filter.update(z)
progress_bar.next()
if not should_update_plots:
continue
plt.cla()
plot_field(field_map, z)
plot_robot(data.debug.real_robot_path[t])
plot_observations(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0],
data.debug.real_robot_path[1:tp1, 1], 'm')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0],
data.debug.noise_free_robot_path[1:tp1, 1], 'g')
plt.plot([data.debug.real_robot_path[t, 0]],
[data.debug.real_robot_path[t, 1]], '*r')
plt.plot([data.debug.noise_free_robot_path[t, 0]],
[data.debug.noise_free_robot_path[t, 1]], '*g')
sim_trajectory_mean_x = np.zeros((len(slam_filter.mu_est) // 3))
sim_trajectory_mean_y = np.zeros((len(slam_filter.mu_est) // 3))
# TODO plot SLAM soltion
for i in range(0, len(slam_filter.mu_est), 3):
sim_trajectory_mean_x[i // 3] = slam_filter.mu_est[i]
sim_trajectory_mean_y[i // 3] = slam_filter.mu_est[i + 1]
plt.plot(sim_trajectory_mean_x, sim_trajectory_mean_y, 'blue')
for i in range(0, len(slam_filter.ld_est), 2):
plot2dcov(slam_filter.ld_est[i:i + 2],
slam_filter.Sigma_ld[:, :, i // 2], 'green', 3)
if tp1 == 1:
print('\nA for t=1:\n', np.round(slam_filter.A, decimals=1))
plot2dcov(slam_filter.mu_est[-3:-1],
slam_filter.Sigma[:-1, :-1, -1],
'blue',
3,
legend='{} +'.format(args.filter_name.upper()))
plt.legend()
if should_show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if should_write_movie:
movie_writer.grab_frame()
progress_bar.finish()
plt.show(block=True)
# file_path = os.path.join(args.output_dir, 'output_data.npy')
# with open(file_path, 'wb') as data_file:
# np.savez(data_file,
# chi=slam_filter.chi)
import pickle
with open("output_data.txt", "wb") as fp: # Pickling
pickle.dump(slam_filter.chi, fp)