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
Exemple #2
0
 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)
Exemple #4
0
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()