3D trilateration Java

Question

I have a problem with my App in Java. I have 3 points and 3 distance on the Earth and I need to find 4th point. In my code I used some equeals from wikipedia to count trilateration with that. The solution should be : 49.195167,16.607208 (GPS on GoogleMap).

I would be very glad, if somebody can help to find mistakes in code. Because it counts wrong GPS.

Thank you a lot!

    float earthR = 6371;
    float p1x = (float) 61.47;
    float p1y = (float) 23.76;
    float p2x = (float) 42.80;
    float p2y = (float) -1.63;
    float p3x= (float) 39.67;
    float p3y= (float) 20.85;
    float r1 = 1470;
    float r2 = 1617;
    float r3 = 1127;


    float P1x = (float) (earthR*(Math.cos(Math.toRadians(p1x))*Math.cos(Math.toRadians(p1y))));
    float P1y = (float) (earthR*(Math.cos(Math.toRadians(p1x))*Math.sin(Math.toRadians(p1y))));
     float P1z = (float) (earthR*(Math.sin(Math.toRadians(p1x))));

   float P2x = (float) (earthR* (Math.cos(Math.toRadians(p2x))*Math.cos(Math.toRadians(p2y))));
   float P2y = (float) (earthR*(Math.cos(Math.toRadians(p2x))*Math.sin(Math.toRadians(p2y))));
   float P2z = (float) (earthR*(Math.sin(Math.toRadians(p2x))));
   float P3x = (float) (earthR* (Math.cos(Math.toRadians(p3x))*Math.cos(Math.toRadians(p3y))));
   float P3y = (float) (earthR*(Math.cos(Math.toRadians(p3x))*Math.sin(Math.toRadians(p3y))));
   float P3z = (float) (earthR*(Math.sin(Math.toRadians(p3x))));

   float exx = (float) ((P2x-P1x)/Math.sqrt(Math.pow(P2z-P1z, 2)+Math.pow((P2x-P1x),2)+Math.pow((P2y-P1y),2)));
   float exy = (float) ((P2y-P1y)/Math.sqrt(Math.pow(P2z-P1z, 2)+Math.pow((P2x-P1x),2)+Math.pow((P2y-P1y),2)));
   float exz = (float) ((P2z-P1z)/Math.sqrt(Math.pow(P2z-P1z, 2)+Math.pow((P2x-P1x),2)+Math.pow((P2y-P1y),2)));
   float EX = (float) Math.sqrt(Math.pow(exx, 2)+Math.pow(exy, 2)+Math.pow(exz,2));

   float i = (float) Math.sqrt(Math.pow((P3x-P1x)*EX, 2)+Math.pow((P3y-P1y)*EX, 2)+Math.pow((P3z-P1z)*EX, 2));

   float eyx = (float) ((P3x-P1x-(i*exx))/Math.sqrt((Math.pow(P3z-P1z-(i*exz),2))+(Math.pow(P3x-P1x-(i*exx),2))+(Math.pow(P3y-P1y-(i*exy),2))));
   float eyy = (float) ((P3y-P1y-(i*exy))/Math.sqrt((Math.pow(P3z-P1z-(i*exz),2))+(Math.pow(P3x-P1x-(i*exx),2))+(Math.pow(P3y-P1y-(i*exy),2))));
   float eyz = (float) ((P3z-P1z-(i*exz))/Math.sqrt((Math.pow(P3z-P1z-(i*exz),2))+(Math.pow(P3x-P1x-(i*exx),2))+(Math.pow(P3y-P1y-(i*exy),2))));
   float EY = (float) Math.sqrt(Math.pow(eyx, 2)+Math.pow(eyy, 2)+Math.pow(eyz, 2));

   float ezx = (exy*eyz)-(exz*exy);
   float ezy = (exz*eyx)-(exx*eyz);
   float ezz = (exx*eyy)-(exy*eyx);
   float EZ = (float) Math.sqrt(Math.pow(ezx, 2)+Math.pow(ezy, 2)+Math.pow(ezz, 2));

   float d = (float) Math.sqrt((Math.pow(P2x-P1x,2))+(Math.pow(P2y-P1y,2))+Math.pow(P2z-P1z, 2));

   float j = (float) Math.sqrt(Math.pow((P3x-P1x)*EY, 2)+Math.pow((P3y-P1y)*EY, 2)+Math.pow((P3z-P1z)*EY, 2));
   float x = (float) ((Math.pow(r1, 2)-Math.pow(r2, 2)+Math.pow(d, 2))/(2*d));
   float y = (float) (Math.pow(r1, 2)-Math.pow(r3, 2)+Math.pow(i, 2)+Math.pow(j, 2))/(2*j)- (i*x/j);

   float z1 = (float) (Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2));
if (z1<0){ z1 = z1*(-1);}
   float z = (float) Math.sqrt(z1);

   float lat = (float) Math.toDegrees(Math.atan2(y,x));
   float lon = (float) Math.toDegrees(Math.asin((z)/earthR));
   System.out.println(lat);
   System.out.println(lon);

Answer 1

First, if z1 is less than zero, that means there is no solution. You can think it as three spheres no intersecting.

if(z1 < 0) return NO_SOLUTION;

If it is greater than zero, that means you have two intersections.

if(z1 > 0)
{
    z1 = Math.sqrt(z1);
    z2 = z1*-1;
}

After this step, you have 2 points, which are:

result1 = P1 + exx + eyy + ez*z1;
result2 = P1 + exx + eyy + ez*z2;

This is where third point comes into play. You calculate the distance of those two results to P3 .

if(Math.abs(distance(result1, P3) - r3) < Math.abs(distance(result2, P3) - r3))
   return result1;
else return result2;

which means, you pick the point which satisfies the distance from it to P3 ie r3 .