室内定位（牛顿迭代算法）（单点模拟测试）python

import numpy as np
import numpy.matlib
import matplotlib.pyplot as plt


N = 4  # 锚数量
M = 1  # 移动节点数

# 距离相关err(归一化到距离的噪声标准差)
distMeasurementErrRatio = 0.1  # 这意味着距离测量的精度是90，例如1米的测量距离的误差大约是0.1米（符合SWM1000模块精度）

networkSize = 5  # 我们考虑移动设备可以漫游的100x100的区域
# 在区域的4个顶点处设置锚点
anchorLoc = np.matlib.array(([0, 0],
                             [networkSize, 0],
                             [0, networkSize],
                             [networkSize, networkSize]
                             ))
# 为移动节点构建随机位置
mobileLoc = networkSize * np.matlib.rand(M, 2)

# 计算欧氏距离
distance = np.matlib.zeros((N, M))
for m in range(0, M):
    for n in range(0, N):
        # m是行(其中第m一个节点），n是列(到第n个锚点距离）
        distance[n, m] = np.sqrt((anchorLoc[n, 0] - mobileLoc[m, 0]) ** 2 + (anchorLoc[n, 1] - mobileLoc[m, 1]) ** 2)

# 噪声测量计算
distanceNoisy = distance + np.multiply(distance, distMeasurementErrRatio * (np.matlib.rand(N, M) - 1 / 2))

# 用牛顿法求解 迭代15次
numOfIteration = 15

# 初始用于猜测的新位置
mobileLocEst = networkSize * np.matlib.rand(M, 2)

for m in range(0, M):
    for i in range(0, numOfIteration):
        # 计算模拟点与锚点距离
        a = anchorLoc - np.matlib.repmat(mobileLocEst[m, :], N, 1)
        distanceEst = np.matlib.sqrt(np.matlib.sum(np.multiply(a, a), 1))
        # 计算导数
        distanceDrv = np.concatenate([np.true_divide((mobileLocEst[m, 0] - anchorLoc[:, 0].reshape(4, 1)), distanceEst),
                                      np.true_divide((mobileLocEst[m, 1] - anchorLoc[:, 1].reshape(4, 1)),
                                                     distanceEst)], 1)
        # delta值
        e = - (distanceDrv.T * distanceDrv).I * distanceDrv.T
        delta = e * (distanceEst - distanceNoisy[:, m])
        # 更新估计值
        mobileLocEst[m, :] = mobileLocEst[m, :] + delta.T


# 计算均方根误差
Err = np.mean(np.sqrt(np.sum(np.multiply((mobileLocEst - mobileLoc), (mobileLocEst - mobileLoc)))))

plt.plot(mobileLocEst[:, 0], mobileLocEst[:, 1], 'o', label="MobileLocEst")
plt.plot(anchorLoc[:, 0], anchorLoc[:, 1], 'o', label="AnchorLocation")
plt.plot(mobileLoc[:, 0], mobileLoc[:, 1], 'o', label="MobileLocation")
plt.xlabel("x")
plt.ylabel("y")
plt.legend(loc='best')
plt.title("Err:  "+str(Err)+" m")
plt.show()

if __name__ == '__main__':
    pass