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
室内定位(牛顿迭代算法)(单点模拟测试)python
最新推荐文章于 2023-01-12 18:41:32 发布