Trilateration with distance measurement errors

In this link, the process of trilateration is given.

And this is my java code for the localization process:

public static double[] localize(final double[] p1, final double[] p2, final double[] p3, final double[] p4, final double r1, final double r2, final double r3, final double r4)
{

    double[] ex = normalize(difference(p2,p1));
    double i = dot(ex, difference(p3, p1));
    double[] ey = normalize(difference(difference(p3,p1), scalar(i,ex)));
    double[] ez = cross(ex, ey);
    double d = distance(p2, p1);
    if(d - r1 >= r2 || r2 >= d + r1 )
        return null;
    double j = dot(ey, difference(p3, p1));
    double x = ((r1*r1) - (r2*r2) + (d*d)) / (2*d);
    double y = (((r1*r1) - (r3*r3) + (i*i) + (j*j)) / (2*j)) - ((i*x) / j);
    double z = r1*r1- x*x - y*y;
    if(z < 0)
        return null;
    double z1 = Math.sqrt(z);
    double z2 = z1*-1;

    double[] result1 = new double[]{p1[0], p1[1], p1[2]};
    result1 = add(result1, scalar(x,ex));
    result1 = add(result1, scalar(y,ey));
    result1 = add(result1, scalar(z1,ez));

    double[] result2 = new double[]{p1[0], p1[1], p1[2]};
    result2 = add(result2, scalar(x,ex));
    result2 = add(result2, scalar(y,ey));
    result2 = add(result2, scalar(z2,ez));

    double d1 = Math.abs(distance(result1, p4) - r4);
    double d2 = Math.abs(distance(result2, p4) - r4);
    if(d1<=d2)
        return result1;
    else
        return result2;

}

But now, as shown in this paper, I want to instroduce some errors to distance measurements.
For an error alpha , There will be six spheres intersecting: three inner spheres and three outer spheres.
Their thickness will be amount of alpha (the measurement error).

I will get two areas instead of two points. But how do I estimate the position of a point given two areas?

In order to write such code, I tried this approach:

public static boolean inside(final double[] p, double[] c, final double r, final double error)
{
    if(Math.pow(p[0]-c[0],2) + Math.pow(p[1]-c[1],2) + Math.pow(p[2]-c[2],2) >= r-error)
        if(Math.pow(p[0]-c[0],2) + Math.pow(p[1]-c[1],2) + Math.pow(p[2]-c[2],2) <= r+error)
            return true;
    return false;


}

public static double[] localize(final double[] p1, final double[] p2, final double[] p3, final double[] p4, final double r1, final double r2, final double r3, final double r4, final double error)
{
    double[][][] result = new double[8][3][3];
    result[0] = intersection(p1,p2,p3,r1-error,r2-error,r3-error);
    result[1] = intersection(p1,p2,p3,r1-error,r2-error,r3+error);
    result[2] = intersection(p1,p2,p3,r1-error,r2+error,r3-error);
    result[3] = intersection(p1,p2,p3,r1-error,r2+error,r3+error);
    result[4] = intersection(p1,p2,p3,r1+error,r2-error,r3-error);
    result[5] = intersection(p1,p2,p3,r1+error,r2-error,r3+error);
    result[6] = intersection(p1,p2,p3,r1+error,r2+error,r3-error);
    result[7] = intersection(p1,p2,p3,r1+error,r2+error,r3+error);
    for(int i=0; i<8; i++)
        if(result[i] != null)
            for(int j=0; j<2; j++)
                if(inside(result[i][j], p4, r4, error))
                    return result[i][j];
    return null;
}

Which gives me null all the times.

Could you help me to get through this?

Edit: The function intersection() is same as localize() . It does not pick any point, but returns both.

Brief summary of the paper:
The paper investigates the 3-D localization by using pairwise distances and the effect of distance measurement errors. The interesting part is, in the paper, the quadratic equations were used to build linear matrices and therefore, the localization was done by using more than four beacons(references). Here, you can search for the keyword error and see how the error was modelled and how the linear equatios were built.

Moreover, I'll refer to this paper for the 2-D version of the problem.

链接地址: http://www.djcxy.com/p/14872.html

上一篇: 3 WiFI强度信号计算距离的三边测量

下一篇: 具有距离测量误差的三边测量