Java中的多点三边测量算法

Question

我正在尝试在我的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 个圆圈来说似乎效果很好。但是，我不知道如何调整这个公式以涵盖解决方案中第四个和第五个点的三边测量。我的数学不太好。

我的公式代码在这里；

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+节点，要么更好的代码实现？