CODEWITHSUNDEEP
pythonnumpymathscipytrilateration
smbrine
smbrine

Reputation: 1

How to find an X Y and Z for a point when one only know coordinates of the reference points and the distance between

Let's say I have 3 points (we might increase the amount if needed). Each has their X, Y and Z distances from the center of the world that we're examining. We also have the unknown point whose coordinates are unknown. We only know the distance between 3 reference points and the point we're searching for.Here are some parts for the convenience:

P = [10, 10, 10] // Point with the unknown coordinates. I defined them to check whether the formula work or not.
P1 = [0, 0, 0] // 1st reference point
P2 = [20, 20, 0] // 2nd reference point
P3 = [0, 20, 20] // 3rd reference point

I have a quite simple pythagorean theorem implementation that takes coordinates of the points and calculates their distances.

def count_dist(x, y, z, x1, y1, z1):
    D = sqrt((x - x1)**2 + (y - y1)**2 + (z - z1)**2)
    return D

With that being said, we can use this logic here:

P1 = [ 0,  0,  0]
P2 = [cube_side, cube_side,  0]
P3 = [ 0, cube_side, cube_side]

D1 = count_dist(P[0], P[1], P[2], P1[0], P1[1], P1[2])
D2 = count_dist(P[0], P[1], P[2], P2[0], P2[1], P2[2])
D3 = count_dist(P[0], P[1], P[2], P3[0], P3[1], P3[2])

After that I'm trying to build a trilateration equation with scipy and numpy to find the coordinates of the hidden point:

def residuals(coords, P1, P2, P3, D1, D2, D3):
    x, y, z = coords
    r1 = np.sqrt((x - P1[0])**2 + (y - P1[1])**2 + (z - P1[2])**2) - D1
    r2 = np.sqrt((x - P2[0])**2 + (y - P2[1])**2 + (z - P2[2])**2) - D2
    r3 = np.sqrt((x - P3[0])**2 + (y - P3[1])**2 + (z - P3[2])**2) - D3
    return np.array([r1, r2, r3])

def trilateration(P1, P2, P3, D1, D2, D3):
    initial_guess = np.array(((P1[0] + P2[0] + P3[0]) / 3,
                              (P1[1] + P2[1] + P3[1]) / 3,
                              (P1[2] + P2[2] + P3[2]) / 3))
    result = least_squares(residuals, 
                           initial_guess, 
                           args=(P1, P2, P3, D1, D2, D3), 
                           method='lm')
    return result.x.tolist()

But it mistakes in 80% cases with the deviation of up to 30% from the maximum distance available in my pre-defined cube. May the problem be somewhere in the code implementation?

Upvotes: 0

Views: 117

Answers (1)

Jeff Y
Jeff Y

Reputation: 2456

Looking at this geometrically, your three reference points are, and any three reference points will be, co-planar. If you’re solving just by distances from reference points (and the solution is non-co-planar with them, as your [10,10,10] is), there will be two solutions, each a reflection of the other through the plane. I haven’t worked out what the reflection of [10,10,10] is through your plane, but it seems to me that any numerical “seeking” algorithm may equally likely come up with either solution.

You may want to increase to four reference points. And make sure they are not co-planar.

Upvotes: 0

Related Questions