iBeacon定位-三点定位实现

基本原理

Trilateration（三边测量）是一种常用的定位算法：

已知三点位置 (x1, y1), (x2, y2), (x3, y3)
已知未知点 (x0, y0) 到三点距离 d1, d2, d3
以 d1, d2, d3 为半径作三个圆，根据毕达哥拉斯定理，得出交点即未知点的位置计算公式：

( x1 - x0 )2 + ( y1 - y0 )2 = d12
( x2 - x0 )2 + ( y2 - y0 )2 = d22
( x3 - x0 )2 + ( y3 - y0 )2 = d32

3InYve

解法推导

设未知点位置为 (x, y)， 令其中的第一个球形 P1 的球心坐标为 (0, 0)，P2 处于相同纵坐标，球心坐标为 (d, 0)，P3 球心坐标为 (i, j)，三个球形半径分别为 r1, r2, r3，z为三球形相交点与水平面高度。则有：

r12 = x2 + y2 + z2
r22 = (x - d)2 + y2 + z2
r32 = (x - i)2 + (y - j)2 + z2
当 z = 0 时， 即为三个圆在水平面上相交为一点，首先解出 x:

x = (r12 - r22 + d2) / 2d
将公式二变形，将公式一的 z2 代入公式二，再代入公式三得到 y 的计算公式：

y = (r12 - r32 - x2 + (x - i)2 + j2) / 2j

java实现

     /**
      * evaluateCoordinates(三点定位核心算法)
      * TODO 三点定位核心算法
      *
      * @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    设定文件
      * @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);
    }