Math - 3d positioning/multilateration

Question

I have a problem involving 3d positioning - sort of like GPS. Given a set of known 3d coordinates (x,y,z) and their distances d from an unknown point, I want to find the unknown point. There can be any number of reference points, however there will be at least four.

So, for example, points are in the format (x,y,z,d). I might have:

(1,0,0,1)
(0,2,0,2)
(0,0,3,3)
(0,3,4,5)

And here the unknown point would be (0,0,0,0).

What would be the best way to go about this? Is there an existing library that supports 3d multilateration? (I have been unable to find one). Since it's unlikely that my data will have an exact solution (all of the 4+ spheres probably won't have a single perfect intersect point), the algorithm would need to be capable of approximating it.

So far, I was thinking of taking each subset of three points, triangulating the unknown based on those three, and then averaging all of the results. Is there a better way to do this?

Answer 1

You could take a non-linear optimisation approach, by defining a "cost" function that incorporates the distance error from each of your observation points.

Setting the unknown point at (x,y,z), and considering a set of N observation points (xi,yi,zi,di) the following function could be used to characterise the total distance error:

C(x,y,z) = sum( ((x-xi)^2 + (y-yi)^2 + (z-zi)^2 - di^2)^2 )
           ^^^
           ^^^ for all observation points i = 1 to N

This is the sum of the squared distance errors for all points in the set. (It's actually based on the error in the squared distance, so that there are no square roots!)

When this function is at a minimum the target point (x,y,z) would be at an optimal position. If the solution gives C(x,y,z) = 0 all observations would be exactly satisfied.

One apporach to minimise this type of equation would be Newton's method. You'd have to provide an initial starting point for the iteration - possibly a mean value of the observation points (if they en-circle (x,y,z)) or possibly an initial triangulated value from any three observations.

Edit: Newton's method is an iterative algorithm that can be used for optimisation. A simple version would work along these lines:

H(X(k)) * dX = G(X(k));  // solve a system of linear equations for the 
                         // increment dX in the solution vector X
X(k+1) = X(k) - dX;      // update the solution vector by dX

The G(X(k)) denotes the gradient vector evaluated at X(k), in this case:

G(X(k)) = [dC/dx
           dC/dy
           dC/dz]

The H(X(k)) denotes the Hessian matrix evaluated at X(k), in this case the symmetric 3x3 matrix:

H(X(k)) = [d^2C/dx^2 d^2C/dxdy d^2C/dxdz
           d^2C/dydx d^2C/dy^2 d^2C/dydz
           d^2C/dzdx d^2C/dzdy d^2C/dz^2]

You should be able to differentiate the cost function analytically, and therefore end up with analytical expressions for G,H.

Another approach - if you don't like derivatives - is to approximate G,H numerically using finite differences.

Hope this helps.

Answer 2

If you can spend the time, an iterative solution should approach the correct solution pretty quickly. So pick any point the correct distance from site A, then go round the set working out the distance to the point then adjusting the point so that it's in the same direction from the site but the correct distance. Continue until your required precision is met (or until the point is no longer moving far enough in each iteration that it can meet your precision, as per the possible effects of approximate input data).

For an analytic approach, I can't think of anything better than what you already propose.

Answer 3

Non-linear solution procedures are not required. You can easily linearise the system. If you take pair-wise differences

$(x-x_i)^2-(x-x_j)^2+(y-y_i)^2-(y-y_j)^2+(z-z_i)^2-(z-z_j)^2=d_i^2-d_j^2$

then a bit of algebra yields the linear equations

$(x_i-x_j) x +(y_i-y_j) y +(zi-zj) z=-1/2(d_i^2-d_j^2+ds_i^2-ds_j^2)$,

where $ds_i$ is the distance from the $i^{th}$ sensor to the origin. These are the equations of the planes defined by intersecting the $i^{th}$ and the $j^{th}$ spheres.

For four sensors you obtain an over-determined linear system with $4 choose 2 = 6$ equations. If $A$ is the resulting matrix and $b$ the corresponding vector of RHS, then you can solve the normal equations

$A^T A r = A^T b$

for the position vector $r$. This will work as long as your sensors are not coplanar.