<jupyter_start><jupyter_text>%%HTML                   Devloppement du robot TwIRTee        Tuteur :Auteur :    <NAME>     Simulation du modèle de positionnement par trilatération<jupyter_code>import matplotlib.pyplot as plt
from numpy import *
from math import pi
from random import *




rate=0.05 #random function to mesured cercles rayons 
max=10 # x,y parametre "t" in robotMove 
npoints=20 # robot's number of steps<jupyter_output><empty_output><jupyter_text>- La fonction anchorsPosition trace l'emplacement des beacons dans le cas de 4 émetteurs en positionnement carré [0,0] [0,1] [1,1] et [1,0] TODO:	Rendre la fonction adaptable en nombre d'emetteurs, en suivant les methodes de placement évoqué dans "Roa et al. - 2007 - Optimal placement of sensors for trilateration Re"<jupyter_code>def anchorsPosition(delta):
	_Xaux=[0,0,delta,delta]
	_Yaux=[0,delta,delta,0]
	_position=[_Xaux,_Yaux]
	return _position;

#vector=anchorsPosition(1000)
vector=[[0,1000,1000,0,500],[0,0,1000,1000,500]]
#vector=[[-1000,0,1000,2000,3000,4000],[0,0,0,0,0,0]]

#vector=[[-700,-700,-160,-160,580,580,1500,1500,2100,2100],[250,1000,-300,1500,-300,1500,-300,1500,250,1000]] 

#plt.plot(vector[0],vector[1])

def drawCercles(vector,_R):
    teta=linspace(0,2*pi,720);
    cercles=[0,0]
    cx=[]
    cy=[]
    for k in range(len(vector[0])):
        for i in range(len(teta)):
            cx.append(vector[0][k]+_R[k]*sin(teta[i]))
            cy.append(vector[1][k]+_R[k]*cos(teta[i]))
    cercles=[cx,cy]
    return cercles

def robotMove(max,npoints):
    t=linspace(0,max,npoints)
    #x=150*sin(t)+60*t+200
    #y=100*t
    x=(-500+250*t)*0.8
    y=0.8*(500+800*sin(t))
    robot=[]
    robot.append(x)
    robot.append(y)
    return robot
def rightRayons(position,vector):
    ray=[]
    for i in range(len(vector[1])):
        xyanchor=array((0,0))
        xyanchor[0]=vector[0][i]
        xyanchor[1]=vector[1][i]
        ray.append(linalg.norm(xyanchor-position))
    return ray
def distanceSensors(rate,ray):
	dvector=[]
	for i in range(len(ray)):
		dvector.append(ray[i]+randint(-int(ray[i]*rate)-1,int(ray[i]*rate+1)))
	return dvector;
#vector=anchorsPosition(1000)
robot=robotMove(max,npoints)<jupyter_output><empty_output><jupyter_text>* 1er cas de figure :mesures parfaites<jupyter_code>plt.figure(1)
plt.title("+ Robot trajectory")
plt.plot(vector[0],vector[1],"o",markersize=15,color="skyBlue")
plt.plot(robot[0],robot[1],"+",color="red")
plt.grid(True)
#    plt.ylim(-1000,2000)
#    plt.xlim(-1000,2000)
plt.show()<jupyter_output><empty_output><jupyter_text>Case 0 : 100% accurate data from DecaWave sensors * in this case, cercles intersect in one and only one point, then the position is unique<jupyter_code>plt.figure(2)
plt.title("Cas parfait de la méthode de trilatération")
i=221
for j in range(4):
    plt.subplot(i+j)
    position=[]
    position.append(robot[0][5*j])
    position.append(robot[1][5*j])
    ray=rightRayons(position,vector)
    cercles = drawCercles(vector,ray)
    plt.plot(cercles[0],cercles[1],"--",color="gray")
    #plt.text(-900,-800, r'$ +: TwIRTee $',color='red')
    plt.plot(vector[0],vector[1],"o",markersize=5,color="blue")
    plt.plot(robot[0][5*j],robot[1][5*j],"<",color="red")
    
#    plt.ylim(-1000,2000)
#    plt.xlim(-1000,2000)
    plt.grid(True)
    plt.show()<jupyter_output><empty_output><jupyter_text>Case 1 : we add some noise to distances<jupyter_code>plt.figure(3)
title="Reel life example, rate : "
title+=str(rate)    
plt.title(title)
i=221
for j in range(4):
    plt.subplot(i+j)
    position=[]
    position.append(robot[0][5*j])
    position.append(robot[1][5*j])
    ray=rightRayons(position,vector)
    mesuredRay=distanceSensors(rate,ray)
    cercles = drawCercles(vector,mesuredRay)
    plt.plot(cercles[0],cercles[1],"--",color="gray")
 #plt.text(-900,-800, r'$ +: TwIRTee $',color='red')
    plt.plot(vector[0],vector[1],"o",markersize=5,color="green")
    plt.plot(robot[0][5*j],robot[1][5*j],"+",color="red")
    plt.plot(robot[0][5*j],robot[1][5*j],"+",color="red")
    
