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