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)**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('')
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)
data = load_data("slam-evaluation-input.npy")
slam = SAM(beta, alphas, initial_state)
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]
# print(data.filter.observations.shape)
slam.predict(u)
trajectory, landmarks = slam.update(z)
progress_bar.next()
if not should_update_plots:
continue
plt.cla()
plot_field(field_map, z, slam.lm_positions,
slam.lm_correspondences)
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 soltion
plt.plot(np.array(trajectory)[:, 0], np.array(trajectory)[:, 1])
plt.scatter(np.array(landmarks)[:, 0], np.array(landmarks)[:, 1])
# print(t)
# for lm in slam.lm_positions:
# # print(len(lm))
# if len(lm)>5:
# lm_mu, lm_sigma = get_gaussian_statistics_xy(np.array(lm[-5:]))
# # print('lm_mu',lm_mu)
# # print('lm_sigma',lm_sigma)
# # print('plot lm')
# plot2dcov(lm_mu, lm_sigma, 3, 50)
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)
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])
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 = 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)
# TODO SLAM update
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
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)
def generate_data(initial_pose,
num_steps,
num_landmarks_per_side,
max_obs_per_time_step,
alphas,
beta,
dt,
animate=False,
plot_pause_s=0.01):
"""
Generates the trajectory of the robot using square path given by `generate_motion`.
:param initial_pose: The initial pose of the robot in the field (format: np.array([x, y, theta])).
:param num_steps: The number of time steps to generate the path for.
:param num_landmarks_per_side: The number of landmarks to use on one side of the field.
:param max_obs_per_time_step: The maximum number of observations to generate per time step of the sim.
:param alphas: The noise parameters of the control actions (format: np.array([a1, a2, a3, a4])).
:param beta: The noise parameter of observations (format: np.array([range (cm), bearing (deg)])).
:param dt: The time difference (in seconds) between two consecutive time steps.
:param animate: If True, this function will animate the generated data in a plot.
:param plot_pause_s: The time (in seconds) to pause the plot animation between two consecutive frames.
:return: SimulationData object.
"""
# Initializations
# State format: [x, y, theta]
state_dim = 3
# Motion format: [drot1, dtran, drot2]
motion_dim = 3
# Observation format: [range (cm, float),
# bearing (rad, float),
# landmark_id (id, int)]
observation_dim = 3
if animate:
plt.figure(1)
plt.ion()
data_length = num_steps + 1
filter_data = SlamInputData(np.zeros((data_length, motion_dim)),
np.empty((data_length, max_obs_per_time_step, observation_dim)))
debug_data = SlamDebugData(np.zeros((data_length, state_dim)),
np.zeros((data_length, state_dim)),
np.empty((data_length, max_obs_per_time_step, observation_dim)))
filter_data.observations[:] = np.nan
debug_data.noise_free_observations[:] = np.nan
debug_data.real_robot_path[0] = initial_pose
debug_data.noise_free_robot_path[0] = initial_pose
field_map = FieldMap(num_landmarks_per_side)
# Covariance of observation noise.
alphas = alphas ** 2
beta = np.array(beta)
beta[1] = np.deg2rad(beta[1])
Q = np.diag([*(beta ** 2), 0])
for i in range(1, data_length):
# Simulate Motion
# Noise-free robot motion command.
t = i * dt
filter_data.motion_commands[i] = generate_motion(t, dt)
# Noise-free robot pose.
debug_data.noise_free_robot_path[i] = \
sample_from_odometry(debug_data.noise_free_robot_path[i - 1],
filter_data.motion_commands[i],
[0, 0, 0, 0])
# Move the robot based on the noisy motion command execution.
debug_data.real_robot_path[i] = sample_from_odometry(debug_data.real_robot_path[i - 1],
filter_data.motion_commands[i],
alphas)
# Simulate Observation
noise_free_observations = sense_landmarks(debug_data.real_robot_path[i], field_map, max_obs_per_time_step)
noisy_observations = np.empty(noise_free_observations.shape)
noisy_observations[:] = np.nan
num_observations = noise_free_observations.shape[0]
for k in range(num_observations):
# Generate observation noise.
observation_noise = sample2d(np.zeros(observation_dim), Q)
# Generate noisy observation as observed by the robot for the filter.
noisy_observations[k] = noise_free_observations[k] + observation_noise
if noisy_observations.shape == (0, 3):
print('hello')
filter_data.observations[i] = noisy_observations
debug_data.noise_free_observations[i] = noise_free_observations
if animate:
plt.clf()
plot_field(field_map, noise_free_observations[:, 2])
plot_robot(debug_data.real_robot_path[i])
plot_observations(debug_data.real_robot_path[i],
debug_data.noise_free_observations[i],
filter_data.observations[i])
plt.plot(debug_data.real_robot_path[1:i, 0], debug_data.real_robot_path[1:i, 1], 'b')
plt.plot(debug_data.noise_free_robot_path[1:i, 0], debug_data.noise_free_robot_path[1:i, 1], 'g')
plt.draw()
plt.pause(plot_pause_s)
if animate:
plt.show(block=True)
# This only initializes the sim data with everything but the first entry (which is just the prior for the sim).
filter_data.motion_commands = filter_data.motion_commands[1:]
filter_data.observations = filter_data.observations[1:]
debug_data.real_robot_path = debug_data.real_robot_path[1:]
debug_data.noise_free_robot_path = debug_data.noise_free_robot_path[1:]
debug_data.noise_free_observations = debug_data.noise_free_observations[1:]
return SimulationData(num_steps, filter_data, debug_data)
def generate_data(initial_pose,
num_steps,
alphas,
beta,
dt,
animate=False,
plot_pause_s=0.01):
"""
Generates the trajectory of the robot using square path given by `generate_motion`.
:param initial_pose: The initial pose of the robot in the field (format: np.array([x, y, theta])).
:param num_steps: The number of time steps to generate the path for.
:param alphas: The noise parameters of the control actions (format: np.array([a1, a2, a3, a4])).
:param beta: The noise parameter of observations.
:param dt: The time difference (in seconds) between two consecutive time steps.
:param animate: If True, this function will animate the generated data in a plot.
:param plot_pause_s: The time (in seconds) to pause the plot animation between two consecutive frames.
:return: SimulationData object.
"""
# Initializations
# State format: [x, y, theta]
state_dim = 3
# Motion format: [drot1, dtran, drot2]
motion_dim = 3
# Observation format: [bearing, marker_id]
observation_dim = 2
if animate:
plt.figure(1)
plt.ion()
data_length = num_steps + 1
filter_data = FilterInputData(np.zeros((data_length, motion_dim)),
np.zeros((data_length, observation_dim)))
debug_data = FilterDebugData(np.zeros((data_length, state_dim)),
np.zeros((data_length, state_dim)),
np.zeros((data_length, observation_dim)))
debug_data.real_robot_path[0] = initial_pose
debug_data.noise_free_robot_path[0] = initial_pose
field_map = FieldMap()
# Covariance of observation noise.
Q = np.diag([beta**2, 0])
for i in range(1, data_length):
# Simulate Motion
# Noise-free robot motion command.
t = i * dt
filter_data.motion_commands[i, :] = generate_motion(t, dt)
# Noise-free robot pose.
debug_data.noise_free_robot_path[i, :] = \
sample_from_odometry(debug_data.noise_free_robot_path[i - 1],
filter_data.motion_commands[i],
np.array([0, 0, 0, 0]))
# Move the robot based on the noisy motion command execution.
debug_data.real_robot_path[i, :] = \
sample_from_odometry(debug_data.real_robot_path[i - 1],
filter_data.motion_commands[i],
alphas)
# Simulate Observation
# (n / 2) causes each landmark to be viewed twice.
lm_id = int(np.mod(np.floor(i / 2), field_map.num_landmarks))
debug_data.noise_free_observations[i, :] = \
get_observation(debug_data.real_robot_path[i], lm_id)
# Generate observation noise.
observation_noise = sample2d(np.zeros(observation_dim), Q)
# Generate noisy observation as observed by the robot for the filter.
filter_data.observations[
i, :] = debug_data.noise_free_observations[i] + observation_noise
if animate:
plt.clf()
plot_field(lm_id)
plot_robot(debug_data.real_robot_path[i])
plot_observation(debug_data.real_robot_path[i],
debug_data.noise_free_observations[i],
filter_data.observations[i])
plt.plot(debug_data.real_robot_path[1:i, 0],
debug_data.real_robot_path[1:i, 1], 'b')
plt.plot(debug_data.noise_free_robot_path[1:i, 0],
debug_data.noise_free_robot_path[1:i, 1], 'g')
plt.draw()
plt.pause(plot_pause_s)
if animate:
plt.show(block=True)
# This only initializes the sim data with everything but the first entry (which is just the prior for the sim).
filter_data.motion_commands = filter_data.motion_commands[1:]
filter_data.observations = filter_data.observations[1:]
debug_data.real_robot_path = debug_data.real_robot_path[1:]
debug_data.noise_free_robot_path = debug_data.noise_free_robot_path[1:]
debug_data.noise_free_observations = debug_data.noise_free_observations[1:]
return SimulationData(num_steps, filter_data, debug_data)
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)**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)
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 # 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)
