Java Multilateration Implementierung basierend auf Wikipedia Gleichungen

Question

Ich versuche einen multilateration Algorithmus in Java zu schreiben, um es für ein Projekt zu testen, an dem ich arbeite, aber habe einige Probleme. Ich folgte dem Algorithmus auf der Wikipedia-Seite here (Gl. 7), aber wenn ich es ausprobiere, bekomme ich die falsche Antwort. Hier

ist die Daten, die ich es versuche mit

double[] truth = new double[] {1, 4, 8}; 
double[][] sensorLocations = new double[][] { 
     {0, 0, 0}, 
     {1, 15, 8}, 
     {13, 4, 8}, 
     {1, 4, 20}   
}; 
double[] timeDelays = new double[] {9, 11, 
      12, 12 }; 

Hinweis für die Zeitverzögerungen ich unter der Annahme bin v = 1 der Einfachheit halber in der Antwort zu finden. Da ich dies tat, machte ich einige Zeitverzögerungen und positionierte die Sensoren mit der Gewissheit, dass die Verzögerung (da v = 1) gleich der Entfernung ist (sqrt (deltax^2 + deltay^2 + deltaz^2))

Wenn ich jedoch meinen Algorithmus durchführe, bekomme ich x = 0,1064, y = 4,2499, z = 8,5968. Ich weiß, das kann keine Lösung sein, weil die Entfernung von den Punkten nicht mit den Zeiten übereinstimmt.

Hier ist mein Algorithmus in Java (ich davon aus, dass die nächste/kleinste Zeit den ersten Punkt wieder der Einfachheit halber in den ersten Schnitt des Algorithmus war)

public double[] CalculatePosition(double[][] sensors, double[] timeDelays) 
{ 
    double[] position = new double[3]; 

    //calculate the planes... 
    double v = 1; 
    double vt1 = v*timeDelays[0]; 

    double[] a = new double[timeDelays.length - 1]; 
    double[] b = new double[timeDelays.length - 1]; 
    double[] c = new double[timeDelays.length - 1]; 
    double[] d = new double[timeDelays.length - 1]; 
    for(int m = 1; m < timeDelays.length; m++) 
    { 
     double vtm = v*timeDelays[m]; 
     a[m-1] = 2*sensors[m][0]/vtm - 2*sensors[0][0]/vt1; 
     b[m-1] = 2*sensors[m][1]/vtm - 2*sensors[0][1]/vt1; 
     c[m-1] = 2*sensors[m][2]/vtm - 2*sensors[0][2]/vt1; 
     d[m-1] = vtm - vt1 - 
       (sensors[m][0] * sensors[m][0] + sensors[m][1] * sensors[m][1] + 
         sensors[m][2] * sensors[m][2])/vtm + 
       (sensors[0][0] * sensors[0][0] + sensors[0][1] * sensors[0][1] + 
         sensors[0][2] * sensors[0][2])/vt1; 
     //negate d to make it in the right form for Gaussian elimination 
     //i.e. from Ax + By + cZ + D = 0 to Ax + By + cZ = D 
     d[m-1] = -d[m-1]; 
    } 

    //Calculate where they intersect best 
    //Gaussian elimination for now... 
    //make the 2 and 3 a 0 
    double mult2 = -a[1]/a[0]; 
    a[1] += mult2 * a[0]; 
    b[1] += mult2 * b[0]; 
    c[1] += mult2 * c[0]; 
    d[1] += mult2 * d[0]; 

    double mult3 = -a[2]/a[0]; 
    a[2] += mult3 * a[0]; 
    b[2] += mult3 * b[0]; 
    c[2] += mult3 * c[0]; 
    d[2] += mult3 * d[0]; 

    mult3 = -b[2]/b[1]; 
    a[2] += mult3 * a[1]; 
    b[2] += mult3 * b[1]; 
    c[2] += mult3 * c[1]; 
    d[2] += mult3 * d[1]; 

    //now use substitution to get the answer! 
    position[2] = d[2]/c[2]; 
    position[1] = (d[1] - position[2] * c[1])/b[1]; 
    position[0] = (d[0] - position[1] * b[0] - position[2] * c[0])/b[0]; 


    return position; 
} 

Kann mir jemand helfen, das Problem mit der finden Algorithmus-Implementierung, die Daten, die ich benutze, um es zu testen, oder irgendwelche Annahmen, die ich gemacht habe? Danke!

Answer 1

einen bestimmten Grund für folgende:

double[] a = new double[timeDelays.length - 1]; 
double[] b = new double[timeDelays.length - 1]; 
double[] c = new double[timeDelays.length - 1]; 
double[] d = new double[timeDelays.length - 1]; 

weil unter Ihnen drittes Element zugreifen, und dies würde überlaufen, wenn Länge von timeDelays 3.

ist