def update(self, z):
# TODO implement correction step
self._state.mu = self._state_bar.mu
self._state.Sigma = self._state_bar.Sigma
# observation model:
# Z = [ atan(m_y-mu_y ; m_x-mu_x) - mu_theta ; signature(id) ] # I did not include the signatere for now
# Take the state and covariance from the prediction stage (with bar)
mu_bar = self._state_bar.mu
Sigma_bar = self._state_bar.Sigma
# print('\n\nState before\n',mu_bar)
# FOR EACH OBSERVATION Z_i IN LANDMARK SET DO (recursive approach (superposition also can be aplied, when we just sum the observation contributions at once)):
# we also need to linearize the observation model
# jacobian of observation function wrt to state, evaluated at mu_t (obtained from the prediction step)
H_t = get_jacobian_H(np.squeeze(mu_bar), z)
S_t = H_t.dot(Sigma_bar.dot(H_t.T)) + self._Q
# Kalman gain - specifies the degree with which the measurement is incorporated into the new state estimate
K_t = Sigma_bar.dot(H_t.T).dot(np.linalg.inv(S_t))
z_expected = get_expected_observation(
np.squeeze(mu_bar), z[1]
) # expected from observation model based on the estimated state
# Innovation - is the difference betw actual measurement and the expected measurment (expected is estimated via observation model)
innovation = np.array([[z[0] - z_expected[0]]])
# innovation = wrap_angle(innovation)
# innovation = (z[0] - z_expected[0] + np.pi) % (2 * np.pi) - np.pi
mu_bar = (mu_bar + K_t.dot(innovation))
Sigma_bar = (np.eye(3) - K_t.dot(H_t)).dot(Sigma_bar)
# END for loop
self._state = Gaussian(mu_bar, Sigma_bar)
def get_gaussian_statistics(samples):
"""
Computes the parameters of the samples assuming the samples are part of a Gaussian distribution.
:param samples: The samples of which the Gaussian statistics will be computed (shape: N x 3).
:return: Gaussian object from utils.objects with the mean and covariance initialized.
"""
assert isinstance(samples, np.ndarray)
assert samples.shape[1] == 3
# Compute the mean along the axis of the samples.
mu = np.mean(samples, axis=0)
# Compute mean of angles.
angles = samples[:, 2]
sin_sum = np.sum(np.sin(angles))
cos_sum = np.sum(np.cos(angles))
mu[2] = np.arctan2(sin_sum, cos_sum)
# Compute the samples covariance.
mu_0 = samples - np.tile(mu, (samples.shape[0], 1))
mu_0[:, 2] = np.array([wrap_angle(angle) for angle in mu_0[:, 2]])
Sigma = mu_0.T @ mu_0 / samples.shape[0]
return Gaussian(mu, Sigma)
def predict(self, u):
# TODO Implement here the EKF, perdiction part. HINT: use the auxiliary functions imported above from tools.task
self._state_bar.mu = self.mu[np.newaxis].T
self._state_bar.Sigma = self.Sigma
# print('\nCovariance before\n',self.Sigma)
# print('State before\n',self.mu)
# print('action:\n',u)
# INPUTS (previous belief):
# previous state X_{t-1}: x, y, theta //our best estimate so far, mean
# previous covariance Sigma_{t-1}: 3*3 matrix:
# [sigma_x_x sigma_x_y sigma_x_theta
# sigma_y_x sigma_y_y sigma_y_theta
# sigma_theta_x sigma_theta_y sigma_theta_theta ]
# Currect actions/motions u: drot1, dtran, drot2
# TASK: Updates mu_bar and Sigma_bar after taking a single prediction step after incorporating the control.
# g(u_t,x{t-1}) = A*x_{t-1} + B*u + R # probablu u with noise, R is not present in this project/tast
# x_t = g(u_t,x{t-1}) + N(0,R) (decomposed into two parts: transition function without noise and mapped (from control to the state space) noise)
# Jacobian for linear transformation g(u_t,x{t-1}) = g(u_t, mu_{t-1}) + G * (x_{t-1} - m_{t-1})
G_t = get_jacobian_G(
self.mu, u
) # derivative of transition function wrt to X_{t-1} evaluated at u_t and mu_{t-1}
# Propagate control trough state pose_mean = g(pose_mean, u)
# Predicted belief (belief_bar) without incorporation the measurements z
# the mean is updated using the deterministic version of the state transition function
mu_bar = get_prediction(self.mu, u)
# Covariance for noise in action space
M_t = get_motion_noise_covariance(
u, self._alphas) # has to be mapped into the state space
# Propagate (transform) total covariance
# The transformation from control space to the state space is performed by another linear approximation
# jacobian V_t is needed for this operation - the derivative of the transformation function g wrt motion parameters u, evaluated at u_t and mu_{t-1}
V_t = get_jacobian_V(self.mu, u)
Sigma_bar = G_t.dot(self.Sigma.dot(G_t.T)) + V_t.dot(M_t.dot(
V_t.T)) # final covariance for the propagation step
self._state_bar = Gaussian(mu_bar, Sigma_bar)
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)
# 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)**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 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)