#    plt.ylim(-1000,2000)
#    plt.xlim(-1000,2000)
    plt.grid(True)
    plt.show()<jupyter_output><empty_output><jupyter_text>circles do not intersect in one point  To Solve this issue, we will try the optimisation methods below : 1- leastsquare using QR decomposition : leastSquareQR : it must compute the X,Y cordinate according to A and b matrix using QR decomposition method<jupyter_code>def leastSquareQR(A,b):
    q,r=linalg.qr(A)
    rinv=linalg.inv(r)
    qtrns=transpose(q)
    x1=matmul(rinv,qtrns)
    x=matmul(x1,b)
    return x<jupyter_output><empty_output><jupyter_text>ComputeXY() : this function must compute the (x,y) coordinates of the robot's coordinates according to Ray[] and anchorsVector* For now, I use leastSquareQR() to solve the problem, after studying several methods of optimisation, I may change this one  def computeXY(ray,anchorsVector):    print(ray)    position=[0,0]    _x=anchorsVector[0]    _y=anchorsVector[1]    A=[]    for i in range(1,len(_x)):        A.append([_x[i]-_x[0],_y[i]-_y[0]])    b=[]    for i in range(1,len(ray)):        b.append(0.5*(ray[0]**2-ray[i]**2+(_x[i]-_x[0])**2+(_y[i]-_y[0])**2))    position=leastSquareQR(A,b)    return positionx=computeXY(mesuredRay,vector)print(x)<jupyter_code>plt.figure(5)

for j in range(4):
    plt.subplot(221+j)
    position=[]
    position.append(robot[0][5*j])
    position.append(robot[1][5*j])

    ray=rightRayons(position,vector);
    mesuredRay=distanceSensors(rate,ray);
    aprox=computeXY(mesuredRay,vector);
    cercles = drawCercles(vector,mesuredRay);    
    
    plt.plot(cercles[0],cercles[1],"--",color="silver")
    plt.plot(vector[0],vector[1],"o",markersize=5,color="Green")
    #plt.text(-900,-500, r'$ + TwIRTee $',color="red")
    plt.plot(robot[0][5*j],robot[1][5*j],"+",color="red",markersize=10)
    #plt.text(-900,-800, r'$ + Approx $',color="blue")
    plt.plot(aprox[0],aprox[1],"+",color="blue")
    
    #print(aprox)
    
#    plt.ylim(-1000,2000)
#    plt.xlim(-1000,2000)
    plt.grid(True)
    plt.show()
    

plt.figure(6)

#position=[[],[]]
approx=[[],[]]
for i in range(len(robot[0])):
    position=[]
    position.append(robot[0][i])
    position.append(robot[1][i])
    ray=rightRayons(position,vector)
    mesuredRay=distanceSensors(rate,ray)
    x=computeXY(mesuredRay,vector)
    
    xOffset=x[0]-position[0]
    yOffset=x[1]-position[1]
    approx[0].append(x[0])
    approx[1].append(x[1])
    
    

plt.title("Rate = 5%")
plt.plot(vector[0],vector[1],"o",markersize=15,color="skyBlue")
plt.plot(robot[0],robot[1],color="green")
plt.plot(approx[0],approx[1],"P",color="blue")
plt.grid(True)
#    plt.ylim(-1000,2000)
#    plt.xlim(-1000,2000)
plt.show()<jupyter_output><empty_output><jupyter_text>error() function must calculate the distance between the position and it's approximation it return an error vector made like this :* error[0] : absis axe* error[1] : error in x* error[2] : error in y* error[3] : error in distance* error[4] : percentage of error on the distance<jupyter_code>def error(robot,vector):
    error=[[],[],[],[],[],[]]
    
    for j in range(len(robot[0])):
        distances=0
        position=[]
        position.append(robot[0][j])
        position.append(robot[1][j])
        ray=rightRayons(position,vector)
        mesuredRay=distanceSensors(rate,ray)
        for i in range(len(mesuredRay)):
            distances+=mesuredRay[i]
        distances=distances/4
        approx=computeXY(mesuredRay,vector)
        error[0]=linspace(0,len(robot[0]),len(robot[0]))
        error[1].append(abs(robot[0][j]-approx[0]))
        error[2].append(abs(robot[1][j]-approx[1]))
        error[3].append(sqrt((robot[0][j]-approx[0])**2 + (robot[1][j]-approx[1])**2))
        error[4].append(100*error[3][j]/(sqrt((robot[0][j])**2 + (robot[1][j])**2)))
        error[5].append(100*error[3][j]/(distances))
        
        
    
    return error
plt.figure(7)
x=error(robot,vector)
plt.title("Pourcentage d'erreur sur la distance mesurée, rate=10%")
#plt.plot(x[0],x[3])
plt.plot(x[0],x[5])

plt.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.18422022858547377, 'fraction_numerical': 0.05788374093646307, 'mean_word_length': 4.6909090909090905, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 34, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '8', 'dir': 'jupyter-structured-clean-dedup', 'id': '387032', 'license': 'MIT', 'n_tokens_mistral': 3469, 'n_tokens_neox': 3195, 'n_words': 509, 'path': 'Aurix code/Simulations/Simu_trilateration.ipynb', 'repo_name': 'SamiEzz/embeddedTwirtee', 'size': '8155'}