Optimal separator for an ellipse

Application to localization

Luc Jaulin












This microsite is associated to the paper

L. Jaulin (2023). Optimal separator for an ellipse; Application to localization, arXiv, math.NA, 2305.10842.

Also published in the book : Valued Approaches to Control and Estimation of Uncertain Systems. Springer.



This work proposes a minimal contractor and a minimal separator for an ellipse in the plane. The task is facilitated using actions induced by the hyperoctahedral group of symmetries. An application related to the localization of an object using multiple sonars is proposed.







Code for the simple ellipse 




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

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


class CtcEllipse0(Ctc):
    def __init__(C,q):
        Ctc.__init__(C,2)
        C.q=q
    def contract(C,X):
        def psi(q): #top vertex of the ellipse
            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))/(2*a2)
        def phi(q,x2):
            a1,b1,c1=q[3],q[1]+q[4]*x2,q[0]+q[2]*x2+q[5]*sqr(x2)
            D1=np.max([b1**2-4*a1*c1,0])
            return (-b1+sqrt(D1))/(2*a1)
        if not(X.is_empty()):
            s=octasym([1,3,2,6,5,4])
            qs=s(C.q)
            if np.isnan(psi(qs)): print("error the constraint should be associated to an ellipse")
            A=IntervalVector([[phi(C.q,psi(C.q)),psi(qs)],[phi(qs,psi(qs)),psi(C.q)]])
            B=X&A
            X=X&IntervalVector([[phi(C.q,B[1].ub()),phi(C.q,B[1].lb())],[phi(qs,B[0].ub()),phi(qs,B[0].lb())]])
        return X



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)


class CtcEllipse(Ctc):
    def __init__(C,q):
        Ctc.__init__(C,2)
        C.q=q
    def contract(C,X):
        def ψ(e): return [1,e[0]*2,e[1]*3,4,e[0]*e[1]*5,6]
        def Contract_ith(I,X):
            s=octasym(I);    e=np.sign(I);
            C0=CtcEllipse0(octasym(ψ(e))(C.q))
            return ctcAction(C0,s,s,X)
        if not (X.is_empty()):
            X=Contract_ith([1,2],X)|Contract_ith([-1,-2],X)|Contract_ith([1,-2],X)|Contract_ith([-1,2],X)
        return X



def testellipse(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 SepEllipse(Sep):
    def __init__(S,q):
        Sep.__init__(S,2)
        S.q=q
    def separate(S,Xin,Xout):
        C=CtcEllipse(S.q)
        X=Xin|Xout
        P=C.contract(X)
        if P.is_empty():
            if testellipse(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
        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 testellipse(Q.mid(),S.q):  Xin=R
        else:  Xout=R
        return Xin,Xout





def ellipse_simple():
    X=IntervalVector([[-2,2],[-2,2]])
    q=[-5,1,1,3,1,2]
    SIVIA(X,SepEllipse(q),0.1,color_map={SetValue.IN: "#FFCC00[#FFAAFFEE]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "yellow[white]"})
    vibes.drawBox(X[0].lb(),X[0].ub(),X[1].lb(),X[1].ub(),'black[transparent]')



vibes.beginDrawing()
ellipse_simple()
vibes.endDrawing()

Code for the simple ellipse with classical forward-backward propagation 



def ellipse_simple_classic():
    X=IntervalVector([[-2,2],[-2,2]])
    q=[-5,1,1,3,1,2]
    _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)
    S=SepFwdBwd(g,Interval([-oo,0]))
    SIVIA(X,S,0.01,color_map={SetValue.IN: "#FFCC00[#FFAAFFEE]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "yellow[white]"})
    vibes.drawBox(X[0].lb(),X[0].ub(),X[1].lb(),X[1].ub(),'black[transparent]')





Code for the generation of the coefficients for the ellipse based on the focus 



def simplify_expression_ellipse_foci():
    a1,a2,b1,b2,x1,x2,l = symbols("a1 a2 b1 b2 x1 x2 l")
    da1=x1-a1
    da2=x2-a2
    db1=x1-b1
    db2=x2-b2
    F=4*(da1**2+da2**2)*(db1**2+db2**2)-(l**2-da1**2-da2**2-db1**2-db2**2)**2
    F=expand(F)
    F=simplify(F)
    F2 = factor(F,x1,x2)
    print("\n F2=",F2)





Code for the sonar application 





class SepEllipseFoci(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&Interval(sqrt((a1-b1)**2+(a2-b2)**2),oo)
    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=SepEllipse(q(S.a1,S.a2,S.b1,S.b2,S.l.ub()))
        S2=SepEllipse(q(S.a1,S.a2,S.b1,S.b2,S.l.lb()))
        S=S1&~S2
        Xin1,Xout1=S1.separate(X,X)
        Xout2,Xin2=S2.separate(X,X)
        Xin=Xin1|Xin2
        Xout=Xout1&Xout2
        #Xin,Xout=S.separate(X,X) # ne fonctionne pas
        return Xin,Xout


def sonar():
    a1,a2=-2,1
    b1,b2=-2,-1
    c1,c2=3,2
    Se1=SepEllipseFoci(a1,a2,b1,b2,Interval(4,6))
    Se2=SepEllipseFoci(a1,a2,c1,c2,Interval(7,9))
    Se=Se1&Se2
    X=IntervalVector([[-7,7],[-7,7]])
    SIVIA(X,Se,0.05,color_map={SetValue.IN: "#FFCC00[#FFAAFFEE]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "white[white]"})
    draw_tiny_box(a1,a2,'red[red]',0.02)
    draw_tiny_box(c1,c2,'black[black]',0.02)
    draw_tiny_box(b1,b2,'black[black]',0.02)
    draw_box(Interval(-6,6),Interval(-6,6),'black[transparent]')



vibes.beginDrawing()
sonar()
vibes.endDrawing()