Пример #1
0
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)
Пример #2
0
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)
Пример #4
0
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)
Пример #5
0
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)
Пример #6
0
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))
Пример #7
0
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)
Пример #8
0
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()
Пример #9
0
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)