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))
