def StateVecDiff(self, vec1, vec2): diff = vec1 - vec2 # wrap angle diff[2] = util.angle_wrap(diff[2]) return diff
def FindWallDistance(self, particle, wall, sensorT): ''' Given a particle position, a wall, and a sensor, find distance to the wall Args: particle (E160_Particle): a particle wall ([float x4]): represents endpoint of the wall sensorT: orientation of the sensor on the robot Return: distance to the closest wall (float)''' point1 = util.Vec2(wall[0], wall[1]) point2 = util.Vec2(wall[2], wall[3]) point2 = point2 - point1 # direction vector from point 1 bot = util.Vec2(particle.x, particle.y) # bot vector heading = util.Vec2(math.cos(util.angle_wrap(particle.theta+sensorT)), math.sin(util.angle_wrap(particle.theta+sensorT))) # heading vector # intersection exists if # bot + v*heading = point1 + u*point2Dir # check if lines are parallel if heading.cross(point2) == 0: return None else: v = (point1 - bot).cross(point2) / heading.cross(point2) # bot vector scaling factor u = (bot - point1).cross(heading) / point2.cross(heading) # wall vector scaling factor # check if intersection exists # v > 0 - Robot facing wall # 0 < u < 1 - Intersection in wall if v < 0 or u < 0 or u > 1: return None else: return heading.scale(v).norm()
def Propagate(self, delta_s, delta_theta, i): '''Propagate all the particles from the last state with odometry readings Args: delta_s (float): distance traveled based on odometry delta_theta(float): change in heading based on odometry return: nothing''' state = self.particles[i] delta_x = delta_s * math.cos(state.theta + delta_theta / 2) delta_y = delta_s * math.sin(state.theta + delta_theta / 2) # minus because robot is backwards self.particles[i].set_state(state.x - delta_x, state.y - delta_y, util.angle_wrap(state.theta + delta_theta))
def update_state(self, state, delta_s, delta_theta): delta_x = delta_s * math.cos(state.theta + delta_theta / 2) delta_y = delta_s * math.sin(state.theta + delta_theta / 2) # minus because robot is backwards state.set_state(state.x - delta_x, state.y - delta_y, util.angle_wrap(state.theta + delta_theta)) self.state_error = self.state_des - self.state_est # keep track of previous error if len(self.previous_state_error) < self.MAX_PAST_MEASUREMENTS: self.previous_state_error.append(self.state_error) else: self.previous_state_error.pop(0) self.previous_state_error.append(self.state_error) return state
def build_path(self, node_indices, node_list): '''Update the trajectory using the node indices return by the path planner''' self.trajectory = [] prev_node = node_list[0] self.trajectory.append(E160_state(prev_node.x, prev_node.y, 0)) for i in range(1, len(node_indices)): current_node = node_list[node_indices[i]] prev_node = node_list[node_indices[i - 1]] desired_angle = util.angle_wrap( math.atan2(current_node.y - prev_node.y, current_node.x - prev_node.x)) desired_state = E160_state(current_node.x, current_node.y, desired_angle) self.trajectory.append(desired_state) print("Trajectory Length: ", len(self.trajectory)) print("Trajectory: ", self.trajectory)
def Propagate(self, Xprev, delta_s, delta_theta): '''Propagate all the particles from the last state with odometry readings Args: delta_s (float): distance traveled based on odometry delta_theta(float): change in heading based on odometry return: nothing''' X = [] for i in range(self.numSigPoints): state = Xprev[i] delta_x = delta_s * math.cos(state.theta + delta_theta / 2) delta_y = delta_s * math.sin(state.theta + delta_theta / 2) # minus because robot is backwards X.append( self.SigmaPoint(state.x - delta_x, state.y - delta_y, util.angle_wrap(state.theta + delta_theta), state.mWeight, state.cWeight)) return X
def Propagate(self, Xprev, delta_s, delta_theta): '''Propagate all the particles from the last state with odometry readings Args: delta_s (float): distance traveled based on odometry delta_theta(float): change in heading based on odometry return: nothing''' X = [] for i in range(self.numSigPoints): state = self.SigmaPoint(Xprev[0, i], Xprev[1, i], Xprev[2, i]) delta_x = delta_s * math.cos(state.theta + delta_theta/2) delta_y = delta_s * math.sin(state.theta + delta_theta/2) # minus because robot is backwards X[:, [i]] = Xprev[:, [i]] X[0, i] -= delta_x X[1, i] -= delta_y X[2, i] = util.angle_wrap(X[2, i] + delta_theta) return X
def LocalizeEstWithUKF(self, delta_s, delta_theta, sensor_readings): ''' Localize the robot with Unscented Kalman filters. Call everything Args: delta_s (float): change in distance as calculated by odometry delta_heading (float): change in heading as calcualted by odometry sensor_readings([float, float, float]): sensor readings from range fingers Return: None''' ## [2] generate set of sigma points Xprev = self.GenerateSigmaPoints(self.state, self.covariance) if self.debug: print("\nXprev: \n", Xprev) input() #------------------------------------------------------------------ ## [3] propagate set of sigma points Xbar_star = self.Propagate(Xprev, delta_s, delta_theta) if self.debug: print("\nXbar_star: \n", Xbar_star) input() #------------------------------------------------------------------ ## [4] calculate mu bar muBarVec = np.zeros((self.n, 1)) for i in range(self.numSigPoints): muBarVec += Xbar_star[i].mWeight * Xbar_star[i].toVec() muBarVec[2] = util.angle_wrap(muBarVec[2]) if self.debug: print("\nmuBarVec: \n", muBarVec) input() #------------------------------------------------------------------ ## [5] calculate sigma bar sigmaBar = np.zeros((self.n, self.n)) for i in range(self.numSigPoints): # sigmaBar += Xprev[i].cWeight * np.matmul(self.StateVecDiff(Xbar_star[i].toVec(), muBarVec), np.transpose(self.StateVecDiff(Xbar_star[i].toVec(), muBarVec))) sigmaBar = np.add(Xprev[i].cWeight * np.matmul( self.StateVecDiff(Xbar_star[i].toVec(), muBarVec), np.transpose(self.StateVecDiff(Xbar_star[i].toVec(), muBarVec))), sigmaBar, out=sigmaBar, casting="unsafe") sigmaBar += self.R_t # add additive noise if self.debug: print("\nsigmaBar: \n", sigmaBar) input() #------------------------------------------------------------------ ## [6] new sigma points # convert mubarVec to a sigma point muBar = self.SigmaPoint(muBarVec[0, 0], muBarVec[1, 0], muBarVec[2, 0], Xprev[0].mWeight, Xprev[0].cWeight) Xbar = self.GenerateSigmaPoints(muBar, sigmaBar) if self.debug: print("\nXbar: \n", Xbar) input() #------------------------------------------------------------------ ## [7] compute expected measurements (Z_t) Zbar = self.ComputeExpSensor(Xbar) if self.debug: print("\nZbar: \n", Zbar) input() #------------------------------------------------------------------ ## [8] compute average measurement (z_t) zBarVec = np.zeros((self.n, 1)) for i in range(self.numSigPoints): zBarVec += Xprev[i].mWeight * Zbar[:, [i]] #------------------------------------------------------------------ ## [9] compute expected uncertainty (S_t) s_t = np.zeros( (len(self.sensor_orientation), len(self.sensor_orientation))) for i in range(self.numSigPoints): s_t += Xprev[i].cWeight * np.matmul( (Zbar[:, [i]] - zBarVec), np.transpose(Zbar[:, [i]] - zBarVec)) z_front = sensor_readings[0] z_right = sensor_readings[1] z_left = sensor_readings[2] z_front_adj = (z_front / self.FAR_READING) * 0.8 + 0.05 z_right_adj = (z_right / self.FAR_READING) * 0.8 + 0.05 z_left_adj = (z_left / self.FAR_READING) * 0.8 + 0.05 self.Q_t = np.array([[z_front_adj, 0, 0], [0, z_right_adj, 0], [0, 0, z_left_adj]]) s_t += self.Q_t # add additive sensor noise #------------------------------------------------------------------ ## [10] compute cross coveriance Sigma bar_x,z sigmaBar_XZ = np.zeros((self.n, len(self.sensor_orientation))) for i in range(self.numSigPoints): # sigmaBar_XZ += Xprev[i].cWeight * np.matmul((Xbar[i].toVec() - muBarVec), np.transpose(Zbar[:,[i]] - zBarVec)) # sigmaBar_XZ += Xprev[i].cWeight * np.matmul(self.StateVecDiff(Xprev[i].toVec(), muBarVec), np.transpose(Zbar[:,[i]] - zBarVec)) sigmaBar_XZ = np.add( Xprev[i].cWeight * np.matmul(self.StateVecDiff(Xprev[i].toVec(), muBarVec), np.transpose(Zbar[:, [i]] - zBarVec)), sigmaBar_XZ, out=sigmaBar_XZ, casting="unsafe") #------------------------------------------------------------------ ## [11] compute Kalman Gain (K_t) # K_t = np.zeros((self.n, self.n)) s_tInv = np.linalg.inv(s_t) K_t = np.matmul(sigmaBar_XZ, s_tInv) # print("\n\nKalman Gain:\n", K_t) #------------------------------------------------------------------ ## [12] update state estimation (mu) # convert sensor readings to numpy array z = np.array([[sensor_readings[0]], [sensor_readings[1]], [sensor_readings[2]]]) mu = self.StateVecDiff(muBarVec, -np.matmul(K_t, (z - zBarVec))) #------------------------------------------------------------------ ## [13] update covariance estimation (sigma) sigma = sigmaBar - np.matmul(K_t, np.matmul(s_t, np.transpose(K_t))) #------------------------------------------------------------------ # update state and covariance and save sigma points self.state.set_state(mu[0, 0], mu[1, 0], mu[2, 0]) self.covariance = sigma self.sigmaPoints = Xbar # print("\n Covariance:\n", self.covariance, "\n") return self.state
def __sub__(self, other): return E160_state(self.x - other.x, self.y - other.y, util.angle_wrap(self.theta - other.theta))
def __add__(self, other): return E160_state(self.x + other.x, self.y + other.y, util.angle_wrap(self.theta + other.theta))