Optimal separator for the hyperbola; Application to localization

Optimal separator for the hyperbola

Application to localization

Luc Jaulin

This microsite is associated to the paper

L. Jaulin (2023). Optimal separator for the hyperbola; Application to localization,

This work proposes a minimal contractor and a minimal separator for the hyperbola in the plane. The task is facilitated using actions induced by the hyperoctahedral group of symmetries. An application related to the TDOA localization.

Code for the simple hyperbola


from vibes import vibes
import numpy as np
from codac import *



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)
        E=Interval(0,0)
        C.Q=[q[0]+E,q[1]+E,q[2]+E,q[3]+E,q[4]+E,q[5]+E]
    def contract(C,X):
        def φ1(Q,X2):
            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
            D1=D1&Interval(0,oo)
            return (-b1+sign(Q[3])*sqrt(D1))/(2*a1)
        def ɸ1(Q,X2,Card):
            R=Interval.EMPTY_SET
            for x2_ in [φ1(Q,Interval(X2.lb())),φ1(Q,Interval(X2.ub()))]: R=R|x2_
            for P in Card:
                if not ((X2&P[1]).is_empty()) : R=R|(φ1(Q,P[1]))
            return R
        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
                R=(-b2+sqrt(D2)*Interval(-1,1))/(2*a2)
                R1=(-b2-sqrt(D2))/(2*a2)
                R2=(-b2+sqrt(D2))/(2*a2)
                if R1.ub()=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)

Code for the TDOA application with a classical forward-backward approach


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()

Code for the TDOA application with the minimal hyperbola separator



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)

Other functions



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)