def __init__(self, x=0., y=0., velocity=0., angle=0., acceleration_sigma=0., motion_sigma=0., distance_sigma=0.,
angle_sigma=0., inconsistency_threshold=9., team='West'):
self.x = x
self.y = y
self.velocity = velocity
self.angle = angle
self.acceleration_sigma = acceleration_sigma
self.motion_sigma = motion_sigma
self.distance_sigma = distance_sigma
self.angle_sigma = angle_sigma
self.team = team
if team == 'West':
self.target_goal = ('GoalPostSE', 'GoalPostNE')
elif team == 'East':
self.target_goal = ('GoalPostSW', 'GoalPostNW')
self.num_mapped_markings = 0
self.SLAM = {'self': 0}
self.EKF = EKF(array([[x], [y]]), zeros((2, 2)),
lambda xy, v_and_a: self.transition_means(xy, v_and_a),
lambda xy, v_and_a: self.transition_means_jacobi(xy, v_and_a),
lambda v_and_a: self.transition_covariances(v_and_a),
lambda xy, marking_indices=tuple():
self.observation_means(xy, marking_indices=marking_indices),
lambda xy, marking_indices=tuple():
self.observation_means_jacobi(xy, marking_indices=marking_indices),
lambda d_and_a, marking_indices=tuple():
self.observation_covariances(d_and_a, marking_indices=marking_indices))
self.inconsistency_threshold = inconsistency_threshold
self.lost = False
def __init__(self,
x=0.,
y=0.,
velocity=0.,
angle=0.,
acceleration_sigma=0.,
motion_sigma=0.,
distance_sigma=0.,
angle_sigma=0.,
inconsistency_threshold=9.,
team='West'):
self.x = x
self.y = y
self.velocity = velocity
self.angle = angle
self.acceleration_sigma = acceleration_sigma
self.motion_sigma = motion_sigma
self.distance_sigma = distance_sigma
self.angle_sigma = angle_sigma
self.team = team
if team == 'West':
self.target_goal = ('GoalPostSE', 'GoalPostNE')
elif team == 'East':
self.target_goal = ('GoalPostSW', 'GoalPostNW')
self.num_mapped_markings = 0
self.SLAM = {'self': 0}
self.EKF = EKF(
array([[x], [y]]),
zeros((2, 2)),
lambda xy, v_and_a: self.transition_means(xy, v_and_a),
lambda xy, v_and_a: self.transition_means_jacobi(xy, v_and_a),
lambda v_and_a: self.transition_covariances(v_and_a),
lambda xy, marking_indices=tuple(): self.observation_means(
xy, marking_indices=marking_indices),
lambda xy, marking_indices=tuple(): self.observation_means_jacobi(
xy, marking_indices=marking_indices),
lambda d_and_a, marking_indices=tuple(): self.
observation_covariances(d_and_a, marking_indices=marking_indices))
self.inconsistency_threshold = inconsistency_threshold
self.lost = False
def run(self):
mu0 = array([[-4.0], [-4.0], [pi/2]])
Sigma0 = zeros((3, 3))
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = mu0.T
transition_means_lambda = lambda m, u: m + array([[u[0, 0] * cos(m[2, 0])],
[u[0, 0] * sin(m[2, 0])],
[u[1, 0]]])
transition_means_jacobi_lambda = lambda m, u: array([[1, 0, - u[0, 0] * sin(m[2, 0])],
[0, 1, u[0, 0] * cos(m[2, 0])],
[0, 0, 1]])
transition_covariances_lambda = lambda u: self.R
observation_means_lambda = lambda m: array([[m[0, 0] ** 2 + m[1, 0] ** 2],
[m[2, 0]]])
observation_means_jacobi_lambda = lambda m: array([[2 * m[0, 0], 2 * m[1, 0], 0],
[0, 0, 1]])
observation_covariances_lambda = lambda z: self.Q
self.ekf = EKF(mu0, Sigma0,
transition_means_lambda, transition_means_jacobi_lambda, transition_covariances_lambda,
observation_means_lambda, observation_means_jacobi_lambda, observation_covariances_lambda)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
T = self.U.shape[0]
for t in range(T):
self.ekf.predict(atleast_2d(self.U[t, :]).T)
self.ekf.update(atleast_2d(self.Z[t, :]).T)
self.MU = concatenate((self.MU, self.ekf.means.T))
self.VAR = concatenate((self.VAR, atleast_2d(diag(self.ekf.covariances))))
print('Final Mean State Estimate:')
print(self.ekf.means)
print('Final Covariance State Estimate:')
print(self.ekf.covariances)
class Player:
def __init__(self,
x=0.,
y=0.,
velocity=0.,
angle=0.,
acceleration_sigma=0.,
motion_sigma=0.,
distance_sigma=0.,
angle_sigma=0.,
inconsistency_threshold=9.,
team='West'):
self.x = x
self.y = y
self.velocity = velocity
self.angle = angle
self.acceleration_sigma = acceleration_sigma
self.motion_sigma = motion_sigma
self.distance_sigma = distance_sigma
self.angle_sigma = angle_sigma
self.team = team
if team == 'West':
self.target_goal = ('GoalPostSE', 'GoalPostNE')
elif team == 'East':
self.target_goal = ('GoalPostSW', 'GoalPostNW')
self.num_mapped_markings = 0
self.SLAM = {'self': 0}
self.EKF = EKF(
array([[x], [y]]),
zeros((2, 2)),
lambda xy, v_and_a: self.transition_means(xy, v_and_a),
lambda xy, v_and_a: self.transition_means_jacobi(xy, v_and_a),
lambda v_and_a: self.transition_covariances(v_and_a),
lambda xy, marking_indices=tuple(): self.observation_means(
xy, marking_indices=marking_indices),
lambda xy, marking_indices=tuple(): self.observation_means_jacobi(
xy, marking_indices=marking_indices),
lambda d_and_a, marking_indices=tuple(): self.
observation_covariances(d_and_a, marking_indices=marking_indices))
self.inconsistency_threshold = inconsistency_threshold
self.lost = False
def transition_means(self, current_xy___vector, velocity_and_angle):
conditional_next_xy_means___vector = deepcopy(current_xy___vector)
v, a = velocity_and_angle
conditional_next_xy_means___vector[0, 0] += v * cos(a)
conditional_next_xy_means___vector[1, 0] += v * sin(a)
return conditional_next_xy_means___vector
def transition_means_jacobi(self, all_xy___vector, velocity_and_angle):
return eye(2 * (self.num_mapped_markings + 1))
def transition_covariances(self, velocity_and_angle):
n = 2 * (self.num_mapped_markings + 1)
conditional_next_xy_covariances___matrix = zeros((n, n))
variance = (velocity_and_angle[0] * self.motion_sigma)**2
conditional_next_xy_covariances___matrix[0, 0] = variance
conditional_next_xy_covariances___matrix[1, 1] = variance
return conditional_next_xy_covariances___matrix
def observation_means(self, xy___vector, marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_means___vector = zeros((2 * n_markings, 1))
self_x, self_y = xy___vector[0:2, 0]
for i in range(n_markings):
k = 2 * i
k_marking = 2 * marking_indices[i]
marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
conditional_observation_means___vector[k, 0] = euclidean_distance(
self_x, self_y, marking_x, marking_y)
conditional_observation_means___vector[k + 1, 0] = ray_angle(
self_x, self_y, marking_x, marking_y)
return conditional_observation_means___vector
def observation_means_jacobi(self, xy___vector, marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_means_jacobi___matrix = zeros(
(2 * n_markings, xy___vector.size))
self_x, self_y = xy___vector[0:2, 0]
for i in range(n_markings):
k = 2 * i
k_marking = 2 * marking_indices[i]
marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
conditional_observation_means_jacobi___matrix[k, (0, 1, k_marking, k_marking + 1)] =\
euclidean_distance_gradients(self_x, self_y, marking_x, marking_y)
conditional_observation_means_jacobi___matrix[k + 1, (0, 1, k_marking, k_marking + 1)] =\
ray_angle_gradients(self_x, self_y, marking_x, marking_y)
return conditional_observation_means_jacobi___matrix
def observation_covariances(self,
observations___vector,
marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_covariances___matrix = zeros(
(2 * n_markings, 2 * n_markings))
for i in range(n_markings):
k = 2 * i
conditional_observation_covariances___matrix[
k, k] = self.distance_sigma**2
conditional_observation_covariances___matrix[
k + 1, k + 1] = self.angle_sigma**2
return conditional_observation_covariances___matrix
def accelerate(self, acceleration):
v = self.velocity + acceleration
if v > 0:
self.velocity = v
def run(self):
self.x += self.velocity * (
cos(self.angle) + normal(scale=self.motion_sigma / 2)
) # to avoid over-confidence
self.y += self.velocity * (
sin(self.angle) + normal(scale=self.motion_sigma / 2)
) # to avoid over-confidence
self.EKF.predict((self.velocity, self.angle))
def augment_map(self, marking):
mapping_jacobi = zeros((2, 2 * (self.num_mapped_markings + 1)))
mapping_jacobi[0, 0] = 1.
mapping_jacobi[1, 1] = 1.
self.num_mapped_markings += 1
self.SLAM[marking.id] = self.num_mapped_markings
observed_distance = euclidean_distance(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.distance_sigma / 2) # to avoid over-confidence
distance_minus_1sd = observed_distance - self.distance_sigma
distance_plus_1sd = observed_distance + self.distance_sigma
observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.angle_sigma / 2) # to avoid over-confidence
c = cos(observed_angle)
s = sin(observed_angle)
angle_minus_1sd = observed_angle - self.angle_sigma
c_minus = cos(angle_minus_1sd)
s_minus = sin(angle_minus_1sd)
angle_plus_1sd = observed_angle + self.angle_sigma
c_plus = cos(angle_plus_1sd)
s_plus = sin(angle_plus_1sd)
self_x_mean = self.EKF.means[0, 0]
self_y_mean = self.EKF.means[1, 0]
new_marking_x_mean = self_x_mean + c * observed_distance
new_marking_y_mean = self_y_mean + s * observed_distance
self.EKF.means = vstack(
(self.EKF.means, new_marking_x_mean, new_marking_y_mean))
x_minus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_minus_1sd
y_minus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_minus_1sd
x_minus_distance_1sd_same_angle = self_x_mean + c * distance_minus_1sd
y_minus_distance_1sd_same_angle = self_y_mean + s * distance_minus_1sd
x_minus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_minus_1sd
y_minus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_minus_1sd
x_same_distance_minus_angle_1sd = self_x_mean + c_minus * observed_distance
y_same_distance_minus_angle_1sd = self_y_mean + s_minus * observed_distance
x_same_distance_plus_angle_1sd = self_x_mean + c_plus * observed_distance
y_same_distance_plus_angle_1sd = self_y_mean + s_plus * observed_distance
x_plus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_plus_1sd
y_plus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_plus_1sd
x_plus_distance_1sd_same_angle = self_x_mean + c * distance_plus_1sd
y_plus_distance_1sd_same_angle = self_y_mean + s * distance_plus_1sd
x_plus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_plus_1sd
y_plus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_plus_1sd
x_1sd = array(
(x_minus_distance_1sd_minus_angle_1sd,
x_minus_distance_1sd_same_angle,
x_minus_distance_1sd_plus_angle_1sd,
x_same_distance_minus_angle_1sd, x_same_distance_plus_angle_1sd,
x_plus_distance_1sd_minus_angle_1sd,
x_plus_distance_1sd_same_angle,
x_plus_distance_1sd_plus_angle_1sd))
y_1sd = array(
(y_minus_distance_1sd_minus_angle_1sd,
y_minus_distance_1sd_same_angle,
y_minus_distance_1sd_plus_angle_1sd,
y_same_distance_minus_angle_1sd, y_same_distance_plus_angle_1sd,
y_plus_distance_1sd_minus_angle_1sd,
y_plus_distance_1sd_same_angle,
y_plus_distance_1sd_plus_angle_1sd))
new_marking_x_own_standard_deviation = max(
abs(x_1sd - new_marking_x_mean))
new_marking_x_own_variance = new_marking_x_own_standard_deviation**2
new_marking_y_own_standard_deviation = max(
abs(y_1sd - new_marking_y_mean))
new_marking_y_own_variance = new_marking_y_own_standard_deviation**2
new_marking_xy_own_covariance = c * s * (self.distance_sigma**2)
new_marking_xy_covariance_matrix = mapping_jacobi.dot(self.EKF.covariances).dot(mapping_jacobi.T) +\
array([[new_marking_x_own_variance, new_marking_xy_own_covariance],
[new_marking_xy_own_covariance, new_marking_y_own_variance]])
new_marking_xy_covariance_with_known_xy = mapping_jacobi.dot(
self.EKF.covariances)
self.EKF.covariances =\
vstack((hstack((self.EKF.covariances, new_marking_xy_covariance_with_known_xy.T)),
hstack((new_marking_xy_covariance_with_known_xy, new_marking_xy_covariance_matrix))))
def observe(self, markings, min_distance_over_distance_sigma_ratio=6.):
observations___vector = array([[]]).T
marking_indices = []
for marking in markings:
if marking.id in self.SLAM:
d = euclidean_distance(self.x, self.y, marking.x, marking.y)
if d >= min_distance_over_distance_sigma_ratio * self.distance_sigma:
observed_distance = d + normal(
scale=self.distance_sigma /
2) # to avoid over-confidence
observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.angle_sigma / 2) # to avoid over-confidence
observations___vector = vstack(
(observations___vector, observed_distance,
observed_angle))
marking_indices += [self.SLAM[marking.id]]
if marking_indices:
current_means = deepcopy(self.EKF.means)
self.EKF.update(observations___vector,
marking_indices=marking_indices)
if euclidean_distance(
current_means[0, 0], current_means[1, 0],
self.EKF.means[0, 0],
self.EKF.means[1, 0]) > self.inconsistency_threshold:
self.lost = True
for marking in markings:
if marking.id not in self.SLAM:
self.augment_map(marking)
def observe_marking_in_front(self,
field,
min_distance_over_distance_sigma_ratio=6.):
min_angle = pi
marking_in_front = None
for marking in field.markings:
a = abs(
angular_difference(
self.angle, ray_angle(self.x, self.y, marking.x,
marking.y)))
d = euclidean_distance(self.x, self.y, marking.x, marking.y)
if (a < min_angle) and (d >= min_distance_over_distance_sigma_ratio
* self.distance_sigma):
min_angle = a
marking_in_front = marking
self.observe((marking_in_front, ))
def orient_randomly(self):
self.angle = uniform(-pi, pi)
def orient_toward_ball(self, ball):
self.angle = arctan2(ball.y - self.y, ball.x - self.x)
def distance_to_ball(self, ball):
return ((ball.x - self.x)**2 + (ball.y - self.y)**2)**0.5
def know_goal(self):
return bool(set(self.target_goal) & set(self.SLAM))
def test___WALTER_2015___RoboAI___MidTerm_Q3():
"""test: WALTER (2015) "Planning, Learning & Estimation in Robotics & A.I.": Mid-Term Exam, Question 3"""
means = array([[0.]])
covariances = array([[0.]])
ekf = EKF(means, covariances,
lambda mu, control: mu + control, lambda mu, control: array([[1.]]), lambda control: array([[0.1 ** 2]]),
lambda state: state, lambda state: array([[1.]]), lambda observation: array([[0.5 ** 2]]))
ekf.predict(1.)
print('Predict 1:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_predicted_1 = array([[1.]])
sigma_squared_predicted_1 = array([[.01]])
predicted_1___check = allclose(ekf.means, mu_predicted_1) &\
allclose(ekf.covariances, sigma_squared_predicted_1)
ekf.update(1.2)
print('Update 1:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_updated_1 = array([[1.0077]])
sigma_squared_updated_1 = array([[.0096]])
sigma_updated_1 = array([[.0981]])
updated_1___check = allclose(ekf.means, mu_updated_1) &\
allclose(ekf.covariances, sigma_squared_updated_1, atol=1e-4) &\
allclose(ekf.standard_deviations(), sigma_updated_1, atol=1e-4)
ekf.predict(1.)
print('Predict 2:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_predicted_2 = array([[2.0077]])
sigma_squared_predicted_2 = array([[.0196]]),
predicted_2___check = allclose(ekf.means, mu_predicted_2) &\
allclose(ekf.covariances, sigma_squared_predicted_2, atol=1e-4)
ekf.update(1.5)
print('Update 2:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_updated_2 = array([[1.9708]])
sigma_squared_updated_2 = array([[.0182]])
sigma_updated_2 = array([[.1349]])
updated_2___check = allclose(ekf.means, mu_updated_2, atol=1e-4) &\
allclose(ekf.covariances, sigma_squared_updated_2, atol=1e-4) &\
allclose(ekf.standard_deviations(), sigma_updated_2, atol=1e-4)
ekf.predict(1.)
print('Predict 3:', ekf.means, ekf.covariances, ekf.standard_deviations())
ekf.update(2.5)
print('Update 3:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_updated_3 = array([[2.9231]])
sigma_updated_3 = array([[.1592]])
updated_3___check = allclose(ekf.means, mu_updated_3, atol=1e-4) &\
allclose(ekf.standard_deviations(), sigma_updated_3, atol=1e-4)
ekf.predict(1.)
print('Predict 4:', ekf.means, ekf.covariances, ekf.standard_deviations())
ekf.update(4.5)
print('Update 4:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_updated_4 = array([[3.9945]])
sigma_updated_4 = array([[.1759]])
updated_4___check = allclose(ekf.means, mu_updated_4, atol=1e-4) &\
allclose(ekf.standard_deviations(), sigma_updated_4, atol=1e-4)
ekf.predict(1.)
print('Predict 5:', ekf.means, ekf.covariances, ekf.standard_deviations())
ekf.update(6.)
print('Update 5:', ekf.means, ekf.covariances, ekf.standard_deviations())
mu_updated_5 = array([[5.136]])
sigma_updated_5 = array([[.1876]])
updated_5___check = allclose(ekf.means, mu_updated_5) &\
allclose(ekf.standard_deviations(), sigma_updated_5, atol=1e-5)
assert predicted_1___check & updated_1___check & predicted_2___check & updated_2___check &\
updated_3___check & updated_4___check & updated_5___check
class Player:
def __init__(self, x=0., y=0., velocity=0., angle=0., acceleration_sigma=0., motion_sigma=0., distance_sigma=0.,
angle_sigma=0., inconsistency_threshold=9., team='West'):
self.x = x
self.y = y
self.velocity = velocity
self.angle = angle
self.acceleration_sigma = acceleration_sigma
self.motion_sigma = motion_sigma
self.distance_sigma = distance_sigma
self.angle_sigma = angle_sigma
self.team = team
if team == 'West':
self.target_goal = ('GoalPostSE', 'GoalPostNE')
elif team == 'East':
self.target_goal = ('GoalPostSW', 'GoalPostNW')
self.num_mapped_markings = 0
self.SLAM = {'self': 0}
self.EKF = EKF(array([[x], [y]]), zeros((2, 2)),
lambda xy, v_and_a: self.transition_means(xy, v_and_a),
lambda xy, v_and_a: self.transition_means_jacobi(xy, v_and_a),
lambda v_and_a: self.transition_covariances(v_and_a),
lambda xy, marking_indices=tuple():
self.observation_means(xy, marking_indices=marking_indices),
lambda xy, marking_indices=tuple():
self.observation_means_jacobi(xy, marking_indices=marking_indices),
lambda d_and_a, marking_indices=tuple():
self.observation_covariances(d_and_a, marking_indices=marking_indices))
self.inconsistency_threshold = inconsistency_threshold
self.lost = False
def transition_means(self, current_xy___vector, velocity_and_angle):
conditional_next_xy_means___vector = deepcopy(current_xy___vector)
v, a = velocity_and_angle
conditional_next_xy_means___vector[0, 0] += v * cos(a)
conditional_next_xy_means___vector[1, 0] += v * sin(a)
return conditional_next_xy_means___vector
def transition_means_jacobi(self, all_xy___vector, velocity_and_angle):
return eye(2 * (self.num_mapped_markings + 1))
def transition_covariances(self, velocity_and_angle):
n = 2 * (self.num_mapped_markings + 1)
conditional_next_xy_covariances___matrix = zeros((n, n))
variance = (velocity_and_angle[0] * self.motion_sigma) ** 2
conditional_next_xy_covariances___matrix[0, 0] = variance
conditional_next_xy_covariances___matrix[1, 1] = variance
return conditional_next_xy_covariances___matrix
def observation_means(self, xy___vector, marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_means___vector = zeros((2 * n_markings, 1))
self_x, self_y = xy___vector[0:2, 0]
for i in range(n_markings):
k = 2 * i
k_marking = 2 * marking_indices[i]
marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
conditional_observation_means___vector[k, 0] = euclidean_distance(self_x, self_y, marking_x, marking_y)
conditional_observation_means___vector[k + 1, 0] = ray_angle(self_x, self_y, marking_x, marking_y)
return conditional_observation_means___vector
def observation_means_jacobi(self, xy___vector, marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_means_jacobi___matrix = zeros((2 * n_markings, xy___vector.size))
self_x, self_y = xy___vector[0:2, 0]
for i in range(n_markings):
k = 2 * i
k_marking = 2 * marking_indices[i]
marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
conditional_observation_means_jacobi___matrix[k, (0, 1, k_marking, k_marking + 1)] =\
euclidean_distance_gradients(self_x, self_y, marking_x, marking_y)
conditional_observation_means_jacobi___matrix[k + 1, (0, 1, k_marking, k_marking + 1)] =\
ray_angle_gradients(self_x, self_y, marking_x, marking_y)
return conditional_observation_means_jacobi___matrix
def observation_covariances(self, observations___vector, marking_indices=tuple()):
n_markings = len(marking_indices)
conditional_observation_covariances___matrix = zeros((2 * n_markings, 2 * n_markings))
for i in range(n_markings):
k = 2 * i
conditional_observation_covariances___matrix[k, k] = self.distance_sigma ** 2
conditional_observation_covariances___matrix[k + 1, k + 1] = self.angle_sigma ** 2
return conditional_observation_covariances___matrix
def accelerate(self, acceleration):
v = self.velocity + acceleration
if v > 0:
self.velocity = v
def run(self):
self.x += self.velocity * (cos(self.angle) + normal(scale=self.motion_sigma / 2)) # to avoid over-confidence
self.y += self.velocity * (sin(self.angle) + normal(scale=self.motion_sigma / 2)) # to avoid over-confidence
self.EKF.predict((self.velocity, self.angle))
def augment_map(self, marking):
mapping_jacobi = zeros((2, 2 * (self.num_mapped_markings + 1)))
mapping_jacobi[0, 0] = 1.
mapping_jacobi[1, 1] = 1.
self.num_mapped_markings += 1
self.SLAM[marking.id] = self.num_mapped_markings
observed_distance = euclidean_distance(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.distance_sigma / 2) # to avoid over-confidence
distance_minus_1sd = observed_distance - self.distance_sigma
distance_plus_1sd = observed_distance + self.distance_sigma
observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.angle_sigma / 2) # to avoid over-confidence
c = cos(observed_angle)
s = sin(observed_angle)
angle_minus_1sd = observed_angle - self.angle_sigma
c_minus = cos(angle_minus_1sd)
s_minus = sin(angle_minus_1sd)
angle_plus_1sd = observed_angle + self.angle_sigma
c_plus = cos(angle_plus_1sd)
s_plus = sin(angle_plus_1sd)
self_x_mean = self.EKF.means[0, 0]
self_y_mean = self.EKF.means[1, 0]
new_marking_x_mean = self_x_mean + c * observed_distance
new_marking_y_mean = self_y_mean + s * observed_distance
self.EKF.means = vstack((self.EKF.means,
new_marking_x_mean,
new_marking_y_mean))
x_minus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_minus_1sd
y_minus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_minus_1sd
x_minus_distance_1sd_same_angle = self_x_mean + c * distance_minus_1sd
y_minus_distance_1sd_same_angle = self_y_mean + s * distance_minus_1sd
x_minus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_minus_1sd
y_minus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_minus_1sd
x_same_distance_minus_angle_1sd = self_x_mean + c_minus * observed_distance
y_same_distance_minus_angle_1sd = self_y_mean + s_minus * observed_distance
x_same_distance_plus_angle_1sd = self_x_mean + c_plus * observed_distance
y_same_distance_plus_angle_1sd = self_y_mean + s_plus * observed_distance
x_plus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_plus_1sd
y_plus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_plus_1sd
x_plus_distance_1sd_same_angle = self_x_mean + c * distance_plus_1sd
y_plus_distance_1sd_same_angle = self_y_mean + s * distance_plus_1sd
x_plus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_plus_1sd
y_plus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_plus_1sd
x_1sd = array((x_minus_distance_1sd_minus_angle_1sd, x_minus_distance_1sd_same_angle,
x_minus_distance_1sd_plus_angle_1sd, x_same_distance_minus_angle_1sd,
x_same_distance_plus_angle_1sd, x_plus_distance_1sd_minus_angle_1sd,
x_plus_distance_1sd_same_angle, x_plus_distance_1sd_plus_angle_1sd))
y_1sd = array((y_minus_distance_1sd_minus_angle_1sd, y_minus_distance_1sd_same_angle,
y_minus_distance_1sd_plus_angle_1sd, y_same_distance_minus_angle_1sd,
y_same_distance_plus_angle_1sd, y_plus_distance_1sd_minus_angle_1sd,
y_plus_distance_1sd_same_angle, y_plus_distance_1sd_plus_angle_1sd))
new_marking_x_own_standard_deviation = max(abs(x_1sd - new_marking_x_mean))
new_marking_x_own_variance = new_marking_x_own_standard_deviation ** 2
new_marking_y_own_standard_deviation = max(abs(y_1sd - new_marking_y_mean))
new_marking_y_own_variance = new_marking_y_own_standard_deviation ** 2
new_marking_xy_own_covariance = c * s * (self.distance_sigma ** 2)
new_marking_xy_covariance_matrix = mapping_jacobi.dot(self.EKF.covariances).dot(mapping_jacobi.T) +\
array([[new_marking_x_own_variance, new_marking_xy_own_covariance],
[new_marking_xy_own_covariance, new_marking_y_own_variance]])
new_marking_xy_covariance_with_known_xy = mapping_jacobi.dot(self.EKF.covariances)
self.EKF.covariances =\
vstack((hstack((self.EKF.covariances, new_marking_xy_covariance_with_known_xy.T)),
hstack((new_marking_xy_covariance_with_known_xy, new_marking_xy_covariance_matrix))))
def observe(self, markings, min_distance_over_distance_sigma_ratio=6.):
observations___vector = array([[]]).T
marking_indices = []
for marking in markings:
if marking.id in self.SLAM:
d = euclidean_distance(self.x, self.y, marking.x, marking.y)
if d >= min_distance_over_distance_sigma_ratio * self.distance_sigma:
observed_distance = d + normal(scale=self.distance_sigma / 2) # to avoid over-confidence
observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
normal(scale=self.angle_sigma / 2) # to avoid over-confidence
observations___vector = vstack((observations___vector,
observed_distance,
observed_angle))
marking_indices += [self.SLAM[marking.id]]
if marking_indices:
current_means = deepcopy(self.EKF.means)
self.EKF.update(observations___vector, marking_indices=marking_indices)
if euclidean_distance(current_means[0, 0], current_means[1, 0],
self.EKF.means[0, 0], self.EKF.means[1, 0]) > self.inconsistency_threshold:
self.lost = True
for marking in markings:
if marking.id not in self.SLAM:
self.augment_map(marking)
def observe_marking_in_front(self, field, min_distance_over_distance_sigma_ratio=6.):
min_angle = pi
marking_in_front = None
for marking in field.markings:
a = abs(angular_difference(self.angle, ray_angle(self.x, self.y, marking.x, marking.y)))
d = euclidean_distance(self.x, self.y, marking.x, marking.y)
if (a < min_angle) and (d >= min_distance_over_distance_sigma_ratio * self.distance_sigma):
min_angle = a
marking_in_front = marking
self.observe((marking_in_front,))
def orient_randomly(self):
self.angle = uniform(-pi, pi)
def orient_toward_ball(self, ball):
self.angle = arctan2(ball.y - self.y, ball.x - self.x)
def distance_to_ball(self, ball):
return ((ball.x - self.x) ** 2 + (ball.y - self.y) ** 2) ** 0.5
def know_goal(self):
return bool(set(self.target_goal) & set(self.SLAM))
class RunEKF:
def __init__(self):
self.R = array([[2.0, 0.0, 0.0],
[0.0, 2.0, 0.0],
[0.0, 0.0, radians(2)]])*1e-6 #1E-4
self.Q = array([[1.0, 0.0],[0.0, radians(1)]])*1E-8 #1E-6
self.U = [] # Array that stores the control data where rows increase with time
self.Z = [] # Array that stores the measurement data where rows increase with time
self.XYT = [] # Array that stores the ground truth pose where rows increase with time
self.MU = [] # Array in which to append mu_t as a row vector after each iteration
self.VAR = [] # Array in which to append var(x), var(y), var(theta)
# from the EKF covariance as a row vector after each iteration
# Read in the control and measurement data from their respective text files
# and populates self.U and self.Z
def readData(self, filenameU, filenameZ, filenameXYT):
print("Reading control data from %s and measurement data from %s" % (filenameU, filenameZ))
self.U = loadtxt(filenameU, comments='#', delimiter=',').astype(float)
self.Z = loadtxt(filenameZ, comments='#', delimiter=',').astype(float)
self.XYT = loadtxt(filenameXYT, comments='#', delimiter=',').astype(float)
# Iterate from t=1 to t=T performing the two filtering steps
def run(self):
mu0 = array([[-4.0], [-4.0], [pi/2]])
Sigma0 = zeros((3, 3))
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = mu0.T
transition_means_lambda = lambda m, u: m + array([[u[0, 0] * cos(m[2, 0])],
[u[0, 0] * sin(m[2, 0])],
[u[1, 0]]])
transition_means_jacobi_lambda = lambda m, u: array([[1, 0, - u[0, 0] * sin(m[2, 0])],
[0, 1, u[0, 0] * cos(m[2, 0])],
[0, 0, 1]])
transition_covariances_lambda = lambda u: self.R
observation_means_lambda = lambda m: array([[m[0, 0] ** 2 + m[1, 0] ** 2],
[m[2, 0]]])
observation_means_jacobi_lambda = lambda m: array([[2 * m[0, 0], 2 * m[1, 0], 0],
[0, 0, 1]])
observation_covariances_lambda = lambda z: self.Q
self.ekf = EKF(mu0, Sigma0,
transition_means_lambda, transition_means_jacobi_lambda, transition_covariances_lambda,
observation_means_lambda, observation_means_jacobi_lambda, observation_covariances_lambda)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
T = self.U.shape[0]
for t in range(T):
self.ekf.predict(atleast_2d(self.U[t, :]).T)
self.ekf.update(atleast_2d(self.Z[t, :]).T)
self.MU = concatenate((self.MU, self.ekf.means.T))
self.VAR = concatenate((self.VAR, atleast_2d(diag(self.ekf.covariances))))
print('Final Mean State Estimate:')
print(self.ekf.means)
print('Final Covariance State Estimate:')
print(self.ekf.covariances)
# Plot the resulting estimate for the robot's trajectory
def plot(self):
# Plot the estimated and ground truth trajectories
ground_truth = plt.plot(self.XYT[:, 0], self.XYT[:, 1], 'g.-', label='Ground Truth')
mean_trajectory = plt.plot(self.MU[:, 0], self.MU[:, 1], 'r.-', label='Estimate')
plt.legend()
# Try changing this to different standard deviations
sigma = 1 # 2 or 3
# Plot the errors with error bars
Error = self.XYT-self.MU
T = range(size(self.XYT,0))
f, axarr = plt.subplots(3, sharex=True)
axarr[0].plot(T,Error[:,0],'r-')
axarr[0].plot(T,sigma*sqrt(self.VAR[:,0]),'b--')
axarr[0].plot(T,-sigma*sqrt(self.VAR[:,0]),'b--')
axarr[0].set_title('X error')
axarr[0].set_ylabel('Error (m)')
axarr[1].plot(T,Error[:,1],'r-')
axarr[1].plot(T,sigma*sqrt(self.VAR[:,1]),'b--')
axarr[1].plot(T,-sigma*sqrt(self.VAR[:,1]),'b--')
axarr[1].set_title('Y error')
axarr[1].set_ylabel('Error (m)')
axarr[2].plot(T,degrees(unwrap(Error[:,2])),'r-')
axarr[2].plot(T,sigma*degrees(unwrap(sqrt(self.VAR[:,2]))),'b--')
axarr[2].plot(T,-sigma*degrees(unwrap(sqrt(self.VAR[:,2]))),'b--')
axarr[2].set_title('Theta error (degrees)')
axarr[2].set_ylabel('Error (degrees)')
axarr[2].set_xlabel('Time')
plt.show()