2

在这个链接中，给出了三边测量的过程。

这是我用于本地化过程的 java 代码：

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;

}

但是现在，如本文所示，我想将一些误差引入距离测量。
对于错误alpha，将有六个球体相交：三个内球体和三个外球体。
它们的厚度将是量alpha（测量误差）。

我会得到两个区域而不是两个点。但是我如何估计给定两个区域的点的位置？

为了编写这样的代码，我尝试了这种方法：

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

这给了我null所有的时间。

你能帮我解决这个问题吗？

编辑：功能intersection()与localize(). 它不选择任何点，但同时返回。

论文摘要：
本文研究了使用成对距离和距离测量误差的影响的 3-D 定位。有趣的是，在论文中，二次方程被用来构建线性矩阵，因此，定位是通过使用四个以上的信标（参考）来完成的。在这里，您可以搜索关键字error并查看如何对错误进行建模以及如何构建线性方程。

此外，关于该问题的二维版本，我将参考这篇论文。

4

0 回答 0