このリンクでは、三辺測量のプロセスが示されています。
そして、これはローカリゼーション プロセス用の私の 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の場合、交差する 6 つの球があります。3 つの内側の球と 3 つの外側の球です。
それらの厚さはalpha（測定誤差）の量になります。
2 ポイントではなく 2 つの領域を取得します。しかし、与えられた 2 つの領域から点の位置を推定するにはどうすればよいでしょうか?
そのようなコードを書くために、私はこのアプローチを試みました：
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 次元位置特定を調査します。興味深いのは、この論文では、線形行列を構築するために二次方程式が使用されたため、4 つ以上のビーコン (参照) を使用して位置特定が行われたことです。ここでは、キーワードエラーを検索して、エラーがどのようにモデル化され、線形方程式がどのように構築されたかを確認できます。
さらに、問題の 2 次元バージョンについては、この論文を参照します。