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