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()
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()
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
def test_all_datasets_readable():
for name in ALL_DATASETS:
Dataset.read(name)
# 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()