def GetTrueMeasurements(self, robot_pose): ''' Get the true measurements for a robot at robot_pose using a noise-free range sensor. ===INPUT=== robot_pose: of class RobotPose, the pose at which the robot measures ===OUTPUT=== a list of Measurement ''' if (not (isinstance(robot_pose, RobotPose))): raise FeatureBasedWorldException('robot_pose should be an instance of RobotPose.') m = [] for feature in self.features: dx = feature.x - robot_pose.x dy = feature.y - robot_pose.y d = np.sqrt(dx * dx + dy * dy) # true distance phi = wrap_angle(np.arctan2(dy, dx) - robot_pose.theta) # true relative bearing m.append(Measurement(d, phi, feature.s)) return m
def measurement_model(self, measurements, pose): ''' Compute the probability of the particle and the measurements given ===INPUT=== measurement: a list of Measurement pose: the proposed pose ===OUTPUT=== a decimal representing the probability ''' w = 1.0 for m in measurements: j = m.s # assuming the signature gives the correspondence fs = self.true_map.GetFeature(j) delta_x = fs.x - pose.x delta_y = fs.y - pose.y d = np.sqrt(delta_x * delta_x + delta_y * delta_y) b = np.arctan2(delta_y, delta_x) - pose.theta # deviation from expected measurement n_r = d - m.r n_phi = wrap_angle(b - m.phi) n_s = fs.s - m.s # compute probability of measurement. w = w * self.measurement_noise.GetProbability([n_r, n_phi, n_s]) return w
def GetTrueMeasurements(self, robot_pose): ''' Get the true measurements for a robot at robot_pose using a noise-free range sensor. ===INPUT=== robot_pose: of class RobotPose, the pose at which the robot measures ===OUTPUT=== a list of Measurement ''' if (not (isinstance(robot_pose, RobotPose))): raise FeatureBasedWorldException( 'robot_pose should be an instance of RobotPose.') m = [] for feature in self.features: dx = feature.x - robot_pose.x dy = feature.y - robot_pose.y d = np.sqrt(dx * dx + dy * dy) # true distance phi = wrap_angle(np.arctan2(dy, dx) - robot_pose.theta) # true relative bearing m.append(Measurement(d, phi, feature.s)) return m
def measurement_update(self, measurement): rp = self.get_pose() # convert measurement into a matrix z = np.matrix([[measurement.r], \ [measurement.phi], \ [measurement.s]]) # line 10 j = measurement.s # assuming the signature gives the correspondence # line 11 fs = self.true_map.GetFeature(j) delta_x = fs.x - rp.x delta_y = fs.y - rp.y q = delta_x * delta_x + delta_y * delta_y # line 12, predicted measurement z_hat = np.matrix([[np.sqrt(q)], \ [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \ [fs.s]]) # line 13 H_t = np.matrix(np.eye(3)) H_t[0, 0] = -delta_x / np.sqrt(q) H_t[0, 1] = -delta_y / np.sqrt(q) H_t[1, 0] = delta_y / q H_t[1, 1] = delta_x / q H_t[1, 2] = -1.0 # line 14 S = H_t * self.sigma * H_t.T + self.R # line 15 K_t = self.sigma * H_t.T * np.linalg.inv(S) # line 16 self.mu = self.mu + K_t * (z - z_hat) # line 17 self.sigma = self.sigma - K_t * H_t * self.sigma
def measurement_update(self, measurement): ''' update the state and covar estimate using the measurement c.f page 314, Probabilistic Robotics ''' print("<---- measurement_update ---->") print(measurement) rp = self.get_pose() # convert measurement into a matrix z = measurement.ToMatrix() # line 8 j = measurement.s # assuming the signature gives the correspondence print("correspondence = " + str(j)) # line 9 if (not self.feature_seen[j]): f = FeatureState(rp.x + measurement.r * np.cos(measurement.phi + rp.theta), \ rp.y + measurement.r * np.sin(measurement.phi + rp.theta), \ measurement.s) # line 10, initialize feature state self.set_feature(j, f) self.feature_seen[j] = True print("newly observed " + str(f)) fs = self.get_feature(j) # line 12 delta_x = fs.x - rp.x delta_y = fs.y - rp.y # line 13 q = delta_x * delta_x + delta_y * delta_y #~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q)) # line 14, predicted measurement z_hat = np.matrix([[np.sqrt(q)], \ [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \ [fs.s]]) #~ print("z_hat = ") #~ print(z_hat) # line 15 F_xj = self.get_feature_selection_matrix(j) # line 16 h_t_i = np.matrix(np.zeros((3, 6))) h_t_i[0, 0] = -delta_x * np.sqrt(q) h_t_i[0, 1] = -delta_y * np.sqrt(q) h_t_i[0, 3] = delta_x * np.sqrt(q) h_t_i[0, 4] = delta_y * np.sqrt(q) h_t_i[1, 0] = delta_y h_t_i[1, 1] = -delta_x h_t_i[1, 2] = -q h_t_i[1, 3] = -delta_y h_t_i[1, 4] = delta_x h_t_i[2, 5] = q H_t = h_t_i * F_xj / q #~ print("H_t=") #~ print(H_t) # line 17 K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T + self.R) # line 18 self.mu += K_t * (z - z_hat) print("mu=") print(self.mu) # line 19 self.sigma += -K_t * H_t * self.sigma
def linearize(self, u_1t, z_1t, c_1t, mu_0t): ''' Linearize the constraints (neg log likelihoods) around the given state estimate. c.f. p347 ~ p348, Probabilistic Robotics === INPUT === u_1t: a list of RobotAction. The k-th item is the action at step k+1. z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1. c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1. mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state. Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features. === OUTPUT === omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix. ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector. ''' num_states = self.T*3 + 3 + self.N*3 # line 2 omega = np.matrix(np.zeros((num_states, num_states))) ksi = np.matrix(np.zeros((num_states, 1))) # line 3. Fix the origin. omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY]) rp = RobotPose(0.0, 0.0, 0.0) # change this # line 4 for t in range(1, self.T+1): mu_last = self.get_pose(mu_0t, t-1) # robot pose at previous step # line 5 x_t_hat = self.get_pose(mu_0t, t) rp_delta = x_t_hat - mu_last # line 6 G_t = np.matrix(np.eye(3)) G_t[0, 2] = -rp_delta[1, 0] G_t[1, 2] = rp_delta[0, 0] # line 7 tmp = np.matrix(np.zeros((3, 6))) tmp[0:3, 0:3] = -G_t tmp[0:3, 3:6] = np.eye(3) F_t = self.get_motion_update_selection_matrix(t) omega += F_t.T * tmp.T * self.R_inv * tmp * F_t # TODO: find a way to do local update # line 8 ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last) # line 10 for t in range(1, self.T+1): z_t = self.get_measurements(z_1t, t) # measurements at step t c_t = self.get_correspondence(c_1t, t) # correspondences at step t mu_t = self.get_pose(mu_0t, t) # robot pose at step t in matrix form # line 12 for i in range(len(z_t)): z_t_i = z_t[i].ToMatrix() # line 13 j = c_t[i] # if first seen then add feature to state. NOTE: this part is missing in the book if (not self.feature_seen[j]): f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \ mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \ z_t_i[2, 0]) self.set_feature(mu_0t, j, f) self.feature_seen[j] = True print("newly observed " + str(f)) mu_j = self.get_feature(mu_0t, j) # map feature in matrix form # line 14 delta_x = mu_j[0, 0] - mu_t[0, 0] delta_y = mu_j[1, 0] - mu_t[1, 0] # line 15 q = delta_x * delta_x + delta_y * delta_y # line 16 z_t_i_hat = np.matrix([[np.sqrt(q)], \ [wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \ [z_t_i[2, 0]]]) # line 17 tmp = np.matrix(np.zeros((3, 6))) tmp[0, 0] = -delta_x * np.sqrt(q) tmp[0, 1] = -delta_y * np.sqrt(q) tmp[0, 3] = delta_x * np.sqrt(q) tmp[0, 4] = delta_y * np.sqrt(q) tmp[1, 0] = delta_y tmp[1, 1] = -delta_x tmp[1, 2] = -q tmp[1, 3] = -delta_y tmp[1, 4] = delta_x tmp[2, 5] = q H_t_i = tmp / q # line 18 F_xj = self.get_measurement_update_selection_matrix(t, j) omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj # TODO: find a way to do local update # line 19 tmp = np.matrix(np.zeros((6, 1))) tmp[0:3, :] = mu_t tmp[3:6, :] = mu_j ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat + H_t_i * tmp) return omega, ksi
def linearize(self, u_1t, z_1t, c_1t, mu_0t): ''' Linearize the constraints (neg log likelihoods) around the given state estimate. c.f. p347 ~ p348, Probabilistic Robotics === INPUT === u_1t: a list of RobotAction. The k-th item is the action at step k+1. z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1. c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1. mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state. Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features. === OUTPUT === omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix. ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector. ''' num_states = self.T * 3 + 3 + self.N * 3 # line 2 omega = np.matrix(np.zeros((num_states, num_states))) ksi = np.matrix(np.zeros((num_states, 1))) # line 3. Fix the origin. omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY]) rp = RobotPose(0.0, 0.0, 0.0) # change this # line 4 for t in range(1, self.T + 1): mu_last = self.get_pose(mu_0t, t - 1) # robot pose at previous step # line 5 x_t_hat = self.get_pose(mu_0t, t) rp_delta = x_t_hat - mu_last # line 6 G_t = np.matrix(np.eye(3)) G_t[0, 2] = -rp_delta[1, 0] G_t[1, 2] = rp_delta[0, 0] # line 7 tmp = np.matrix(np.zeros((3, 6))) tmp[0:3, 0:3] = -G_t tmp[0:3, 3:6] = np.eye(3) F_t = self.get_motion_update_selection_matrix(t) omega += F_t.T * tmp.T * self.R_inv * tmp * F_t # TODO: find a way to do local update # line 8 ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last) # line 10 for t in range(1, self.T + 1): z_t = self.get_measurements(z_1t, t) # measurements at step t c_t = self.get_correspondence(c_1t, t) # correspondences at step t mu_t = self.get_pose(mu_0t, t) # robot pose at step t in matrix form # line 12 for i in range(len(z_t)): z_t_i = z_t[i].ToMatrix() # line 13 j = c_t[i] # if first seen then add feature to state. NOTE: this part is missing in the book if (not self.feature_seen[j]): f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \ mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \ z_t_i[2, 0]) self.set_feature(mu_0t, j, f) self.feature_seen[j] = True print("newly observed " + str(f)) mu_j = self.get_feature(mu_0t, j) # map feature in matrix form # line 14 delta_x = mu_j[0, 0] - mu_t[0, 0] delta_y = mu_j[1, 0] - mu_t[1, 0] # line 15 q = delta_x * delta_x + delta_y * delta_y # line 16 z_t_i_hat = np.matrix([[np.sqrt(q)], \ [wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \ [z_t_i[2, 0]]]) # line 17 tmp = np.matrix(np.zeros((3, 6))) tmp[0, 0] = -delta_x * np.sqrt(q) tmp[0, 1] = -delta_y * np.sqrt(q) tmp[0, 3] = delta_x * np.sqrt(q) tmp[0, 4] = delta_y * np.sqrt(q) tmp[1, 0] = delta_y tmp[1, 1] = -delta_x tmp[1, 2] = -q tmp[1, 3] = -delta_y tmp[1, 4] = delta_x tmp[2, 5] = q H_t_i = tmp / q # line 18 F_xj = self.get_measurement_update_selection_matrix(t, j) omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj # TODO: find a way to do local update # line 19 tmp = np.matrix(np.zeros((6, 1))) tmp[0:3, :] = mu_t tmp[3:6, :] = mu_j ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat + H_t_i * tmp) return omega, ksi
def measurement_update(self, measurement): """ update the state and covar estimate using the measurement c.f page 314, Probabilistic Robotics """ print("<---- measurement_update ---->") print(measurement) rp = self.get_pose() # convert measurement into a matrix z = measurement.ToMatrix() # line 8 j = measurement.s # assuming the signature gives the correspondence print("correspondence = " + str(j)) # line 9 if not self.feature_seen[j]: f = FeatureState( rp.x + measurement.r * np.cos(measurement.phi + rp.theta), rp.y + measurement.r * np.sin(measurement.phi + rp.theta), measurement.s, ) # line 10, initialize feature state self.set_feature(j, f) self.feature_seen[j] = True print("newly observed " + str(f)) fs = self.get_feature(j) # line 12 delta_x = fs.x - rp.x delta_y = fs.y - rp.y # line 13 q = delta_x * delta_x + delta_y * delta_y # ~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q)) # line 14, predicted measurement z_hat = np.matrix([[np.sqrt(q)], [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], [fs.s]]) # ~ print("z_hat = ") # ~ print(z_hat) # line 15 F_xj = self.get_feature_selection_matrix(j) # line 16 h_t_i = np.matrix(np.zeros((3, 6))) h_t_i[0, 0] = -delta_x * np.sqrt(q) h_t_i[0, 1] = -delta_y * np.sqrt(q) h_t_i[0, 3] = delta_x * np.sqrt(q) h_t_i[0, 4] = delta_y * np.sqrt(q) h_t_i[1, 0] = delta_y h_t_i[1, 1] = -delta_x h_t_i[1, 2] = -q h_t_i[1, 3] = -delta_y h_t_i[1, 4] = delta_x h_t_i[2, 5] = q H_t = h_t_i * F_xj / q # ~ print("H_t=") # ~ print(H_t) # line 17 K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T + self.R) # line 18 self.mu += K_t * (z - z_hat) print("mu=") print(self.mu) # line 19 self.sigma += -K_t * H_t * self.sigma