def H(self, x, marker_id):
"""Compute the Jacobian of the observation with respect to the state."""
prev_x, prev_y, prev_theta = x.ravel()
dx = self.MARKER_X_POS[marker_id] - x[0]
dy = self.MARKER_Y_POS[marker_id] - x[1]
h_x = np.asscalar((dy / np.power(dx, 2)) / (1 + np.power(dy / dx, 2)))
h_y = np.asscalar(-(1 / dx) / (1 + np.power(dy / dx, 2)))
return np.array(
[[minimized_angle(h_x),
minimized_angle(h_y),
minimized_angle(-1)]])
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
loc_particles = np.ones((self.particles.shape))
loc_weights = np.ones(self.num_particles) / self.num_particles
for i in range(self.num_particles):
loc_particles[i, :] = env.forward(
self.particles[i, :],
env.sample_noisy_action(u)).ravel() # use noisy input
z_hat = env.observe(loc_particles[i, :], marker_id)
loc_weights[i] = env.likelihood(minimized_angle(z_hat - z),
self.beta) + 1e-300
loc_weights = loc_weights / np.sum(loc_weights)
self.particles, self.weights = self.resample(loc_particles,
loc_weights)
mean, cov = self.mean_and_variance(self.particles)
return mean, cov
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
#prediction
# YOUR IMPLEMENTATION HERE
Gt = env.G(self.mu, u)
Vt = env.V(self.mu, u)
Mt = env.noise_from_motion(u, self.alphas)
mu_t_bar = env.forward(self.mu, u)
Rt = Vt.dot(Mt).dot(Vt.T)
sigma_t_bar = Gt.dot(self.sigma).dot(Gt.T) + Rt
Qt = self.beta
Ht = env.H(mu_t_bar, marker_id)
Kt = sigma_t_bar.dot(Ht.T).dot(
np.linalg.inv(Ht.dot(sigma_t_bar).dot(Ht.T) + Qt))
h_mu_t = env.observe(mu_t_bar, marker_id)
mu_t = mu_t_bar + Kt.dot(minimized_angle(z - h_mu_t))
sigma_t = (np.identity(3) - Kt.dot(Ht)).dot(sigma_t_bar)
self.mu = mu_t
self.sigma = sigma_t
return self.mu, self.sigma
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
partical_cal = np.zeros((self.num_particles, 3))
z_exp = np.zeros(self.num_particles)
weights = np.zeros(self.num_particles)
for i in range(self.num_particles):
u_noise = env.sample_noisy_action(u, alphas=self.alphas).ravel()
partical_cal[i, :] = env.forward(self.particles[i, :],
u_noise).ravel()
z_exp[i] = env.observe(partical_cal[i, :], marker_id)
innovation = minimized_angle(z - z_exp[i])
weights[i] = env.likelihood(innovation, self.beta)
weights += 1.e-300
weights /= sum(weights)
self.particles, self.weights = self.resample(partical_cal, weights)
mean, cov = self.mean_and_variance(self.particles)
return mean, cov
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
# YOUR IMPLEMENTATION HERE
particles = self.particles
weights = self.weights
sum_weights = 0
for i in range(self.num_particles):
noise_act = env.sample_noisy_action(u, self.alphas)
x_bar = env.forward(self.particles[i], noise_act)
particles[i, 0] = x_bar[0]
particles[i, 1] = x_bar[1]
particles[i, 2] = x_bar[2]
inno = minimized_angle(z - env.observe(x_bar, marker_id))
weights[i] = env.likelihood(inno, self.beta)
sum_weights = sum_weights + weights[i]
norm_weights = weights / sum_weights
self.particles = self.resample(particles, norm_weights)
mean, cov = self.mean_and_variance(self.particles)
return mean, cov
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
# YOUR IMPLEMENTATION HERE
# PREDICTION STEP
G = env.G(self.mu, u)
V = env.V(self.mu, u)
M = env.noise_from_motion(u, self.alphas)
predicted_mean = env.forward(self.mu, u)
predicted_sigma = G @ self.sigma @ G.T + V @ M @ V.T
#CORRECTION STEP
predicted_measurement_mean = env.observe(predicted_mean, marker_id)
H = env.H(predicted_mean, marker_id).reshape((1, -1))
pred_measurement_cov = H @ predicted_sigma @ H.T + np.diag(self.beta)
kalamn_gain = predicted_sigma @ H.T @ np.linalg.inv(
pred_measurement_cov)
self.mu = predicted_mean + kalamn_gain @ (
minimized_angle(z - predicted_measurement_mean))
interm = kalamn_gain @ H
self.sigma = (np.eye(interm.shape[0]) - interm) @ predicted_sigma
return self.mu, self.sigma
def observe(self, x, marker_id):
"""Compute observation, given current state and landmark ID.
x: [x, y, theta]
marker_id: int
"""
dx = self.MARKER_X_POS[marker_id] - x[0]
dy = self.MARKER_Y_POS[marker_id] - x[1]
return np.array([minimized_angle(np.arctan2(dy, dx) - x[2])]).reshape(
(-1, 1))
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
# YOUR IMPLEMENTATION HERE
# Update
# print(env.V(self.mu, u))
mean = env.forward(self.mu, u)
sigma = env.G(self.mu, u) @ self.sigma @ env.G(self.mu, u).T + env.V(self.mu, u) @env.noise_from_motion(u, self.alphas) @ env.V(self.mu, u).T
S = env.H(mean, marker_id).dot(sigma.dot(env.H(mean, marker_id).T)) + self.beta
K = sigma.dot(env.H(mean, marker_id).T*np.linalg.inv(S))
self.sigma = (np.identity(3) - K * env.H(mean, marker_id)) @ sigma
self.mu = mean + K * minimized_angle(z - env.observe(mean, marker_id))
self.mu[-1] = minimized_angle(self.mu[-1])
return self.mu, self.sigma
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
# YOUR IMPLEMENTATION HERE
particles = np.zeros((self.num_particles, 3))
weights = np.ones(self.num_particles)
for i in range(self.num_particles):
sigma = env.V(self.particles[i,:], u) @env.noise_from_motion(u, self.alphas) @ env.V(self.particles[i,:], u).T# + env.G(self.particles[i,:], u) @ self.sigma @ env.G(self.particles[i,:], u).T
particles[i,:] = np.random.multivariate_normal(env.forward(self.particles[i,:], u).ravel(), sigma)
particles[i,2] = minimized_angle(particles[i,2])
dz = np.array(minimized_angle(env.observe(particles[i,:].ravel(), marker_id) - z)).reshape(-1, 1)
weights[i] = env.likelihood(dz, self.beta)# * self.weights[i]
weights = weights/np.sum(weights)
self.particles, self.weights = self.resample(particles, weights)
mean, cov = self.mean_and_variance(self.particles)
return mean, cov
def mean_and_variance(self, particles):
"""Compute the mean and covariance matrix for a set of equally-weighted
particles.
particles: (n x 3) matrix of poses
"""
mean = particles.mean(axis=0)
mean[2] = np.arctan2(
np.cos(particles[:, 2]).sum(),
np.sin(particles[:, 2]).sum())
zero_mean = particles - mean
for i in range(zero_mean.shape[0]):
zero_mean[i, 2] = minimized_angle(zero_mean[i, 2])
cov = np.dot(zero_mean.T, zero_mean) / self.num_particles
return mean.reshape((-1, 1)), cov
def forward(self, x, u):
"""Compute next state, given current state and action.
Implements the odometry motion model.
x: [x, y, theta]
u: [rot1, trans, rot2]
"""
prev_x, prev_y, prev_theta = x
rot1, trans, rot2 = u
x_next = np.zeros(x.size)
theta = prev_theta + rot1
x_next[0] = prev_x + trans * np.cos(theta)
x_next[1] = prev_y + trans * np.sin(theta)
x_next[2] = minimized_angle(theta + rot2)
return x_next.reshape((-1, 1))
def update(self, env, u, z, marker_id):
"""Update the state estimate after taking an action and receiving a landmark
observation.
u: action
z: landmark observation
marker_id: landmark ID
"""
# Prediction step:
mu_t_bar = env.forward(self.mu, u)
state_matrix = env.G(self.mu, u)
control_matrix = env.V(self.mu, u)
Motion_noise = env.noise_from_motion(u, self.alphas)
pred_cov = np.dot(np.dot(
state_matrix, self.sigma), state_matrix.T) + np.dot(
np.dot(control_matrix, Motion_noise), control_matrix.T)
# Correction step:
expected_obs = env.observe(mu_t_bar, marker_id)
H_t = env.H(mu_t_bar, marker_id)
Q_t = np.array([[self.beta[0][0]]])
S_t = np.dot(np.dot(H_t, pred_cov), H_t.T) + Q_t
Kalman_gain = np.dot(np.dot(pred_cov, H_t.T), np.linalg.inv(S_t))
self.mu = mu_t_bar + np.dot(Kalman_gain,
(minimized_angle(z - expected_obs)))
self.sigma = np.dot(np.eye(3) - np.dot(Kalman_gain, H_t), pred_cov)
return self.mu, self.sigma
def localize(env, policy, filt, x0, num_steps, plot=False):
# Collect data from an entire rollout
states_noisefree, states_real, action_noisefree, obs_noisefree, obs_real = \
env.rollout(x0, policy, num_steps)
states_filter = np.zeros(states_real.shape)
states_filter[0, :] = x0.ravel()
errors = np.zeros((num_steps, 3))
position_errors = np.zeros(num_steps)
mahalanobis_errors = np.zeros(num_steps)
if plot:
fig = env.get_figure()
for i in range(num_steps):
x_real = states_real[i+1, :].reshape((-1, 1))
u_noisefree = action_noisefree[i, :].reshape((-1, 1))
z_real = obs_real[i, :].reshape((-1, 1))
marker_id = env.get_marker_id(i)
if filt is None:
mean, cov = x_real, np.eye(3)
else:
# filters only know the action and observation
mean, cov = filt.update(env, u_noisefree, z_real, marker_id)
states_filter[i+1, :] = mean.ravel()
if plot:
fig.clear()
plot_field(env, marker_id)
plot_robot(env, x_real, z_real)
plot_path(env, states_noisefree[:i+1, :], 'g', 0.5)
plot_path(env, states_real[:i+1, :], 'b')
if filt is not None:
plot_path(env, states_filter[:i+1, :2], 'r')
fig.canvas.flush_events()
errors[i, :] = (mean - x_real).ravel()
errors[i, 2] = minimized_angle(errors[i, 2])
position_errors[i] = np.linalg.norm(errors[i, :2])
cond_number = np.linalg.cond(cov)
if cond_number > 1e12:
print('Badly conditioned cov (setting to identity):', cond_number)
print(cov)
cov = np.eye(3)
mahalanobis_errors[i] = \
errors[i:i+1, :].dot(np.linalg.inv(cov)).dot(errors[i:i+1, :].T)
mean_position_error = position_errors.mean()
mean_mahalanobis_error = mahalanobis_errors.mean()
anees = mean_mahalanobis_error / 3
if filt is not None:
print('-' * 80)
print('Mean position error:', mean_position_error)
print('Mean Mahalanobis error:', mean_mahalanobis_error)
print('ANEES:', anees)
if plot:
plt.show(block=True)
return position_errors
