3D 三边测量 Java

Posted 2023-02-23

【中文标题】3D 三边测量 Java

【英文标题】：3D trilateration Java

【发布时间】：2013-03-14 15:45:47

【问题描述】：

我的 Java 应用程序有问题。我在地球上有 3 个点和 3 个距离，我需要找到第 4 个点。在我的代码中，我使用了来自***的一些 equals 来计算三边测量。解决方案应该是：49.195167,16.607208（GoogleMap 上的 GPS）。

如果有人可以帮助找出代码中的错误，我将非常高兴。因为它计数错误的GPS。

非常感谢！

    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);

【参考方案1】：

首先，如果z1 小于零，则表示无解。你可以认为它是三个不相交的球体。

if(z1 < 0) return NO_SOLUTION;

如果大于零，则表示您有两个交叉点。

if(z1 > 0)

    z1 = Math.sqrt(z1);
    z2 = z1*-1;

经过这一步，你有2个点，分别是：

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

这就是第三点发挥作用的地方。你计算这两个结果到P3的距离。

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

这意味着，您选择满足与P3 的距离的点，即r3。