import math as m
import numpy as n
from numpy.linalg import inv
from numpy.linalg import norm
import matplotlib.pyplot as plt
# Bounds of our playground
xmin = 0
xmax = 3
ymin = 0
ymax = 2
# Base station position in global frame
xb = [0.0, 0.0, 3.0]
yb = [0.0, 2.0, 1.0]
# Simulation settings
# Measure array init (global)
mes = [0.0, 0.0, 0.0]
# Gaussian noise std
sigma = 2*m.pi/180.0 # 2 degrees
# FUNCTION USEFULL FOR THE REAL IMPLEMENTATION
# Function to get angle with a known positon
def angle(x,y,i):
angle = m.atan2(yb[i]-y,xb[i]-x)
return angle
# Line equation crossing base station i with theta angle
def line_equation(i,theta):
# Parameters of y=mx+p returned in a buffer [M,p]
buff = [0,0]
# Slope of the line
buff[0] = m.tan(theta)
# Origin ordinate
buff[1] = yb[i]-buff[0]*xb[i]
return buff
epsilon = 0.3
# Line intersection
def intersection(buff1,buff2):
# If the slopes are too similar
if abs(buff1[0]-buff2[0])