Multi-point trilateration algorithm in Java

Question

I'm trying to implement a trilateration algorithm into my Android app to determine a user's indoor location. I'm using ultra-wideband beacons to get the distances to fixed points. I was able to adapt the method suggested in Trilateration Method Android Java as follows:

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

}

Using this approach gives me a user location, but is not terribly accurate. How can I extend this to use more than 3 known locations/distances? Ideally N number of points where N>=3.

Shogun

This will certainly help you: gis.stackexchange.com/questions/40660/… – Sedberry

It looks like that link only provides a solution using a third-party software package called Mathematica. I'm needing something that is in Java. Ideally I won't have to include a third-party library or SDK, but simply adjust the above algorithm. – Shogun

They do use that for the number crunching but the math is still the same using non-linear least squares The Apache Math Library has all the functions you will need – Sedberry

Honestly, the math is a bit over my head so trying to convert that Mathematica formula into a Java function is problematic. Could you provide a code snippet? Interestingly, there seems to be a problem with the algorithm I pasted above. When I place myself really close to one of the beacons (like within 1 foot), the trilateration results in something like 15 meters away. I also tried to implement the algorithm at code.google.com/p/talking-points-3/source/browse/trunk/… but got a similar result. – Shogun

@Shogun Did you found a solution since then? I'm facing the same issue... – Mcintire

Answer 1

When formulated in the correct manner, the multilateration problem is an optimization problem.

Most scholarly examples, like the one on wikipedia, deal with exactly three circles and assume perfectly accurate information. These circumstances allow for much simpler problem formulations with exact answers, and are usually not satisfactory for practical situations like the one you describe.

The problem in R² or R³ euclidean space with distances that contain measurement error, an area (ellipse) or volume (ellipsoid) of interest is usually obtained instead of a point. If a point estimate is desired instead of a region, the area centroid or volume centroid should be used. R² space requires at least 3 non-degenerate points and distances to obtain a unique region; and similarly R³ space requires at least 4 non-degenerate points and distances to obtain a unique region.

Here is a open source java library that will easily meet your needs: https://github.com/lemmingapex/Trilateration

It uses a popular nonlinear least squares optimizer, the Levenberg-Marquardt algorithm, from 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);

Benzocaine

I doesnt give me the right result in my case... Have you tried it with real location point with latitude and latitude? – Mcintire

@Mcintire You would need to convert coordinates in (lat, long, altitude) to a cartesian coordinate system like ECEF: en.wikipedia.org/wiki/ECEF See github.com/lemmingapex/trilateration/issues/1 – Benzocaine

I have not tried implementing this, but how accurate is it? – Carolus

I was using the above solution for trilat, Do you know how the algorithm approaches for the solution, say for example all three reference points are in a straight line, then the circles will meet in two places, which one it should count. another like due to nise three circles doent have a common intersected area, one of the circle has some common area with each of the other circle, so we have 4 intersection point here, how it will determine the centroid in this case – Difficulty

Answer 2

I found this solution in an e-book;

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

I coded this into a Java example and it seems to work pretty well for 3 circles. However, I have no idea how to adapt this formula to cover trilateration with a 4th and 5th point in the solution. My maths is just not that good.

My code for the formula is here;

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

}

I would ideally like a code solution that could work with 3+ nodes and also, where multiple points were used, would weight the solution more towards the point derived from nodes with small radius values.

Anyone got any ideas?

Either how to expand the book formula for 4+ nodes, or a better code implementation?

Drag