Java Multipoint Trilateration Algorithm

Question

I am trying to embed the trilateration algorithm in my android app to determine the local location of the user. I use ultra-wideband beacons to get distances to fixed points. I was able to adapt the method suggested in the Trilateration method of 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 the user's location, but not very accurately. How can I expand this to use more than 3 known locations / distances? Ideally, N is the number of points where N> = 3.

Answer 1

When formulated correctly, the problem of multilateration is a problem of optimization.

Most scientific examples, such as wikipedia , engage in exactly three circles and suggest accurate information. These circumstances can greatly simplify the formulation of tasks with accurate answers and are usually not suitable for practical situations such as those that you describe.

A problem in R ² or R ^{3 is} Euclidean space with distances that contain a measurement error, a region of interest (ellipse) or volume (ellipsoid) of a point. If a point estimate is needed instead of an area, you must use the area centroid or volume centroid. R ² requires at least 3 non-degenerate points and distances to obtain a single region; and thus, the space R ³ requires at least 4 non-degenerate points and distances to obtain a single domain.

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

It uses the 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); 

Answer 2

I found this solution in an e-book;

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

I coded this using Java as an example, and it seems to work very well for three circles. However, I do not know how to adapt this formula to cover trilateration with the 4th and 5th point in the solution. My math is just not so good.

Here is my code for the formula:

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

Ideally, I would like to use a code solution that could work with 3 + nodes, and also, when several points were used, I could weigh the solution more towards the point obtained from nodes with small radius values.

Does anyone have any ideas?

Either how to expand the book formula for 4+ nodes, or is it better to implement the code?