def predict(self, u):
# TODO Implement here the PF, perdiction part
for i in range(self.num_p):
self.X[i] = sample_from_odometry(self.X[i], u, self._alphas)
self._state_bar = get_gaussian_statistics(self.X)
def predict(self, u):
# TODO Implement here the PF, perdiction part
for i in range(self.M):
self.X[i] = sample_from_odometry(self.X[i], u, self._alphas)
stats = get_gaussian_statistics(self.X)
if not self.global_loc:
self._state_bar.mu = stats.mu
self._state_bar.Sigma = stats.Sigma
def predict(self, u):
# PF, prediction part
for m in range(self.M):
self.X_bar[:, m] = sample_from_odometry(self.X[:, m], u,
self._alphas)
self.w_bar = self.w / sum(self.w) # keep previous weights
updated_pose_bar = pose_from_particles(self.X_bar, self.w_bar)
self._state_bar.mu = updated_pose_bar[np.newaxis].T
self._state_bar.Sigma = get_gaussian_statistics(self.X_bar.T).Sigma
def predict(self, u):
# TODO Implement here the PF, perdiction part
for i in range(self.M):
self.particles[i] = sample_from_odometry(
self.particles[i], u, self._alphas) # many noisy particles
self.X = self.particles
gaussian_parameters = get_gaussian_statistics(
self.particles
) # parameters of the gaussian distribution of particles
self._state_bar.mu = gaussian_parameters.mu
self._state_bar.Sigma = gaussian_parameters.Sigma
def predict(self, u):
# TODO Implement here the PF, perdiction part
self._state_bar.mu = self.mu
self._state_bar.Sigma = self.Sigma
# for particle in self.particle_set:
# print(sample_from_odometry(particle, u, self._alphas))
for idx, particle in enumerate(self.particle_set):
self.particle_set[idx] = sample_from_odometry(
particle, u, self._alphas)
# print('particle_set', self.particle_set)
self._state_bar = get_gaussian_statistics(self.particle_set)
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)
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)
