[Python] Why does my implementation of trilateration give wrong results?

I am trying to do an implementation of trilateration. The function gets three 3d coordinates and distances from base stations for every coordinate. It must return the position of the point in 3d space trilateration.

def trilateration(P1, P2, P3, r1, r2, r3):

p1 = np.array([0, 0, 0])
p2 = np.array([P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]])
p3 = np.array([P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]])
v1 = p2 - p1
v2 = p3 - p1

Xn = (v1)/np.linalg.norm(v1)

tmp = np.cross(v1, v2)

Zn = (tmp)/np.linalg.norm(tmp)

Yn = np.cross(Xn, Zn)

i = np.dot(Xn, v2)
d = np.dot(Xn, v1)
j = np.dot(Yn, v2)

X = ((r1**2)-(r2**2)+(d**2))/(2*d)
Y = (((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(X))
Z1 = np.sqrt(r1**2-X**2-Y**2)
Z2 = np.sqrt(r1**2-X**2-Y**2)*(-1)

K1 = P1 + X*Xn + Y * Yn + Z1 * Zn
K2 = p1 + X * Xn + Y * Yn - Z2 * Zn
return K1


I have a test example. With those cordinates and distances P1=(2,2,0), P2=(3,3,0), P3=(1,4,0) r1=1, r2=1, r3=1.4142, it should return P=(2,3,0).

But it is returning [3.33253331 1.66746669 1.33373281].

What is wrong in my code?

Continue reading...