Многоточечный алгоритм трилатерации в Java

Question

Я пытаюсь внедрить алгоритм trilateration в свое приложение для 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

Я нашел это решение в электронной книге;

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+ узлов, либо улучшить реализацию кода?

Answer 2

При правильной формулировке проблема многоуровневости является проблемой оптимизации.

Большинство научных примеров, таких как один на wikipedia, касаются ровно трех кругов и предполагают точную информацию. Эти обстоятельства позволяют получить более простые формулировки проблем с точными ответами и обычно не подходят для практических ситуаций, подобных тем, которые вы описываете.

Проблема в R или R 3 евклидовом пространстве с расстояния, которые содержат ошибки измерения, область (Эллипс) или объем (эллипсоид), представляющий интерес, обычно получают вместо точки. Если необходима точечная оценка вместо области, необходимо использовать центроид площади или объемный центроид. R пространство требует не менее 3 невырожденных точек и расстояний для получения уникальной области; и аналогично R пространство требует не менее 4 невырожденных точек и расстояний для получения уникальной области.

Вот с открытым исходным кодом Java библиотека, которая будет легко удовлетворить ваши потребности: https://github.com/lemmingapex/Trilateration

Он использует популярный оптимизатор нелинейных наименьших квадратов, алгоритм Левенберга-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);