基于维基百科方程的Java多点化实现

Question

我正在尝试在java中编写一个多点定位算法来测试我正在处理的项目，但是我遇到了一些问题。我按照维基百科页面here（eqn 7）上的算法，但是当我尝试它时，我得到了错误的答案。

这是我正在尝试的数据

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

注意时间延迟我假设v = 1，为了简单起见，我想出答案。因为我做了这个，我做了一些时间延迟，并确定传感器知道延迟（因为v = 1）与距离相同（sqrt（deltax ^ 2 + deltay ^ 2 + deltaz ^ 2））

但是，当我通过算法运行时，得到x = 0.1064，y = 4.2419，z = 8.5968。我知道这不是一个解决方案，因为距离点的距离与时间不匹配。

这是我在java中的算法（我假设最接近/最小的时间是第一次在算法的第一次切割中为了简单起见）

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

任何人都可以帮我找到算法实现的问题，我用来测试它的数据，或者我做的任何假设？谢谢！