Mehrpunkt-Trilaterationsalgorithmus in Java

Question

Ich versuche, einen Trilaterationsalgorithmus in meine Android-App zu implementieren, um den Standort eines Benutzers zu bestimmen. Ich verwende Ultra-Breitband-Beacons, um die Entfernungen zu festen Punkten zu erhalten. Ich konnte das Verfahren in Trilateration Method Android Java vorgeschlagen anzupassen, wie folgt:

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

} 

Mit diesem Ansatz gibt mir einen Benutzerstandort, ist aber nicht sehr genau. Wie kann ich dies erweitern, um mehr als 3 bekannte Orte/Entfernungen zu verwenden? Idealerweise N Punkte mit N> = 3.

Answer 1

Ich fand diese Lösung in einem E-Buch;

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

I codiert diese in ein Java-Beispiel, und es scheint für drei Kreise ziemlich gut zu funktionieren. Ich habe jedoch keine Ahnung, wie ich diese Formel anpassen kann, um die Trilateration mit einem vierten und einem fünften Punkt in der Lösung abzudecken. Meine Mathematik ist einfach nicht so gut.

Mein Code für die Formel ist hier;

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

} 

Ich würde im Idealfall wie eine Code-Lösung, die mit 3+ Knoten arbeiten konnte und auch, wo mehrere Punkte verwendet wurden, würde die Lösung mehr auf dem Punkt von Knoten mit kleinem Radius Werten abgeleitet gewichten .

Wer hat irgendwelche Ideen?

Entweder, wie die Buchformel für 4+ Knoten oder eine bessere Code-Implementierung zu erweitern?

Answer 2

Bei richtiger Formulierung ist das Multilaterationsproblem ein Optimierungsproblem.

Die meisten wissenschaftlichen Beispiele, wie die auf wikipedia, befassen sich mit genau drei Kreisen und nehmen vollkommen genaue Informationen an. Diese Umstände erlauben viel einfachere Problemformulierungen mit genauen Antworten und sind in der Regel nicht zufriedenstellend für praktische Situationen wie die, die Sie beschreiben.

Das Problem in R 2 oder R 3 euklidischer Raum mit Abständen, die Messfehler enthalten, wird ein Bereich (Ellipse) oder das Volumen (Ellipsoid) von Interesse in der Regel an Stelle von einem Punkt erhalten. Wenn statt einer Region eine Punktschätzung gewünscht wird, sollte der Flächenschwerpunkt oder Volumenschwerpunkt verwendet werden. R Platz benötigt mindestens 3 nicht entartete Punkte und Abstände, um eine eindeutige Region zu erhalten; und in ähnlicher Weise benötigt der Raum mindestens 4 nicht-entartete Punkte und Abstände, um eine eindeutige Region zu erhalten.

Hier ist eine Open-Source-Java-Bibliothek, die Ihre Anforderungen leicht erfüllen wird: https://github.com/lemmingapex/Trilateration

Es verwendet ein beliebtes nicht-lineare kleinste Quadrate Optimierer, den Levenberg-Marquardt-Algorithmus, von 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);