3D Trilateration using given distances of unknown fixed points

I am new to this forum and not a native english speaker, so please be nice! :)

Here is the challenge I face at the moment: I want to calculate the (approximate) relative coordinates of yet unknown points in a 3D euclidean space based on a set of given distances between 2 points. In my first approach I want to ignore possible multiple solutions, just taking the first one by random.

e.g.: given set of distances: (I think its creating a pyramid with a right-angled triangle as a base)

P1-P2-Distance

• 1-2-30
• 2-3-40
• 1-3-50
• 1-4-60
开发者_开发技巧
• 2-4-60
• 3-4-60

Step1: Now, how do I calculate the relative coordinates for those points?

I figured that the first point goes to 0,0,0 so the second one is 30,0,0.

After that the third points can be calculated by finding the crossing of the 2 circles from points 1 and 2 with their distances to point 3 (50 and 40 respectively). How do I do that mathematically? (though I took these simple numbers for an easy representation of the situation in my mind). Besides I do not know how to get to the answer in a correct mathematical way the third point is at 30,40,0 (or 30,0,40 but i will ignore that).

But getting the fourth point is not as easy as that. I thought I have to use 3 spheres in calculate the crossing to get the point, but how do I do that?

Step2: After I figured out how to calculate this "simple" example I want to use more unknown points... For each point there is minimum 1 given distance to another point to "link" it to the others. If the coords can not be calculated because of its degrees of freedom I want to ignore all possibilities except one I choose randomly, but with respect to the known distances.

Step3: Now the final stage should be this: Each measured distance is a bit incorrect due to real life situation. So if there are more then 1 distances for a given pair of points the distances are averaged. But due to the imprecise distances there can be a difficulty when determining the exact (relative) location of a point. So I want to average the different possible locations to the "optimal" one.

Can you help me going through my challenge step by step?

---

You need to use trigonometry - specifically, the 'cosine rule'. This will give you the angles of the triangle, which lets you solve the 3rd and 4th points.

The rules states that

c^2 = a^2 + b^2 - 2abCosC

where a, b and c are the lengths of the sides, and C is the angle opposite side c.

In your case, we want the angle between 1-2 and 1-3 - the angle between the two lines crossing at (0,0,0). It's going to be 90 degrees because you have the 3-4-5 triangle, but let's prove:

50^2 = 30^2 + 40^2 - 2*30*40*CosC
CosC = 0
C = 90 degrees

This is the angle between the lines (0,0,0)-(30,0,0) and (0,0,0)- point 3; extend along that line the length of side 1-3 (which is 50) and you'll get your second point (0,50,0).

Finding your 4th point is slightly trickier. The most straightforward algorithm that I can think of is to firstly find the (x,y) component of the point, and from there the z component is straightforward using Pythagoras'.

Consider that there is a point on the (x,y,0) plane which sits directly 'below' your point 4 - call this point 5. You can now create 3 right-angled triangles 1-5-4, 2-5-4, and 3-5-4.

You know the lengths of 1-4, 2-4 and 3-4. Because these are right triangles, the ratio 1-4 : 2-4 : 3-4 is equal to 1-5 : 2-5 : 3-5. Find the point 5 using trigonometric methods - the 'sine rule' will give you the angles between 1-2 & 1-4, 2-1 and 2-4 etc.

The 'sine rule' states that (in a right triangle)

a / SinA = b / SinB = c / SinC

So for triangle 1-2-4, although you don't know lengths 1-4 and 2-4, you do know the ratio 1-4 : 2-4. Similarly you know the ratios 2-4 : 3-4 and 1-4 : 3-4 in the other triangles.

I'll leave you to solve point 4. Once you have this point, you can easily solve the z component of 4 using pythagoras' - you'll have the sides 1-4, 1-5 and the length 4-5 will be the z component.

---

I'll initially assume you know the distances between all pairs of points.

As you say, you can choose one point (A) as the origin, orient a second point (B) along the x-axis, and place a third point (C) along the xy-plane. You can solve for the coordinates of C as follows:

given: distances ab, ac, bc
assume
A = (0,0)
B = (ab,0)
C = (x,y)  <- solve for x and y, where:
  ac^2 = (A-C)^2 = (0-x)^2 + (0-y)^2 = x^2 + y^2
  bc^2 = (B-C)^2 = (ab-x)^2 + (0-y)^2 = ab^2 - 2*ab*x + x^2 + y^2

-> bc^2 - ac^2 = ab^2 - 2*ab*x
-> x = (ab^2 + ac^2 - bc^2)/2*ab
-> y = +/- sqrt(ac^2 - x^2)

For this to work accurately, you will want to avoid cases where the points {A,B,C} are in a straight line, or close to it.

Solving for additional points in 3-space is similar -- you can expand the Pythagorean formula for the distance, cancel the quadratic elements, and solve the resulting linear system. However, this does not directly help you with your steps 2 and 3...

Unfortunately, I don't know a well-behaved exact solution for steps 2 and 3, either. Your overall problem will generally be both over-constrained (due to conflicting noisy distances) and under-constrained (due to missing distances).

You could try an iterative solver: start with a random placement of all your points, compare the current distances with the given ones, and use that to adjust your points in such a way as to improve the match. This is an optimization technique, so I would look up books on numerical optimization.

---

If you know the distance between the nodes (fixed part of system) and the distance to the tag (mobile) you can use trilateration to find the x,y postion.

I have done this using the Nanotron radio modules which have a ranging capability.