Solved

3d trilateration with multiple known points.

Posted on 2015-01-05
13
1,215 Views
Last Modified: 2015-01-07
Hi all, I am attempting to write a C# trilateration function, but it is beyond my limited math knowledge...

I have a number of known points (currently 8, but this number may change)
I have the x, y, z coordinates for each.

P1[3];
P2[3];
P3[3];
P4[3];
P5[3];
P6[3];
P7[3];
P8[3];
P3[3];
where each array has 3 values, x, y and z.

I know the distance from the unknown point (x)  to each of the known points.

double D1;
double D2;
double D3;
double D4;
double D5;
double D6;
double D7;
double D8;


So given these values, I need to find the xyz coordinates of the unknown point X[3];

What is the most straightforward way to do this? I have found examples in other code languages, but nothing that I understand in c#. And only ever with three points.
Any and all assistance would be much appreciated!

Thanks in advance!
0
Comment
Question by:chemicalx001
  • 4
  • 3
  • 2
  • +3
13 Comments
 

Author Comment

by:chemicalx001
ID: 40531787
update:

I have trilateration working with three known points. I converted this example:

 (http://www.experts-exchange.com/Other/Math_Science/Q_21253179.html)

to C#, and it seems to be going according to plan.

Now, to add more known points, would it be better to cycle through groups of three and merge the results somehow?
(groups of 4 would suit much better, but I am unsure how to add that function)

Or update the math to cope with eight points?

code as follows:
(note, converted but not really understood)

public static void trilaterate2(double[] P1, double[] P2, double[] P3, double[] L)
    {


                   //define station points and distances
                        double x1 = P1[0];
                        double y1 = P1[1];
                        double z1 = P1[2];

                        double x2 = P2[0];
                        double y2 = P2[1];
                        double z2 = P2[2];

                        double x3 = P3[0];
                        double y3 = P3[1];
                        double z3 = P3[2];

                        double L1 = L[0];
                        double L2 = L[1];
                        double L3 = L[2];

                   //caluculate coords in plane of stations
                        double LB1 = Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1));
                        double LB2 = Math.Sqrt((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2) + (z3 - z2) * (z3 - z2));
                        double LB3 = Math.Sqrt((x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3) + (z1 - z3) * (z1 - z3));

                    double X = (L1*L1  - L2*L2  + LB1*LB1)/(2*LB1 );
                    double C1 = Math.Sqrt (L1*L1 - X*X);
                    if (L1*L1 - X*X < 0){Console.Write("no solution");}
                    double XB = (LB3*LB3 - LB2*LB2 + LB1*LB1 )/(2*LB1 );
                    if (LB3*LB3 - XB* XB < 0 ){Console.Write("no solution");}
                    double CB=  Math.Sqrt(LB3*LB3 - XB* XB );
                    if (C1*C1+(XB - X)*(XB - X)< 0){Console.Write("no solution");}
                    double D1 = Math.Sqrt(C1*C1+(XB - X)*(XB - X));
                    double Y = (D1*D1 - L3*L3  + CB*CB  )/(2*CB );
                    if (C1*C1 - Y*Y < 0){Console.Write("no solution");}
                    double Z = Math.Sqrt(C1 * C1 - Y * Y);

                   //Now transform X,Y,Z to x,y,z
                    //Find the unit vectors in X,Y,Z
                    double Xx = (x2-x1);
                    double Xy = (y2-y1);
                    double Xz = (z2-z1);
                    double Xl = Math.Sqrt(Xx*Xx+Xy*Xy+Xz*Xz);
                    Xx = Xx / Xl;
                    Xy = Xy / Xl;
                    Xz = Xz / Xl;


                    double t =- ((x1-x3)*(x2-x1)+(y1-y3)*(y2-y1)+(z1-z3)*(z2-z1))/(LB1*LB1);
                    double Yx = (x1+(x2-x1)*t-x3);
                    double Yy = (y1+(y2-y1)*t-y3);
                    double Yz = (z1+(z2-z1)*t-z3);
                    double Yl = Math.Sqrt(Yx*Yx+Yy*Yy+Yz*Yz);
                    Yx =- (Yx/Yl);
                    Yy =- (Yy/Yl);
                    Yz =- (Yz/Yl);

                    double Zx = (Xy * Yz - Xz * Yy);
                    double Zy = (Xz * Yx - Xx * Yz);
                    double Zz = (Xx * Yy - Xy * Yx);
                    //document.write(' Zx='+Zx.toFixed(5)+' Zy='+Zy.toFixed(5)+' Zz='+Zz.toFixed(5)+'<br>')

                    double x = (x1 + X * Xx + Y * Yx + Z * Zx);
                    double y = (y1 + X * Xy + Y * Yy + Z * Zy);
                    double z = (z1 + X * Xz + Y * Yz + Z * Zz);

                    x = (x1 + X * Xx + Y * Yx - Z * Zx);
                    y = (y1 + X * Xy + Y * Yy - Z * Zy);
                    z = (z1 + X * Xz + Y * Yz - Z * Zz);

                    Console.Write(x + " " + y + " " + z);
                    
    }

