Algoritmo di trilaterazione multipunto in Java

Question

Sto cercando di implementare un algoritmo di trilaterazione nella mia app per Android per determinare la posizione interna di un utente. Sto usando i beacon a banda ultralarga per ottenere le distanze dai punti fissi. Sono stato in grado di adattare il metodo suggerito in Trilateration Method Android Java come segue:

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

} 

Usando questo approccio mi dà una posizione dell'utente, ma non è molto preciso. Come posso estendere questo per utilizzare più di 3 posizioni/distanze note? Idealmente N numero di punti in cui N> = 3.

Answer 1

Ho trovato questa soluzione in un e-book;

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

ho codificato questo in un esempio Java e sembra funzionare abbastanza bene per 3 cerchi. Tuttavia, non ho idea di come adattare questa formula per coprire la trilaterazione con un 4 ° e 5 ° punto nella soluzione. La mia matematica non è così bella.

Il mio codice per la formula è qui;

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

} 

Vorrei idealmente come una soluzione di codice che potrebbe funzionare con 3+ nodi e anche, dove più punti sono stati usati, sarebbe la soluzione più peso verso il punto derivata dai nodi con valori di raggio piccole .

Qualcuno ha qualche idea?

O come espandere la formula del libro per 4+ nodi o un'implementazione del codice migliore?

Answer 2

Quando è stato formulato correttamente, il problema di multilaterazione è un problema di ottimizzazione.

La maggior parte degli esempi accademici, come quello su wikipedia, si occupa esattamente di tre cerchi e si basa su informazioni perfettamente accurate. Queste circostanze consentono formulazioni di problemi molto più semplici con risposte esatte e in genere non sono soddisfacenti per situazioni pratiche come quella che descrivi.

Il problema in R o R 3 spazio euclideo con distanze che contengono errore di misura, un'area (ellisse) o il volume (ellissoide) di interesse è solitamente ottenuto invece di un punto. Se si desidera una stima puntuale anziché una regione, è necessario utilizzare il centroide dell'area o il centroide del volume. R spazio richiede almeno 3 punti non degenerati e distanze per ottenere una regione unica; e allo stesso modo lo spazio R richiede almeno 4 punti e distanze non degenerati per ottenere una regione unica.

Qui è una libreria open source di Java che facilmente soddisfare le vostre esigenze: https://github.com/lemmingapex/Trilateration

Esso utilizza un popolare non lineari minimi quadrati ottimizzatore, l'algoritmo di Levenberg-Marquardt, da 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);