├── README.md ├── images ├── 1.png └── rmse.png └── landmarc.py /README.md: -------------------------------------------------------------------------------- 1 | # Indoor_Positioning_Algorithm 2 | Introduce some classic indoor positioning algorithms and code implementations 3 | landmarc.py 为 landmarc 算法的 python 实现。 4 | 5 | 运行结果如下： 6 | 7 | result of predict [[2.34335539 4.31106933] 8 | [4.66056494 4.67537382] 9 | [5.30972862 2.65652928] 10 | [6.66997345 7.33489215] 11 | [8.33493883 8.33058597]] 12 | mse: [0.09676413061163142, 0.10538215388148721, 0.1179721388002703, 0.11215275412587787, 0.4481151447145073] 0.1760772644267548 13 | 14 | 效果图如下（每次运行结果不同）： 15 | 16 | k = 3 时的定位效果图 17 | 18 | ![](images/1.png) 19 | 20 | 定位均方根误差（1-5 为 5 个待测标签的 rmse，system 为系统误差，即前五个的均值）： 21 | 22 | ![](images/rmse.png) 23 | -------------------------------------------------------------------------------- /images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangkai26/Indoor_Positioning_Algorithm.eg.landmarc/d0cfd74277364c474786cfeac46f9bee8df387ca/images/1.png -------------------------------------------------------------------------------- /images/rmse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangkai26/Indoor_Positioning_Algorithm.eg.landmarc/d0cfd74277364c474786cfeac46f9bee8df387ca/images/rmse.png -------------------------------------------------------------------------------- /landmarc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | # Calculate the Euclidean distance between two points 5 | def dist(x,y): 6 | return np.sqrt(np.sum(np.square(np.array(x) - np.array(y)))) 7 | 8 | 9 | # Calculate the Euclidean distance between Tags and Readers 10 | # eg. 121 Tags, 4 Readers,the matrix returned .shape = (4,121) 11 | def mat_dist(tags,readers): 12 | n = len(readers) 13 | m = len(tags) 14 | ref_dist = [[0] * m for _ in range(n)] 15 | for i in range(len(readers)): 16 | for j in range(len(tags)): 17 | ref_dist[i][j] = dist(readers[i],tags[j]) 18 | ref_dist = np.array(ref_dist) 19 | return ref_dist 20 | 21 | 22 | # Calculate the Euclidean distance between the rssi value 23 | # of the reference label and the test label 24 | # eg. ref.shape=(4,121),test.shape=(4,5),the matrix returned .shape=(5,121) 25 | def euclidean_distance(ref,test): 26 | m = test.shape[1] # 5 27 | n = ref.shape[1] # 121 28 | num_read = ref.shape[0] 29 | d = [[0] * n for i in range(m)] 30 | for i in range(m): 31 | for j in range(n): 32 | var = 0 33 | for k in range(num_read): 34 | var += np.square(ref[k][j] - test[k][i]) 35 | d[i][j] = np.sqrt(var) 36 | return np.array(d) 37 | 38 | 39 | # 选出 d 中每行前 k 个最大值，d.shape = (5,121) 40 | # k 默认为 3 41 | # the naming of be_knn is a bit random 42 | # used to selected the top k largest values of each row,k defaults to 3 43 | def be_knn(d,k=3): 44 | m,n = d.shape 45 | k = 3 46 | d = np.sort(d) 47 | sort_d = np.zeros((m,k)) 48 | for i in range(m): 49 | sort_d[i] = d[i][-k:] 50 | return sort_d 51 | 52 | 53 | # this knn is different to above be_knn 54 | # knn is used to find the coordinates of the top k largest values in each row 55 | def knn(d,k=3): 56 | m,n = d.shape 57 | k = 3 58 | d = np.argsort(d) 59 | sort_d = np.zeros((m,k)) 60 | for i in range(m): 61 | sort_d[i] = d[i][:k] 62 | return sort_d 63 | 64 | 65 | # Start positioning 66 | # sort_d_cor,sort_d .shape both was(5,3),sort_d is rssi,sort_d_cor is coordinates 67 | def predict(sort_d_cor,sort_d,ref_tag): 68 | m,k = sort_d_cor.shape 69 | res = np.zeros((m,2)) 70 | for i in range(m): 71 | x,y = 0,0 72 | sum_w = 0 73 | for j in range(k): 74 | sum_w += 1/sort_d[i][j] 75 | w = [0] * k 76 | for j in range(k): 77 | w[j] = 1/sort_d[i][j] / sum_w 78 | for j in range(k): 79 | # print(ref_tag[int(sort_d_cor[i][j])]) 80 | x += w[j] * ref_tag[int(sort_d_cor[i][j])][0] 81 | y += w[j] * ref_tag[int(sort_d_cor[i][j])][1] 82 | print(w) 83 | res[i] = x,y 84 | return res 85 | 86 | 87 | # Calculate the RMSE of positioning 88 | def pre_error(test,pred): 89 | m,n = len(test),len(pred) 90 | if m != n: 91 | print('demension error!') 92 | error = [] 93 | for i in range(m): 94 | x1,y1 = test[i] 95 | x2,y2 = pred[i] 96 | error.append(np.sqrt(np.square(x1-x1))+np.square(y1-y2)) 97 | system_error = sum(error)/m 98 | return error,system_error 99 | 100 | 101 | 102 | # generate readers,There are four readers. 103 | # Try to avoid integers, because the denominator cannot be 0 in division. 104 | # The number and position coordinates can be changed according to your needs. 105 | reader = [[0.5,0.5],[10.5,0.5],[0.5,10.5],[10.5,10.5]] 106 | reader = np.array(reader) 107 | 108 | # generate reference tag,total 121. 109 | ref_tag = [] 110 | for i in range(11): 111 | for j in range(11): 112 | ref_tag.append([i,j]) 113 | ref_tag = np.array(ref_tag) 114 | 115 | ref_read_dist = mat_dist(ref_tag,reader) 116 | 117 | # ref_read_dist.shape = (4,121) 118 | # generate the RSSI of reference Tags 119 | # 对数路径损耗模型 120 | n = 2.2 # 距离衰减因子 121 | ref_rssi = -30 - 10 * n * np.log(ref_read_dist) #shape=(4,121) 122 | 123 | # generate tag to be located 124 | # generate the RSSI of test Tags 125 | test_tag = np.array([[2,4],[5,5],[5,3],[7,7],[8,9]]) 126 | test_read_dist = mat_dist(test_tag,reader) # .shape=(4.5) no problem 127 | test_rssi = -30 - 10 * n * np.log(test_read_dist) + np.random.randint(0,8,size=(4,5))#shape=(4,5) 128 | 129 | D = euclidean_distance(ref_rssi,test_rssi) # (5,121) 130 | 131 | sort_d = be_knn(D) 132 | sort_d_cor = knn(D) # .shape = (5,3) 133 | 134 | # This two line codes is used for verification 135 | # print(sord_d[0][0]) 136 | # print(ref_tag[26]) 137 | 138 | pred_coor = predict(sort_d_cor,sort_d,ref_tag) 139 | print("result of predict",pred_coor) 140 | 141 | # Calculate the RMSE 142 | error,system_error = pre_error(test_tag,pred_coor) 143 | print('mse:',error,system_error) 144 | 145 | 146 | 147 | # x and y coordinates used for drawing 148 | x_ref = [a[0] for a in ref_tag] 149 | y_ref = [a[1] for a in ref_tag] 150 | # x and y of readers 151 | x_read = [a[0] for a in reader] 152 | y_read = [a[1] for a in reader] 153 | 154 | x_test = [a[0] for a in test_tag] 155 | y_test = [a[1] for a in test_tag] 156 | 157 | x_pred = [a[0] for a in pred_coor] 158 | y_pred = [a[1] for a in pred_coor] 159 | # plt.plot(xs[0],ys[0],"ro",[0,0,10,10],[0,10,0,10],'ro') 160 | fig = plt.figure() 161 | ax = fig.add_subplot(2,2,3) 162 | ax.plot(x_ref,y_ref,"ro",x_read,y_read,'yo') 163 | 164 | ax.plot(x_ref,y_ref,"ro",label='reference_tag') 165 | ax.plot(x_read,y_read,'y^',label='reader') 166 | ax.plot(x_test,y_test,'go',label='test_tag') 167 | ax.plot(x_pred,y_pred,'bo',label='pred_tag') 168 | 169 | for i in range(5): 170 | plt.arrow(x_test[i],y_test[i], x_pred[i]-x_test[i],y_pred[i]-y_test[i], ec='red', shape='full', length_includes_head='True', head_width=0.02) 171 | 172 | # ax.legend(loc='best') 173 | ax.legend(loc=2, bbox_to_anchor=(1.05,1.0),borderaxespad = 0.) 174 | # ax.legend(bbox_to_anchor=(1.25,1),loc='upper right') 175 | 176 | name_list = [1,2,3,4,5,'system'] 177 | error.append(system_error) 178 | ax2 = fig.add_subplot(2,2,2) 179 | ax2.bar(x=range(len(error)),height=error,color='rgb',tick_label=name_list) 180 | plt.show() 181 | --------------------------------------------------------------------------------