Probability

 

from elice_utils import EliceUtils

import numpy as np

from generate import generate_data

import matplotlib.pyplot as plt

elice_utils = EliceUtils()

def plt_show():

    plt.savefig("fig")

    elice_utils.send_image("fig.png")

def gaussian_pdf(x, mu, sigma):

    #TODO

    pdf = 1 / np.sqrt(2 * np.pi * sigma ** 2) * np.exp(-((x - mu) ** 2) / (2 * sigma ** 2))

    return pdf

def estimate_robot_position(landmarks, noisy_distances, sigma):

    """

    Estimate the robot's position using Maximum Likelihood Estimation.

   

    Parameters:

    - landmarks: A list of true positions of landmarks.

    - noisy_distances: A list of noisy measurements of distances from the robot to landmarks.

    - sigma: Standard deviation of the noise in distance measurements.

   

    Returns:

    - Estimated position of the robot as [x, y].

    """

    x_space = np.linspace(-5, 5, 200)

    y_space = np.linspace(-5, 5, 200)

    max_likelihood = -np.inf

    best_position = None

    # TODO

    for x in x_space:

        for y in y_space:

            likelihood = 1.0

            for i in range(len(landmarks)):

                distance_prediction = np.sqrt((x - landmarks[i][0])**2 + (y - landmarks[i][1])**2)

                likelihood *= gaussian_pdf(noisy_distances[i], distance_prediction, sigma)

               

            if likelihood > max_likelihood:

                max_likelihood = likelihood

                best_position = [x, y]

    return best_position

def visualize(true_position, estimated_position, landmarks):

    plt.scatter(*true_position, color='red', label="True Position")

    plt.scatter(*estimated_position, color='green', marker='x', label="Estimated Position")

    plt.scatter(landmarks[:, 0], landmarks[:, 1], color='blue', marker='o', label="Landmarks")

    plt.legend()

    plt.xlabel('X-axis')

    plt.ylabel('Y-axis')

    plt.title('True vs. Estimated Robot Position')

    plt.grid(True)

    plt_show()

def main():

    true_position = [1.5, -0.5]

    sigma = 0.5

    landmarks, noisy_distances = generate_data(true_position,5,sigma)

    estimated_position = estimate_robot_position(landmarks, noisy_distances, sigma)

    #TODO

    visualize(true_position, estimated_position, landmarks)

    #elice_utils.send_image('elice.png')

    #elice_utils.send_file('data/input.txt')

if __name__ == "__main__":

    main()