0

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?

smbrine
  • 1
  • 2

1 Answers1

0

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.

Jeff Y
  • 2,437
  • 1
  • 11
  • 18
  • See also https://stackoverflow.com/a/40959536/5379657 – Jeff Y May 18 '23 at 12:07
  • Alternatively, if you “know” which side of the plane you want your solution, and the method gives the other solution, you can work out the reflection formulas to convert it. Generally it’s not going to be good enough to just look at “inside the cube or not” because for some, both solutions are inside the cube. I notice there is also an optional bounds argument to least_squares that might serve a similar purpose. – Jeff Y May 18 '23 at 12:20