Пример #1
0
def demo(dataset_name):
    """
    Read data and show true trajectory and trajectory given by dynamics model.
    """
    dataset = Dataset.read(dataset_name)

    # Ground-truth trajectory vs trajectory from dynamics model:
    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111)
    gps_traj = get_ground_truth_traj(dataset)
    ax1.plot(gps_traj[:, 1], gps_traj[:, 2], label='ground')
    dr_traj = get_dead_reckoning_traj(dataset, dynamics)
    ax1.plot(dr_traj[:, 1], dr_traj[:, 2], label='dead-reckoning')
    alt_dr_traj = get_alt_dead_reckoning_traj(dataset, dynamics)
    ax1.plot(alt_dr_traj[:, 1], alt_dr_traj[:, 2], label='alt-dead-reckoning')
    ax1.set_xlim(X_MIN - 1, X_MAX + 1)
    ax1.set_ylim(Y_MIN - 1, Y_MAX + 1)
    ax1.legend()

    # Components of ground-truth trajectory vs my trajectory:
    controls = get_controls(dataset)
    fig2 = plt.figure()
    plot_components(fig2, 'ground', gps_traj, dr_traj, controls)

    plt.show()
Пример #2
0
def demo(dataset_name):
    """
    Read data and show map of laser data for the entire run.
    """
    dataset = Dataset.read(dataset_name)
    fig = plt.figure()
    ax = fig.add_subplot(111)
    draw_map(ax, dataset)
    plt.show()
Пример #3
0
def demo(dataset_name, num_particles):
    """
    Run LocPF on the given dataset.
    """
    seed = 666
    np.random.seed(seed)
    random.seed(seed)

    dataset = Dataset.read(dataset_name)
    pf = LocPF(dataset, num_particles)
    pf.run()
    return pf
Пример #4
0
def test_all_datasets_readable():
    for name in ALL_DATASETS:
        Dataset.read(name)
Пример #5
0
    # Laser rows are (time, laser0, ..., laser360).
    ground_lasers = dataset.get_all_ground_lasers()
    noisy_lasers = dataset.get_all_lasers()
    diffs = noisy_lasers[:, 1:] - ground_lasers[:, 1:]

    # Estimate noise variance, flattened across all angles.
    diffs = diffs.flatten()
    std = np.std(diffs)
    var = std ** 2
    print "lasers var = {}".format(var)

    # Plot estimated noise distributions.
    xs = np.arange(-1, 1, 0.001)
    ys = scipy.stats.norm.pdf(xs, loc=0, scale=std)
    plt.figure()
    plt.plot(xs, ys, color='red')
    plt.hist(diffs, bins=100, normed=True)


if __name__ == "__main__":
    if len(sys.argv) != 2:
        raise RuntimeError("Usage example: {} 1_straight".format(sys.argv[0]))

    dataset = Dataset.read(sys.argv[1])

    # estimate_controls_noise(ground_readings, noisy_readings)
    # estimate_gps_noise(ground_readings, noisy_readings)
    estimate_lasers_noise(dataset)

    plt.show()