Java中的多点三边测量算法

我正在尝试在我的Android应用程序中实现三边测量算法，以确定用户的室内位置。 我正在使用超宽带信标来获得到固定点的距离。 我能够调整Trilateration Method Android Java中建议的方法如下：

public LatLng getLocationByTrilateration( LatLng location1, double distance1, LatLng location2, double distance2, LatLng location3, double distance3){ //DECLARE VARIABLES double[] P1 = new double[2]; double[] P2 = new double[2]; double[] P3 = new double[2]; double[] ex = new double[2]; double[] ey = new double[2]; double[] p3p1 = new double[2]; double jval = 0; double temp = 0; double ival = 0; double p3p1i = 0; double triptx; double tripty; double xval; double yval; double t1; double t2; double t3; double t; double exx; double d; double eyy; //TRANSALTE POINTS TO VECTORS //POINT 1 P1[0] = location1.latitude; P1[1] = location1.longitude; //POINT 2 P2[0] = location2.latitude; P2[1] = location2.longitude; //POINT 3 P3[0] = location3.latitude; P3[1] = location3.longitude; //TRANSFORM THE METERS VALUE FOR THE MAP UNIT //DISTANCE BETWEEN POINT 1 AND MY LOCATION distance1 = (distance1 / 100000); //DISTANCE BETWEEN POINT 2 AND MY LOCATION distance2 = (distance2 / 100000); //DISTANCE BETWEEN POINT 3 AND MY LOCATION distance3 = (distance3 / 100000); for (int i = 0; i < P1.length; i++) { t1 = P2[i]; t2 = P1[i]; t = t1 - t2; temp += (t*t); } d = Math.sqrt(temp); for (int i = 0; i < P1.length; i++) { t1 = P2[i]; t2 = P1[i]; exx = (t1 - t2)/(Math.sqrt(temp)); ex[i] = exx; } for (int i = 0; i < P3.length; i++) { t1 = P3[i]; t2 = P1[i]; t3 = t1 - t2; p3p1[i] = t3; } for (int i = 0; i < ex.length; i++) { t1 = ex[i]; t2 = p3p1[i]; ival += (t1*t2); } for (int i = 0; i < P3.length; i++) { t1 = P3[i]; t2 = P1[i]; t3 = ex[i] * ival; t = t1 - t2 -t3; p3p1i += (t*t); } for (int i = 0; i < P3.length; i++) { t1 = P3[i]; t2 = P1[i]; t3 = ex[i] * ival; eyy = (t1 - t2 - t3)/Math.sqrt(p3p1i); ey[i] = eyy; } for (int i = 0; i < ey.length; i++) { t1 = ey[i]; t2 = p3p1[i]; jval += (t1*t2); } xval = (Math.pow(distance1, 2) - Math.pow(distance2, 2) + Math.pow(d, 2))/(2*d); yval = ((Math.pow(distance1, 2) - Math.pow(distance3, 2) + Math.pow(ival, 2) + Math.pow(jval, 2))/(2*jval)) - ((ival/jval)*xval); t1 = location1.latitude; t2 = ex[0] * xval; t3 = ey[0] * yval; triptx = t1 + t2 + t3; t1 = location1.longitude; t2 = ex[1] * xval; t3 = ey[1] * yval; tripty = t1 + t2 + t3; return new LatLng(triptx,tripty); } 

使用这种方法给我一个用户位置，但不是非常准确。 如何将其扩展为使用超过3个已知位置/距离？ 理想情况下N个点数，其中N> = 3。

Answer 1

当以正确的方式配制时，多点定位问题是优化问题。

大多数学术性的例子，如维基百科上的例子，只涉及三个圆圈，并假设完全准确的信息。 这些情况允许更简单的问题表达式具有精确的答案，并且通常不能满足您描述的实际情况。

通常获得R ²或R ³欧几里德空间中包含测量误差，面积（椭圆）或体积（椭圆体）的距离而不是点的问题。 如果需要点估计而不是区域，则应使用区域质心或体积质心。 R ²空间需要至少3个非简并点和距离才能获得一个独特的区域; 类似地，R ³空间需要至少4个非简并点和距离以获得唯一区域。

这是一个开源的Java库，可以轻松满足您的需求： https ： //github.com/lemmingapex/Trilateration

它使用了一种流行的非线性最小二乘优化器，Levenberg-Marquardt算法，来自Apache Commons Math。

 double[][] positions = new double[][] { { 5.0, -6.0 }, { 13.0, -15.0 }, { 21.0, -3.0 }, { 12.42, -21.2 } }; double[] distances = new double[] { 8.06, 13.97, 23.32, 15.31 }; NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer()); Optimum optimum = solver.solve(); // the answer double[] calculatedPosition = optimum.getPoint().toArray(); // error and geometry information RealVector standardDeviation = optimum.getSigma(0); RealMatrix covarianceMatrix = optimum.getCovariances(0); 

Answer 2

我在电子书中找到了这个解决方案;

https://books.google.co.uk/books?id=Ki2DMaeeHpUC&pg=PA78

我将其编码为一个Java示例，它似乎适用于3个圆圈。 但是，我不知道如何调整此公式以涵盖解决方案中的第4点和第5点的三边测量。 我的数学不太好。

我的公式代码在这里;

 private void findCenter() { int top = 0; int bot = 0; for (int i=0; i<3; i++) { Circle c = circles.get(i); Circle c2, c3; if (i==0) { c2 = circles.get(1); c3 = circles.get(2); } else if (i==1) { c2 = circles.get(0); c3 = circles.get(2); } else { c2 = circles.get(0); c3 = circles.get(1); } int d = c2.x - c3.x; int v1 = (cx * cx + cy * cy) - (cr * cr); top += d*v1; int v2 = cy * d; bot += v2; } int y = top / (2*bot); Circle c1 = circles.get(0); Circle c2 = circles.get(1); top = c2.r*c2.r+c1.x*c1.x+c1.y*c1.y-c1.r*c1.r-c2.x*c2.x-c2.y*c2.y-2*(c1.y-c2.y)*y; bot = c1.x-c2.x; int x = top / (2*bot); imHere = new Circle(x,y,5); } 

理想情况下，我想要一个可以使用3个以上节点的代码解决方案，并且在使用多个点的情况下，可以将解决方案更多地用于从具有小半径值的节点导出的点。

有人有任何想法吗？

要么如何扩展4+节点的图书公式，还是更好的代码实现？