Open in new window


called:

public static double[] L = new double[3] { 1782, 1795, 1491 };
        public static double[] P1 = new double[3] { 1000, 1000, 0 };
        public static double[] P2 = new double[3] { 1000, 2000, 0 };
        public static double[] P3 = new double[3] { 2000, 1000, 0 };

        static void Main(string[] args)
        {
            MathTest.trilaterate2(P1,P2,P3,L);


            Console.ReadLine();
        }

Open in new window

0
 
LVL 61

Expert Comment

by:gheist
ID: 40532929
There is no documentation of your used function anywhere.
Can you share at least its description (I am mostly concerned about point definition)
Also would be nice to share if geosphere is involved.
0
 
LVL 79

Expert Comment

by:David Johnson, CD, MVP
ID: 40532942
trilateration is the intersection of 3 circles, triangles or spheres.  So you need the x,y co-ordinates of the 3 points AND their radius
http://en.wikipedia.org/wiki/Trilateration#Preliminary_and_final_computations
0
 
LVL 61

Expert Comment

by:gheist
ID: 40532961
Makes little sense as 3 points look like dots to me in question...
0
 

Author Comment

by:chemicalx001
ID: 40533084
Hi Guys, thanks. these are the known points:

 double x1 = P1[0];
                        double y1 = P1[1];
                        double z1 = P1[2];

                        double x2 = P2[0];
                        double y2 = P2[1];
                        double z2 = P2[2];

                        double x3 = P3[0];
                        double y3 = P3[1];
                        double z3 = P3[2];

and these are the distances from the unknown. (radius of circle)

                        double L1 = L[0];
                        double L2 = L[1];
                        double L3 = L[2];

I know i should get my head around the equation, but i am in a hurry to get this project finished! It seems to be working when i run it on 3 known points, the problem is, i have more than 3. I am trialling cycling through groups of 3 and averaging the result. Is this the best way to go about it?

Thanks again.
0
 
LVL 61

Expert Comment

by:gheist
ID: 40533111
You must use integers to keep accuracy of numbers. Double will never yield any solution as around 1000 the detail is 1e-12
0
Is Your Active Directory as Secure as You Think?

More than 75% of all records are compromised because of the loss or theft of a privileged credential. Experts have been exploring Active Directory infrastructure to identify key threats and establish best practices for keeping data safe. Attend this month’s webinar to learn more.

 
LVL 32

Assisted Solution

by:Robberbaron (robr)
Robberbaron (robr) earned 250 total points
ID: 40533234
but to raise David Johnsons reply again, for a triangulation you only need 3 points.  Any more does not help...

unless you expect a there to be an error in the distances and you want the 'average' answer,  which is what GPS does.... the more satellites (eg points and distances), the better the answer.

