Mathematics - 3D Positioning / Multilateration

Question

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

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

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

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

What would be the best way? Is there an existing library supporting 3d multilateration? (I could not find him). Since it is unlikely that my data will have an exact solution (all 4+ spheres probably will not have any perfect intersection points), the algorithm should have come close to it.

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

Answer 1

You can use a non-linear optimization approach by defining a “cost” function that includes an error in the distance from each of your observation points.

Setting an unknown point in (x,y,z) and examining the set of observation points N (xi,yi,zi,di) to characterize the total distance error, the following function can be used:

 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. (This is actually based on a squared distance error, so there are no square roots!)

When this function is at least, the target point (x,y,z) will be in the optimal position. If the solution gives C(x,y,z) = 0 , all observations will be accurately performed.

One way to minimize this type of equation will be Newton's method . You would have to specify the starting point for the iteration — perhaps the average value of the observation points (if they span (x,y,z) ) or perhaps the initial triangulated value from any three observations.

Edit: Newton method is an iterative algorithm that can be used for optimization. A simple version will work in the following directions:

 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 

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

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

H(X(k)) denotes the Hessian matrix estimated in X(k) , in this case the 3x3 symmetric 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, get analytic 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 time, the iterative solution should quickly come up with the right solution. Therefore, select any point at the correct distance from site A, then go to the set by setting the distance to the point, then adjust the point so that it is in the same direction from the place, but with the correct distance. Continue until your required accuracy is achieved (or until the point no longer moves far enough at each iteration to match your accuracy, depending on the possible effects of the approximate input).

For an analytical approach, I cannot think of anything better than what you already offer.

Answer 3

Non-linear solution procedures are not required. You can easily linearize the system. If you accept paired differences

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

then a bit of algebra gives 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 sensor $ i ^ {th} $ to the origin. These are equations of planes defined by the intersection of $ i ^ {th} $ and $ j ^ {th} $ spheres.

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

$ A ^ TA r = A ^ T b $

for the position vector $ r $. This will work until your sensors are coplanar.

Answer 4

Look, Newton or Newton-Gauss will not work. This is because you have a gap at certain points in the space of the derivatives of this function. Believe me, I tried Newton - it almost never converges. Instead, I tried a different set of algorithms. Try using the direct search algorithm to optimize (find the minimum or maximum function). It works for me - but sometimes it converges at the wrong point - probably because of the "planarity" of the 4 selected points. In this case, you can choose a different starting point - and you will probably get the correct answer. The algorithmic example I found is VERY easy to implement and works amazingly well. Hope that helped. Greetings.