This microsite is associated to the paper
L. Jaulin (2023).
Optimal separator for the hyperbola; Application to localization,
from vibes import vibes import numpy as np from codac import * def Cardinals(q): def rho(q): a2,b2,c2=4*q[3]*q[5]-q[4]**2,4*q[3]*q[2]-2*q[1]*q[4],4*q[3]*q[0]-q[1]**2 D2=b2**2-4*a2*c2 return (-b2+sqrt(D2)*Interval(-1,1))/(2*a2) X2 = rho(q) Card=[[φ1(q,X2.lb(),True),X2.lb()]] Card.append([φ1(q,X2.ub(),True),X2.ub()]) q=[q[0],q[2],q[1],q[5],q[4],q[3]] X1 = rho(q) Card.append([X1.ub(),φ1(q,X1.ub(),True)]) Card.append([X1.lb(),φ1(q,X1.lb(),True)]) #for P in Card: draw_tiny_box(P[0],P[1],'green',0.01) return Card def ɸ1(q,X2,Card): R=[] for x2_ in [φ1(q,X2.lb()),φ1(q,X2.ub())]: if not(np.isnan(x2_)): R.append(x2_) for P in Card: if ((X2+1.E-10*Interval(-1,1))).contains(P[1]): R.append(φ1(q,P[1],True)) # 1E-10 : to avoid 0 notin [0,1] if R==[]: return Interval.EMPTY_SET return Interval(np.min(R),np.max(R)) def φ1(q,x2,force=False): a1,b1,c1=q[3],q[1]+q[4]*x2,q[0]+q[2]*x2+q[5]*sqr(x2) D1=b1**2-4*a1*c1 if (force and D1<0): D1=0 return (-b1+np.sign(q[3])*sqrt(D1))/(2*a1) def testHyperbola(x,q): return (q[0]+q[1]*x[0]+q[2]*x[1]+q[3]*x[0]*x[0]+q[4]*x[0]*x[1]+q[5]*x[1]*x[1]<0 ) class SepHyperbola(Sep): def __init__(S,q): Sep.__init__(S,2) S.q=q def separate(S,Xin,Xout): C=CtcHyperbola(S.q) X=Xin|Xout P=C.contract(X) if P.is_empty(): if testHyperbola(X.mid(),S.q): Xin=P else: Xout=P return Xin,Xout L=[Xout[i].rad()-P[i].rad() for i in range(0,len(X))] w=np.max(L) i = L.index(w) e1=P[i].lb()-Xout[i].lb() e2=Xout[i].ub()-P[i].ub() if e1+e2<=0.00000000001: return Xin,Xout #no significant improvement Q = X.copy() R = X.copy() if e2>e1: Q[i]=Interval(P[i].ub(),X[i].ub()) R[i]=Interval(X[i].lb(),P[i].ub()) else: Q[i]=Interval(X[i].lb(),P[i].lb()) R[i]=Interval(P[i].lb(),X[i].ub()) if testHyperbola(Q.mid(),S.q): Xin=R else: Xout=R return Xin,Xout def ψ(e): e=vect2matsym(e) Q=[1,e[0,0]*2+e[1,0]*3 ,e[0,1]*2+e[1,1]*3 , e[0,0]**2*4+e[1,0]**2*6 , (e[0,0]*e[1,1]+e[0,1]*e[1,0])*5 , e[0,1]**2*4+e[1,1]**2*6] Q=(np.rint(Q)).astype(int) return Q class CtcHyperbola(Ctc): def __init__(C,q): Ctc.__init__(C,2) C.q=q def contract(C,X): def C1(s,X): return CtcHyperbolaSym(s,C.q).contract(X) def C2(s,X): return C1(s,X)&C1([s[1],s[0]],X) return C2([1,2],X)|C2([1,-2],X)|C2([-1,2],X)|C2([-1,-2],X) class CtcHyperbolaSym(Ctc): def __init__(C,s,q): Ctc.__init__(C,2) C.s,C.q=s,q def contract(C,X): return ctcActionSym(CtcHyperbola0(octasym(ψ(C.s))(C.q)),C.s,X) class CtcHyperbola0(Ctc): def __init__(C,q): Ctc.__init__(C,2) C.q=q def contract(C,X): Card=Cardinals(C.q) X[0]=X[0]&ɸ1(C.q,X[1],Card) return X def notAnHyperbola(q): if q[3]*q[5]-(1/4)*q[4]**2>=0: print ("not an hyperbola") return True def sivia_hyperbola(X,q): if notAnHyperbola(q): return S=SepHyperbola(q) SIVIA(X,S,0.1,color_map=col0) X=IntervalVector([[-2,2],[-2,2]]) q=[-1,1,1,3,30,-2] #q=[-1,5,2,-2,30,-2] sivia_hyperbola(X,q)
def TDOA_classic(X): def SepHyp(q): _g="q5*sqr(x2)+q4*x1*x2+q3*sqr(x1)+q2*x2+q1*x1+q0" _g=_g.replace("q0",str(q[0])).replace("q1",str(q[1])).replace("q2",str(q[2])).replace("q3",str(q[3])).replace("q4",str(q[4])).replace("q5",str(q[5])) g=Function('x1','x2',_g) return SepFwdBwd(g,Interval([-oo,0])) S1=SepHyp([2250.391900000008, 1732.1199999999992, -2581.3200000000006, -74.35999999999996, -72, 245.64000000000004]) S2=SepHyp([3568.7279000000153, 1514.519999999998, -2747.7200000000016, -61.55999999999989, -72, 258.4400000000001]) S3=SepHyp([-1814.2640999999846, -108.35999999999854, 621.7200000000006, 24.840000000000018, -72, 24.840000000000018]) S4=SepHyp([-28.696099999977775, -293.95999999999935, 512.9200000000012, 31.240000000000023, -72, 31.240000000000023]) S12=S1&~S2 S34=S3&~S4 SIVIA(X,S12&S34,0.05,color_map=col0) vibes.beginDrawing() X=IntervalVector([[0,20],[0,20]]) TDOA_classic(X) vibes.endDrawing()
class SepHyperbolaFoci(Sep): def __init__(S,a1,a2,b1,b2,l): Sep.__init__(S,2) S.a1,S.a2,S.b1,S.b2=a1,a2,b1,b2 S.l=l& sqrt((a1-b1)**2+(a2-b2)**2) * Interval(-1,1) def separate(S,Xin,Xout): def q(a1,a2,b1,b2,l): q0=-a1**4-2*a1**2*a2**2+2*a1**2*b1**2+2*a1**2*b2**2+2*a1**2*l**2-a2**4+2*a2**2*b1**2+2*a2**2*b2**2+2*a2**2*l**2-b1**4-2*b1**2*b2**2+2*b1**2*l**2-b2**4+2*b2**2*l**2-l**4 q1=4*a1**3-4*a1**2*b1+4*a1*a2**2-4*a1*b1**2-4*a1*b2**2-4*a1*l**2-4*a2**2*b1+4*b1**3+4*b1*b2**2-4*b1*l**2 q2=4*a1**2*a2-4*a1**2*b2+4*a2**3-4*a2**2*b2-4*a2*b1**2-4*a2*b2**2-4*a2*l**2+4*b1**2*b2+4*b2**3-4*b2*l**2 q3=-4*a1**2+8*a1*b1-4*b1**2+4*l**2 q4=-8*a1*a2+8*a1*b2+8*a2*b1-8*b1*b2 q5=-4*a2**2+8*a2*b2-4*b2**2+4*l**2 return [q0,q1,q2,q3,q4,q5] X=Xin|Xout S1=SepHyperbola(q(S.a1,S.a2,S.b1,S.b2,S.l.lb())) S2=SepHyperbola(q(S.a1,S.a2,S.b1,S.b2,S.l.ub())) #S=S1|~S2 # should work but did not, I had to decompose Xin1,Xout1=S1.separate(X,X) Xout2,Xin2=S2.separate(X,X) Xin=Xin1|Xin2 Xout=Xout1&Xout2 #Xin,Xout=S.separate(X,X) return Xin,Xout def TDOA(X,Y1,Y2): a1,a2=13,7 b1,b2=4,6 c1,c2=16,10 Se1=SepHyperbolaFoci(a1,a2,b1,b2,Y1) Se2=SepHyperbolaFoci(a1,a2,c1,c2,Y2) Se=Se1&Se2 SIVIA(X,Se2,0.05,color_map=col0}) draw_tiny_box(a1,a2,'black[black]',0.05) draw_tiny_box(c1,c2,'black[black]',0.05) draw_tiny_box(b1,b2,'black[black]',0.05) #draw_box(Interval(-6,6),Interval(-6,6),'black[transparent]') draw_tiny_box(0,0,'black[black]',0.002) vibes.drawLine([[X[0].lb(),0],[X[0].ub(),0]],'black') vibes.drawLine([[0,X[1].lb()],[0,X[1].ub()]],'black') vibes.drawBox(X[0].lb(),X[0].ub(),X[1].lb(),X[1].ub(),'black[transparent]') X=IntervalVector([[0,20],[0,20]]) Y1,Y2=Interval(8-eps,8+eps),Interval(4-eps,4+eps) TDOA(X,Y1,Y2,0.05)
col0={SetValue.IN: "#FFCC00[#FFAAFFEE]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "yellow[white]"} def toBox(X): if type(X)==list: X=IntervalVector(X) return(X) def draw_box(X,Y,col='darkblue'): vibes.drawBox(X.lb(),X.ub(),Y.lb(),Y.ub(),col) def draw_tiny_box(x,y,col='darkblue',eps=0.02): X=Interval(x-eps,x+eps) Y=Interval(y-eps,y+eps) draw_box(X,Y,col) def octasym(I): return lambda L : [(1.0*np.sign(I[i]))*L[np.abs(I[i])-1] for i in range(0,len(I))] def sym(X,s): b=True if type(X)==list else False X=toBox(X) if b: return s([X[i] for i in range(0,len(X))]) else: return IntervalVector(s([X[i] for i in range(0,len(X))])) def ctcAction(C,s,_s,X): return sym(C.contract(sym(X,s)),_s) def ctcActionSym(C,I,X): s=octasym(I) _s=octasym(invertsym(I)) return ctcAction(C,s,_s,X) def vect2matsym(s): n=len(s) M=np.zeros([n,n]) for i in range(n): e=np.sign(s[i]) j=np.abs(s[i])-1 M[j][i]=e return M def mat2vectsym(M): n=len(M) v=[] for j in range(n): for i in range(n): if not(M[i,j]==0): v.append(np.int(np.sign(M[i,j])*(i+1))) return(v) def mult2vectsym_via_mat(v1,v2): M1=vect2matsym(v1) M2=vect2matsym(v2) M=M1@M2 v=mat2vectsym(M) return v def invertsym(v): M=vect2matsym(v) N = np.linalg.inv(M) return mat2vectsym(N)