前言
三点定位法是蓝牙定位中非常实用的算法,但是实际使用的时候,我们可能会遇到超过三点的情况,那么此时一种可取的方式就是只取最强的三个信号,也可以通过最小二乘法来对多点进行快速求解。
思路
最小二乘法的精髓在于通过矩阵运算来快速求解。那么如何把蓝牙信号转换为矩阵呢?
我们假设有一组蓝牙坐标
P{(x1,y1),(x2,y2),...,(xn,yn)}和对应的蓝牙距离值(通过蓝牙RSSI计算得到)
D={d1,d2,...,dn},可设此时目标位置在
(x,y)处,我们可以得到公式:
⎩⎪⎪⎪⎨⎪⎪⎪⎧(x1−x)2+(y1−y)2=d12(x2−x)2+(y2−y)2=d22...(xn−x)2+(yn−y)2=dn2
化简得:
⎩⎪⎪⎪⎨⎪⎪⎪⎧x2+y2−2x1x−2y1y=d12−x12−y12x2+y2−2x2x−2y2y=d22−x22−y22...x2+y2−2xnx−2yny=dn2−xn2−yn2
对上面的式子同时减去第n行公式,消去
x2+y2,得:
⎩⎪⎪⎪⎨⎪⎪⎪⎧(xn−x1)x+(yn−y1)y=(d12−dn2+xn2+yn2−x12−y12)/2(xn−x2)x+(yn−y2)y=(d22−dn2+xn2+yn2−x22−y22)/2...(xn−xn−1)x+(yn−yn−1)y=(dn−12−dn2+xn2+yn2−xn−12−yn−12)/2
这么一来矩阵就出来了:
X=⎣⎢⎢⎡xn−x1xn−x2...xn−xn−1yn−y1yn−y2yn−yn−1⎦⎥⎥⎤(1)
β=[xy](1)
Y=⎣⎢⎢⎡(d12−dn2+xn2+yn2−x12−y12)/2(d22−dn2+xn2+yn2−x22−y22)/2...(dn−12−dn2+xn2+yn2−xn−12−yn−12)/2⎦⎥⎥⎤(1)
可知:
Xβ=Y
那么利用最小二乘法就可以快速求解
β的近似解
β′了:
β′=(XTX)−1XTY
实现
具体的实现涉及矩阵运算,这里也就不特别说明了,如果是python语言会比较容易(numpy库)。用Java的朋友可以看看这篇文章。