Karush–Kuhn–Tucker conditions to build efficient contractors; Application to TDoA localization

Karush–Kuhn–Tucker conditions to build efficient contractors

Application to TDoA localization

Luc Jaulin

This microsite is associated to the paper

L. Jaulin (2023). Karush–Kuhn–Tucker conditions to build efficient contractors, Application to TDoA localization arXiv, math.NA 2306.09679.

This work proposes an efficient contractor for the TDoA (Time Differential of Arrival) equation. The contractor is based on a minimal inclusion test which is built using the Karush–Kuhn–Tucker (KKT) conditions. An application related to the localization of sound sources using a TDoA technique is proposed.

A zip file with the source codes (use Vibes Viewer):
See details below.

Code to generate the level curve of the TDoA function : isocontour (Section 2.1 of the paper)



from roblib import *  # available at https://www.ensta-bretagne.fr/jaulin/roblib.py


def draw3d(x1,x2,f):
    fig = figure()
    ax = Axes3D(fig,auto_add_to_figure=False)
    fig.add_axes(ax)
    ax.plot_surface(x1,x2,f)

def drawcontour(x1,x2,f):
    contour(x1,x2,f,30)
    draw_box_border(a[0]-e,a[0]+e,a[1]-e,a[1]+e,"red",1)
    draw_box_border(b[0]-e,b[0]+e,b[1]-e,b[1]+e,"red",1)


def Cardinals(X1,X2,a,b):
    def inside(x1,a1,b1):
        return (a1<x1<b1) or (b1<x1<a1)        
    def φ1(x1,a,b):
        a1,a2,b1,b2=a[0],a[1],b[0],b[1]
        return (a2*abs(x1-b1)-b2*abs(x1-a1))/(abs(x1-b1)-abs(x1-a1))
    def φ2(x2,a,b):
        return φ1(x2,[a[1],a[0]],[b[1],b[0]])
    draw_box_border(X1[0],X1[1],X2[0],X2[1],'darkblue',1)
    Card=[[X1[0],X2[0]]]
    Card.append([X1[0],X2[1]])
    Card.append([X1[1],X2[0]])
    Card.append([X1[1],X2[1]])
    x2=φ1(X1[0],a,b)
    if inside(x2,X2[0],X2[1]): Card.append([X1[0],x2])
    x2=φ1(X1[1],a,b)
    if inside(x2,X2[0],X2[1]): Card.append([X1[1],x2])
    x1=φ2(X2[0],a,b)
    if inside(x1,X1[0],X1[1]): Card.append([x1,X2[0]])
    x1=φ2(X2[1],a,b)
    if inside(x1,X1[0],X1[1]): Card.append([x1,X2[1]])
    for P in Card:
        draw_box_border(P[0]-e,P[0]+e,P[1]-e,P[1]+e,"green",1)


e=0.02
a=[-1,-3]
b=[2,2]
x1,x2 = meshgrid(arange(-5,5,0.01), arange(-5,5,0.01))
dxa1=x1-a[0]
dxa2=x2-a[1]
dxb1=x1-b[0]
dxb2=x2-b[1]
f=sqrt(dxa1**2+dxa2**2)-sqrt(dxb1**2+dxb2**2)
drawcontour(x1,x2,f)
Cardinals([-1.2,4],[-4.1,1.2],a,b)

Code to illustrate the KKT contractor : ctctdoaKKT (Section 2.2 of the paper)



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

class CtcTDoAFwdKKT(Ctc):
    def __init__(C,a,b):
        Ctc.__init__(C,3)
        C.a,C.b=a,b
    def contract(C,X):
        def φ1(x1,a,b):
            a1,a2,b1,b2=a[0],a[1],b[0],b[1]
            X1=Interval(x1,x1)
            return (a2*abs(X1-b1)-b2*abs(X1-a1))/(abs(X1-b1)-abs(X1-a1))
        def φ2(x2,a,b): return φ1(x2,[a[1],a[0]],[b[1],b[0]])
        def f(x1,x2):
            return sqrt((x1-a[0])**2+(x2-a[1])**2)-sqrt((x1-b[0])**2+(x2-b[1])**2)
        if X.is_empty(): return IntervalVector.empty(3)
        a,b=C.a,C.b
        X1,X2=X[0],X[1]
        F=[f(X1.lb(),X2.lb()),f(X1.lb(),X2.ub()),f(X1.ub(),X2.lb()),f(X1.ub(),X2.ub())]
        F=Interval(np.min(F),np.max(F))
        for x1 in [X1.lb(),X1.ub()]:
            if not(X2.is_disjoint(φ1(x1,a,b))): F=F|f(x1,φ1(x1,a,b))
        for x2 in [X2.lb(),X2.ub()]:
            if not(X1.is_disjoint(φ2(x2,a,b))): F=F|f(φ2(x2,a,b),x2)
        X[2]=X[2]&F
        return IntervalVector([X[0],X[1],X[2]])


