Algorytm trilateracji wielopunktowej w Javie

Question

Próbuję zaimplementować algorytm trilateracji w mojej aplikacji na Androida, aby określić wewnętrzną lokalizację użytkownika. Używam radiolatarni ultraszerokopasmowych, aby uzyskać odległości do stałych punktów. Udało mi się dostosować metodę zaproponowaną w Trilateration Method Android Java następująco:

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

} 

Stosując to podejście daje mi lokalizację użytkownika, ale nie jest strasznie dokładna. Jak mogę to rozszerzyć, aby korzystać z więcej niż 3 znanych lokalizacji/odległości? Idealnie N liczba punktów gdzie N> = 3.

Answer 1

Znalazłem to rozwiązanie w e-booku;

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

I kodowane to na przykład Java i wydaje się całkiem dobrze działa na 3 kołach. Jednak nie mam pojęcia, jak zaadaptować tę formułę, aby pokryć trilaterację czwartym i piątym punktem rozwiązania. Moja matematyka nie jest po prostu dobra.

Mój kod formuły jest tutaj;

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

} 

będę idealnie jak roztwór kod, który może współpracować z 3+ węzłów, a także, w których stosuje się wiele punktów, to waga roztworu bardziej w kierunku punktu pochodzących z węzłów przy małych wartościach promieniu .

Ktoś ma jakieś pomysły?

Albo jak rozszerzyć formułę książki dla 4+ węzłów, czy lepszą implementację kodu?

Answer 2

Prawidłowo sformułowany problem z multilateracją jest problemem optymalizacyjnym.

Większość przykładów naukowych, takich jak ten na wikipedia, dotyczy dokładnie trzech okręgów i przyjmuje idealnie dokładne informacje. Te okoliczności pozwalają na znacznie prostsze formułowanie problemów z dokładnymi odpowiedziami i zwykle nie są satysfakcjonujące dla praktycznych sytuacji, takich jak te, które opisujesz.

Problemem R lub R euklidesowa przestrzeni z odległości, które zawierają błąd pomiaru, obszar (elipsy) lub objętość (elipsoidy) będących przedmiotem zainteresowania, uzyskuje się zazwyczaj, a nie punktu. Jeśli zamiast regionu jest pożądane oszacowanie punktu, należy użyć środka ciężkości lub centroidu objętości obszaru. R Miejsce wymaga co najmniej 3 niezwyrodnionych punktów i odległości, aby uzyskać unikalny region; i podobnie R wymaga co najmniej 4 niezwyrodnionych punktów i odległości, aby uzyskać unikalny region.

Oto open source Java biblioteki, która będzie łatwo swoich potrzeb: https://github.com/lemmingapex/Trilateration

Wykorzystuje popularne optymalizator nieliniowych najmniejszych kwadratów algorytmu Levenberg-Marquardt z 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);