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)