|
|
|
|
|
|
|
At the end of the lesson, you should have built a Python program which implements the interval localization.
You have to install the packages : roblib, numpy, matplotlib, scipy, math
A zip file containg the start programs is available here: 
The correction is given below
import math
oo= float('inf')
nan=float('nan')
class Interval:
def __init__(x,a,b):
if a>b: x.lb,x.ub = nan,nan
else: x.lb,x.ub = a,b
def is_empty(x): return math.isnan(x.lb)
def __add__(x,y): return Interval(x.lb+y.lb, x.ub+y.ub)
def __sub__(x, y): return Interval(x.lb-y.ub, x.ub-y.lb)
def __mul__(x, y):
L=[x.lb*y.lb,x.lb*y.ub,x.ub*y.lb,x.ub*y.ub]
return Interval(min(L),max(L))
def __rmul__(x, y): return x*Interval(y,y)
def __contains__(x, a): return (x.lb<=a<=x.ub)
def __truediv__(x, y):
if 0 in y: return Interval(-oo, oo)
else: return x*Interval(1./y.ub, 1./y.lb)
def __and__(x,y):
if (x.is_empty() or y.is_empty()): return Interval(1,0)
else: return Interval(max(x.lb,y.lb),min(x.ub,y.ub))
def __or__(x,y):
if x.is_empty(): return y
elif y.is_empty(): return x
else: return Interval(min(x.lb,y.lb),max(x.ub,y.ub))
def __repr__(x): return '[{}, {}]'.format(x.lb, x.ub)
def sqr(x):
L=[x.lb**2,x.ub**2]
if 0 in x: return Interval(0,max(L))
else: return Interval(min(L),max(L))
def sqrt(x):
x=x&Interval(0,oo)
return Interval(math.sqrt(x.lb),math.sqrt(x.ub))
from myinterval import * def cadd(z,x,y): # z=x+y return z&x+y, x&z-y, y&z-x def cminus(z,x,y): # z=x-y return z&x-y, x&z+y, y&x-z def cmul(z,x,y): # z=x*y return z&x*y, x&z/y, y&z/x def csqr(y,x): # y=x^2 x1=x&Interval(-oo,0); x2=x&Interval(0,oo); y=(y&sqr(x1))|(y&sqr(x2)); if y.is_empty(): return y,y else: x1=x1&(-1*sqrt(y)); x2=x2&(sqrt(y)); return y,x1|x2;
from mycontractor import *
from roblib import *
def cdist(d,ax,ay,bx,by): # Forward-Backward contractor for d^2=(ax-ay)^2+(bx-by)^2
d2=sqr(d)
vx=ax-bx
vy=ay-by
vx2=sqr(vx)
vy2=sqr(vy)
d2=d2&(vx2+vy2)
d2,d=csqr(d2,d)
d2,vx2,vy2=cadd(d2,vx2,vy2)
vx2,vx=csqr(vx2,vx)
vy2,vy=csqr(vy2,vy)
vx,ax,bx=cminus(vx,ax,bx)
vy,ay,by=cminus(vy,ay,by)
return d,ax,ay,bx,by
_x=[66,-44,80,-30,12,-20,0,-50] # Unknow positions of the robots
_y=[-70,55,80,-15,70,0,10,-50]
N=len(_x)
D=[[Interval(0,oo) for i in range(N)] for j in range(N)] # Distance matrix
X=[Interval(-oo,oo) for i in range(N)]
Y=[Interval(-oo,oo) for i in range(N)]
print("D=",D)
for i in range(0,N):
for j in range(0,N):
if (i!=j) and (i+j)%3>0: # I use the condition (i+j)%3>0 to say that not all measurements are known
_d=int(round(sqrt((_x[j]-_x[i])**2+(_y[j]-_y[i])**2))) #int(round) creates a noise in [-0.5,0.5]
D[i][j]=Interval(_d-0.5,_d+0.5)&Interval(0,oo)
for i in range(0,3): # The position of the first three robots is approximately known
X[i]=Interval(_x[i]-0.1,_x[i]+0.1) # Equivalently, the first three robots and anchors
Y[i]=Interval(_y[i]-0.1,_y[i]+0.1)
ax=init_figure(-100,100,-100,100)
for k in range(0,10): # The contractors are called several times
for i in range(0,N):
for j in range(0,N):
D[i][j],X[i],Y[i],X[j],Y[j]=cdist(D[i][j],X[i],Y[i],X[j],Y[j])
draw_box_border(X[i].lb,X[i].ub,Y[i].lb,Y[i].ub,'blue',1)
for i in range(0,N): # Final solution is drawn
draw_box_border(X[i].lb,X[i].ub,Y[i].lb,Y[i].ub,'green',2)
draw_box_border(_x[i]-0.2,_x[i]+0.2,_y[i]-0.2,_y[i]+0.2,'red',2)
pause(1000)
Students and some teachers
Organizers
Aurore boréale
