多点定位算法

在室内定位中，部署蓝牙信标，在终端接收到蓝牙信标的广播信号，计算RSSI值，推算出各信标到终端的距离，由此实现定位。

RSSI距离转换

• d：距离
• A：RSSI在1m时候的初始值
• RSSI：rssi值
• n：环境衰减因子，需要实际环境拟合计算得出
  计算公式如下：
  d = 10^{(A-RSSI)/(10*n)}

# 蓝牙距离算法
A = -59 # RSSI at 1m
n = 3.3  # 环境衰减因子，需要实际环境拟合计算得出
RSSI = input('请输入RSSI值：')
d = 10 ** ((A-int(RSSI))  / (10 * n))
print('距离为：%s'%d)

多点定位

因为距离为估算，以蓝牙信标做圆，未必能相交于一点。如下图：

可以通过正规方程求解最优解，以下是代码。

import numpy as np

# 多个点(x,y,d)
a = (4,3,5**(1/2))
b = (4,0,3**(1/2))
c = (3,3,2)
d = (1,1,2)
all_point = [a,c,b,d]

X_list = []
y_list = []
for i in all_point[1:]:
    # x
    X_list.append([all_point[0][0]-i[0],all_point[0][1]-i[1]])
    # y
    yi=[(i[2]**2-all_point[0][2]**2+all_point[0][0]**2+all_point[0][1]**2-i[0]**2-i[1]**2)/2,]
    y_list.append(yi)

X = np.array(X_list)
y = np.array(y_list)

# 正规方程法，其中X和y是array格式
def Normal_Equation(X,y):
    Xt = np.transpose(X)
    XtX = np.dot(Xt,X)
    Xty = np.dot(Xt,y)
    beta = np.linalg.solve(XtX,Xty)
    return beta

beta = Normal_Equation(X,y)
beta

print((beta[0][0],beta[1][0]))

输出为：

(2.904255319148936, 1.1595744680851063)

可见计算结果很符合需求