Java中的多点三边测量算法答案

【问题标题】：Multi-point trilateration algorithm in JavaJava中的多点三边测量算法
【发布时间】：2015-05-19 21:13:21
【问题描述】：

我正在尝试在我的 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>=3 的点数为 N。

【问题讨论】：

这肯定会对你有所帮助：gis.stackexchange.com/questions/40660/…
该链接似乎只提供了使用名为 Mathematica 的第三方软件包的解决方案。我需要Java中的东西。理想情况下，我不必包含第三方库或 SDK，只需调整上述算法即可。
他们确实使用它来计算数字，但使用 non-linear least squares 的数学仍然相同 Apache Math Library 具有您需要的所有功能
老实说，数学有点过头了，所以试图将 Mathematica 公式转换为 Java 函数是有问题的。你能提供一个代码sn-p吗？有趣的是，我上面粘贴的算法似乎有问题。当我将自己真正靠近其中一个信标（例如在 1 英尺内）时，三边测量会导致大约 15 米的距离。我也尝试在code.google.com/p/talking-points-3/source/browse/trunk/… 实现该算法，但得到了类似的结果。
@Chris 从那以后你找到解决方案了吗？我也面临同样的问题...

标签： java android gps indoor-positioning-system trilateration

【解决方案1】：

当以正确的方式表述时，多点定位问题是一个优化问题。

大多数学术示例，例如 wikipedia 上的示例，恰好处理三个圆圈并假设信息完全准确。这些情况允许更简单的问题公式和准确的答案，并且对于像您描述的那样的实际情况通常不能令人满意。

R² 或 R³ 欧几里得空间中的问题，其距离包含测量误差，通常获得感兴趣的面积（椭圆）或体积（椭圆体）一点的。如果需要点估计而不是区域，则应使用面积质心或体积质心。 R²空间需要至少3个非退化点和距离才能得到一个唯一的区域；同样，R³ 空间需要至少 4 个非退化点和距离才能获得唯一区域。

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

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

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

【讨论】：

在我的情况下，我没有给我正确的结果...您是否尝试过具有纬度和纬度的真实位置点？
@Jaythaking 您需要将（纬度、经度、高度）中的坐标转换为笛卡尔坐标系，例如 ECEF：en.wikipedia.org/wiki/ECEF 请参阅 github.com/lemmingapex/trilateration/issues/1
我没有尝试过实现这个，但它有多准确？
我用上面的trilat解决方案，你知道算法是如何解决这个问题的吗，比如说所有三个参考点都在一条直线上，那么圆圈会在两个地方相遇，它应该算哪一个。另一个类似，由于 nise 三个圆没有共同的相交区域，一个圆与另一个圆有一些共同的区域，所以我们这里有 4 个交点，在这种情况下它将如何确定质心

【解决方案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 = (c.x * c.x + c.y * c.y) - (c.r * c.r);
        top += d*v1;

        int v2 = c.y * 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+节点如何扩展书本公式，还是更好的代码实现？

【讨论】：