基本原理
Trilateration（三邊測量）是一種常用的定位算法：
已知三點(diǎn)位置 (x1, y1), (x2, y2), (x3, y3)
已知未知點(diǎn) (x0, y0) 到三點(diǎn)距離 d1, d2, d3
以 d1, d2, d3 為半徑作三個(gè)圓纳猪，根據(jù)畢達(dá)哥拉斯定理拳氢，得出交點(diǎn)即未知點(diǎn)的位置計(jì)算公式：
( x1 - x0 )2 + ( y1 - y0 )2 = d12
( x2 - x0 )2 + ( y2 - y0 )2 = d22
( x3 - x0 )2 + ( y3 - y0 )2 = d32
解法推導(dǎo)
設(shè)未知點(diǎn)位置為 (x, y)， 令其中的第一個(gè)球形 P1 的球心坐標(biāo)為 (0, 0)柳爽，P2 處于相同縱坐標(biāo)卫旱，球心坐標(biāo)為 (d, 0)垒玲，P3 球心坐標(biāo)為 (i, j)唱歧，三個(gè)球形半徑分別為 r1, r2, r3朦乏，z為三球形相交點(diǎn)與水平面高度锥累。則有：
r12 = x2 + y2 + z2
r22 = (x - d)2 + y2 + z2
r32 = (x - i)2 + (y - j)2 + z2
當(dāng) z = 0 時(shí)， 即為三個(gè)圓在水平面上相交為一點(diǎn)集歇，首先解出 x:
x = (r12 - r22 + d2) / 2d
將公式二變形桶略，將公式一的 z2 代入公式二，再代入公式三得到 y 的計(jì)算公式：
y = (r12 - r32 - x2 + (x - i)2 + j2) / 2j
java實(shí)現(xiàn)
/**
* evaluateCoordinates(三點(diǎn)定位核心算法)
* TODO 三點(diǎn)定位核心算法
*
* @Title: evaluateCoordinates
* @Description: TODO
* @param @param xa
* @param @param ya
* @param @param xb
* @param @param yb
* @param @param xc
* @param @param yc
* @param @param ra
* @param @param rb
* @param @param rc
* @param @return 設(shè)定文件
* @return Point 返回類型
* @throws
*/
public static Point evaluateCoordinates(BigDecimal xa,BigDecimal ya, BigDecimal xb, BigDecimal yb, BigDecimal xc, BigDecimal yc, BigDecimal ra, BigDecimal rb, BigDecimal rc){
BigDecimal a2 = ra.multiply(ra);
BigDecimal b2 = rb.multiply(rb);
BigDecimal c2 = rc.multiply(rc);
BigDecimal xa2 = xa.multiply(xa);
BigDecimal xb2 = xb.multiply(xb);
BigDecimal xc2 = xc.multiply(xc);
BigDecimal ya2 = ya.multiply(ya);
BigDecimal yb2 = yb.multiply(yb);
BigDecimal yc2 = yc.multiply(yc);
BigDecimal rbc = b2.subtract(c2);
BigDecimal xbc = xb2.subtract(xc2);
BigDecimal ybc = yb2.subtract(yc2);
BigDecimal va = rbc.subtract(xbc).subtract(ybc).divide(new BigDecimal("2.0"));
BigDecimal rba = b2.subtract(a2);
BigDecimal xba = xb2.subtract(xa2);
BigDecimal yba = yb2.subtract(ya2);
BigDecimal vb = rba.subtract(xba).subtract(yba).divide(new BigDecimal("2.0"));
BigDecimal xcb = xc.subtract(xb);
BigDecimal xab = xa.subtract(xb);
BigDecimal yab = ya.subtract(yb);
BigDecimal ycb = yc.subtract(yb);
BigDecimal va1 = va.multiply(xab);
BigDecimal vb1 = vb.multiply(xcb);
BigDecimal aa = yab.multiply(xcb);
BigDecimal ab = ycb.multiply(xab);
BigDecimal y1 = vb1.subtract(va1);
BigDecimal y2 = aa.subtract(ab);
BigDecimal y = y1.divide(y2,10,RoundingMode.DOWN);
BigDecimal x;
if (xcb.signum() != 0) {
x = va.subtract((y.multiply(ycb))).divide(xcb,10,RoundingMode.DOWN);
} else {
x = vb.subtract(y.multiply(yab)).divide(xab,10,RoundingMode.DOWN);
}
return new Point(x,y);
}