is that what you are trying to do ?  
http://en.wikipedia.org/wiki/Centroid#Of_a_finite_set_of_points

a bit better than average probably
0
 

Author Comment

by:chemicalx001
ID: 40533345
thanks! My issue is that am getting different results depending on which 3 points I use. Not ideal. So I am averaging the results. Which is working, but very slowly! Way too much latency to be of any use really.

So really my problem is with the trilateration not giving reliable results.
So back to my original question....

Does anyone have a C# formula to trilaterate the location of an unknown point from three known points (xyz) and distance from each known point to the unknown point?


Meanwhile, I will look into centroid, thanks.
0
 
LVL 27

Accepted Solution

by:
d-glitch earned 250 total points
ID: 40533500
>> My issue is that am getting different results depending on which 3 points I use.

That means that there is an error (or errors) somewhere.  
Do you know why?  How accurately do you know the positions of the base stations?  How did you measure the distances?

>>  I am trialling cycling through groups of 3 and averaging the result. Is this the best way to go about it?

A better technique might be to minimize the RMS error.

You have n base stations A thru N    and   n distances  Da thru Dn.

Once you have an approximate solution X, calculate a new set of distances between this point and your base stations to get a new set of distances  Dax thru Dnx.

The RMS error is    sqrt[  (1 - Dax/Da)²   +   (1 - Dbx/Db)²   +   (1 - Dcx/Dc)²   +   . . .    +   (1 - Dnx/Dn)² ]  

Move your solution point in the x, y, and z directions to minimize the error.
0
 

Author Comment

by:chemicalx001
ID: 40535393
Thanks guys! I am reading up on RMS and getting better results.  I also had an error in my code that was causing the lag, I was not clearing the list after i averaged it.  

Thanks again for your help.
0
 
LVL 27

Expert Comment

by:d-glitch
ID: 40535694
Thanks for the points.  And one final note:

You more you know about the source or your errors, the better you can handle it.

In particular, the error function I showed earlier would be appropriate for errors in measuring the distance.

If the distance measurements were perfect, and the errors are in the coordinates of the base stations, you would not want to normalize with distance.

       RMS Error  =  sqrt[  (Da - Dax)²   +   (Db - Dbx)²   +   (Dc - Dcx)²   +   . . .    +   (Dn - Dnx)² ]
0
 
LVL 22

Expert Comment

by:earth man2
ID: 40536915
You can use Excel to do this using solver functionality
 excel screenshotYou can extend the array of data points...
0

Featured Post

Is Your Active Directory as Secure as You Think?

More than 75% of all records are compromised because of the loss or theft of a privileged credential. Experts have been exploring Active Directory infrastructure to identify key threats and establish best practices for keeping data safe. Attend this month’s webinar to learn more.

Question has a verified solution.

If you are experiencing a similar issue, please ask a related question

Suggested Solutions

Entity Framework is a powerful tool to help you interact with the DataBase but still doesn't help much when we have a Stored Procedure that returns more than one resultset. The solution takes some of out-of-the-box thinking; read on!
It was really hard time for me to get the understanding of Delegates in C#. I went through many websites and articles but I found them very clumsy. After going through those sites, I noted down the points in a easy way so here I am sharing that unde…
This tutorial will introduce the viewer to VisualVM for the Java platform application. This video explains an example program and covers the Overview, Monitor, and Heap Dump tabs.
In this fourth video of the Xpdf series, we discuss and demonstrate the PDFinfo utility, which retrieves the contents of a PDF's Info Dictionary, as well as some other information, including the page count. We show how to isolate the page count in a…

867 members asked questions and received personalized solutions in the past 7 days.

Join the community of 500,000 technology professionals and ask your questions.

Join & Ask a Question

Need Help in Real-Time?

Connect with top rated Experts

12 Experts available now in Live!

Get 1:1 Help Now