logo标签列表

Java中的多点三边定位算法

javaandroidgpsindoor-positioning-systemtrilateration
10

我正在尝试将三边定位算法实现到我的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。

1
这肯定会对你有所帮助： http://gis.stackexchange.com/questions/40660/trilateration-algorithm-for-n-amount-of-points - Chris Stillwell
1
看起来该链接只提供了使用名为Mathematica的第三方软件包的解决方案。我需要的是Java中的解决方案。理想情况下，我不需要包含第三方库或SDK，而只需调整上述算法即可。 - Chris
他们确实使用它进行数字计算，但是使用非线性最小二乘法的数学仍然是相同的。Apache Math Library拥有您所需的所有函数。 - Chris Stillwell
1
老实说，这个数学公式有点超出我的能力范围，所以尝试将那个Mathematica公式转换成Java函数是有问题的。你能提供一段代码片段吗？有趣的是，我贴上面的算法似乎存在问题。当我靠近其中一个信标（比如在1英尺内），三边测量的结果会显示大约15米远。我还尝试了在https://code.google.com/p/talking-points-3/source/browse/trunk/WifiPosition/src/TalkingPoint/thejoo/Trilateration.java?r=109实现该算法，但得到了类似的结果。 - Chris
@Chris 自那时起，你找到解决方案了吗？ 我也遇到了同样的问题... - Jaythaking
2个回答
9

当正确建立多边定位问题时，它是一个优化问题。

大多数学术示例（如wikipedia上的示例）涉及精确三个圆，并假定信息完全准确。这些情况允许使用更简单的问题公式并得到确切答案，通常不适用于您描述的实际情况。

在R2或R3欧几里德空间中，如果距离包含测量误差，则通常会获得感兴趣区域（椭圆）或体积（椭球），而不是点。如果需要点估计而不是区域，则应使用区域重心或体积重心。 R2空间需要至少3个非退化点和距离才能获得唯一区域；类似地，R3空间需要至少4个非退化点和距离才能获得唯一区域。

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

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
1
@Jaythaking，您需要将（纬度、经度、高度）坐标系转换为类似于ECEF的笛卡尔坐标系：https://en.wikipedia.org/wiki/ECEF请查看https://github.com/lemmingapex/trilateration/issues/1 - Scott Wiedemann
我尚未尝试实现这个，但它有多准确？ - Guru Teja
我之前使用了上述的三边测量解决方案，你知道算法是如何处理这个问题的吗？比如说，如果三个参考点在一条直线上，那么圆将在两个地方相交，它应该计算哪一个。另外，由于噪声的影响，三个圆可能没有共同的交集区域，其中一个圆与其他每个圆都有一些公共区域，因此在这种情况下会有4个交点，它将如何确定质心。 - user3123372
1
我在一本电子书中找到了这个解决方案；

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

我将这个问题编写成了Java示例，对于三个圆形似乎工作得很好。然而，我不知道如何调整这个公式以涵盖解决方案中第四个和第五个点的三边测量问题。我的数学水平就是这样。
这是公式的代码：
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);

}

Here is a example of what I get

我希望有一个编程解决方案，可以适用于3个或以上的节点，并且在使用多个点时，会更加倾向于使用半径值较小的节点派生的点。
有人有任何想法吗？
要么扩展4个或更多节点的公式，要么实现更好的代码？
网页内容由stack overflow 提供, 点击上面的
可以查看英文原文，
原文链接