class SepTDoA_Fwd_ab(Sep):
    def __init__(S,a,b,Y):
        Sep.__init__(S,2)
        S.Y=Y
        S.Cfwd=CtcTDoAFwdKKT(a,b)
    def separate(S,Xin,Xout):
        X=Xin|Xout
        Z=IntervalVector([X[0],X[1],Interval(-oo,oo)])
        S.Cfwd.contract(Z)
        if Z[2].is_subset(S.Y):
            Xin=IntervalVector.empty(2)
            return (Xin,Xout)
        if Z[2].is_disjoint(S.Y):
            Xout=IntervalVector.empty(2)
            return (Xin,Xout)
        return Xin,Xout

vibes.beginDrawing()
X=IntervalVector([[-15,15],[-15,15]])
a,b=[-1,-2],[2,3]
S=SepTDoA_Fwd_ab(a,b,Interval(3,5))
SIVIA(X,S,0.01)
vibes.endDrawing()

The same resolution as above using a classical forward-backward contractor : ctctdoaClassic (Section 2.2 of the paper)


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


class CtcTDoAFwdClassic(Ctc):
    def __init__(C,a,b):
        Ctc.__init__(C,3)
        a1,a2,b1,b2=a[0],a[1],b[0],b[1]
        _g="sqrt((x1-a1)^2+(x2-a2)^2)-sqrt((x1-b1)^2+(x2-b2)^2)-x3"
        _g=_g.replace("a1",str(a1)).replace("a2",str(a2)).replace("b1",str(b1)).replace("b2",str(b2))
        C.Cg=CtcFwdBwd(Function('x1','x2','x3',_g))
    def contract(C,X):
        if X.is_empty(): return IntervalVector.empty(3)
        X[2]=Interval(-oo,oo)
        C.Cg.contract(X)
        return X


class CtcTDoABwdclassic(Ctc):
    def __init__(C,a,b):
        Ctc.__init__(C,3)
        C.a,C.b=a,b
    def contract(C,X):
        if X.is_empty(): return IntervalVector.empty(3)
        a1,a2,b1,b2=C.a[0],C.a[1],C.b[0],C.b[1]
        _g="sqrt((x1-a1)^2+(x2-a2)^2)-sqrt((x1-b1)^2+(x2-b2)^2)"
        _g=_g.replace("a1",str(a1)).replace("a2",str(a2)).replace("b1",str(b1)).replace("b2",str(b2))
        g=Function('x1','x2',_g)
        Cg=CtcFwdBwd(g,X[2])
        Zout=IntervalVector([X[0],X[1]])
        Cg.contract(Zout)
        X[0],X[1]=Zout[0],Zout[1]
        return IntervalVector([X[0],X[1],X[2]])

class SepTDoA_Fwd_ab(Sep):
    def __init__(S,a,b,Y,Cfwd):
        Sep.__init__(S,2)
        S.Y=Y
        S.Cfwd=Cfwd(a,b)
    def separate(S,Xin,Xout):
        X=Xin|Xout
        Z=IntervalVector([X[0],X[1],Interval(-oo,oo)])
        S.Cfwd.contract(Z)
        if Z[2].is_subset(S.Y):
            Xin=IntervalVector.empty(2)
            return (Xin,Xout)
        if Z[2].is_disjoint(S.Y):
            Xout=IntervalVector.empty(2)
            return (Xin,Xout)
        return Xin,Xout

vibes.beginDrawing()
X=IntervalVector([[-15,15],[-15,15]])
a,b=[-1,-2],[2,3]
Y=Interval(3,5)
S=SepTDoA_Fwd_ab(a,b,Y,CtcTDoAFwdClassic)
SIVIA(X,S,0.01)
vibes.endDrawing()

Inverse of two disks via TDoA function with three receivers: ctctdoa2Disks (Section 3.4 of the paper)


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

class SepAct(Sep):   # Return the separator  S = C \bullet Sy
    def __init__(S,Cfwd,Cback,Sy):
        Sep.__init__(S,2)
        S.Cfwd=Cfwd
        S.Cback=Cback
        S.Sy=Sy
    def separate(S,Xin,Xout):
        I=range(0,S.Sy.nb_var)
        X=Xin|Xout
        Z=[IntervalVector([X[0],X[1],Interval(-oo,oo)]) for i in I]
        for i in I: S.Cfwd[i].contract(Z[i])
        Y=IntervalVector([Z[i][2] for i in I])
        Yin,Yout=Y.copy(),Y.copy()
        S.Sy.separate(Yin,Yout)
        Zin= [IntervalVector([Z[i][0],Z[i][1],Yin [i]]) for i in I]
        Zout=[IntervalVector([Z[i][0],Z[i][1],Yout[i]]) for i in I]
        def Cbwd(Z):
            X=IntervalVector([Interval(-oo,oo),Interval(-oo,oo)])
            for i in I:
                S.Cback[i].contract(Z[i])
                X=X&IntervalVector([Z[i][0],Z[i][1]])
            return X
        Xin=Cbwd(Zin)
        Xout=Cbwd(Zout)
        return Xin,Xout

def sivia_TDoA_Sy(X,M,Sy,epsi):
    I = range(0,Sy.nb_var)
    Cfwd=[CtcTDoAFwdKKT(M[i],M[i+1])  for i in I]
    Cback=[CtcTDoABwdclassic(M[i],M[i+1]) for i in I]
    S=SepAct(Cfwd,Cback,Sy)
    SIVIA(X,S,0.01)


vibes.beginDrawing()

X=IntervalVector([[-15,15],[-15,15]])
Sy1=SepFwdBwd(Function("y1","y2","(y1-2)^2+(y2-1)^2-1"),Interval(-oo,0))
Sy2=SepFwdBwd(Function("y1","y2","(y1+1)^2+(y2+2)^2-1"),Interval(-oo,0))
Sy=Sy1|Sy2
M=[[-1,-2],[2,3],[4,1]]
sivia_TDoA_Sy(X,M,Sy,0.01)
vibes.endDrawing()

Inverse of a possibility distribution via TDoA function with three receivers: ctctdoaRegions (Section 4 of the paper)



from vibes import vibes
import numpy as np
from codac import *
from ctclib import draw_tiny_box
from ctctdoaKKT import CtcTDoAFwdKKT
from ctctdoaDisk import SepAct,drawMicro,sivia_TDoA_Sy

col0={SetValue.IN: "#FFCC00[#FFAAFFEE]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "yellow[white]"}
col6={SetValue.IN: "#CCAA33[#FF00FF22]", SetValue.OUT: "#AADDFF[#AAFFFFEE]",SetValue.UNKNOWN: "yellow[white]"}
col5={SetValue.IN: "#CCAA66[#FF00DD55]", SetValue.OUT: "transparent",SetValue.UNKNOWN: "yellow[white]"}
col4={SetValue.IN: "#CCAA88[#FF00BB77]", SetValue.OUT: "transparent",SetValue.UNKNOWN: "yellow[white]"}
col3={SetValue.IN: "#CCAAAA[#FF0099AA]", SetValue.OUT: "transparent",SetValue.UNKNOWN: "yellow[white]"}
col2={SetValue.IN: "#CCAACC[#FF0033CC]", SetValue.OUT: "transparent",SetValue.UNKNOWN: "yellow[white]"}
col1={SetValue.IN: "#FF0000FF[orange]", SetValue.OUT: "transparent",SetValue.UNKNOWN: "yellow[white]"}


def confidence_regions_abc(X,M):
    Col=[col6,col5,col4,col3,col2,col1]
    z=[16,8,4,2,1,0.5]
    imax=len(Col)
    for i in range(0,imax):
        _g="(y1-2)^2+(y2-1)^2-z".replace("z",str(z[i]))
        Sy=SepFwdBwd(Function("y1","y2",_g),Interval(-oo,0))
        sivia_TDoA_Sy(X,M,Sy,Col[i],0.01)
    drawMicro(M)

def disk_regions_abc():
    X=IntervalVector([[-10,10],[-10,10]])
    Col=[col6,col5,col4,col3,col2,col1]
    z=[16,8,4,2,1,0.5]
    imax=len(Col)
    for i in range(0,imax):
        _g="(y1-2)^2+(y2-1)^2-z".replace("z",str(z[i]))
        Sy=SepFwdBwd(Function("y1","y2",_g),Interval(-oo,0))
        SIVIA(X,Sy,0.01,color_map=Col[i])



vibes.beginDrawing()
X=IntervalVector([[-15,15],[-15,15]])
confidence_regions_abc(X,[[-1,-2],[2,3],[4,1]])
#disk_regions_abc()
vibes.endDrawing()