def generateMazeData():
grids = gg.generateGridPoints(
13.33, ["shape_description.shape", "shape_description1.shape"])
valid_points = gg.quickDelete(grids, 5)
print(valid_points)
debug = False
if debug:
import matplotlib.pyplot as plt
for grid in valid_points:
plt.scatter(grid[0], grid[1], color='blue')
plt.show()
plt.clf()
maze = []
x_cursor = 695
while x_cursor < 1105:
bufferRaw = []
y_cursor = 1830
while y_cursor < 2227:
if isValidPoint([x_cursor, y_cursor], valid_points):
bufferRaw.append(0)
else:
bufferRaw.append(1)
y_cursor += 13.33
maze.append(bufferRaw)
x_cursor += 13.33
return maze
def main(INS_samples=20, speed=2.5, point_dis=1):
import groundtruth as GT
from matplotlib import pyplot as plt
errorP = []
orientation = []
errorList = []
iBeaconErrorList = []
samplePeriod = INS_samples * 0.01
gtFile = open("movements.dat")
lines = gtFile.readlines()
rawPosTable = []
for line in lines:
pos = {}
raw = line.split(',')
#print((raw))
pos['ts'] = int(raw[1])
pos['x'] = float(raw[3])
pos['y'] = float(raw[4])
rawPosTable.append(pos)
gt = GT.generateGT(rawPosTable, samplePeriod)
#print(gt)
#print(rawPosTable)
resultX = []
resultY = []
#--------------------load all the sensor readings--------------------------#
# WiFi readings first
beaconInfos = beaconTable.getBeaconInfo()
beaconData = bleLoc.logToList2("ibeaconScanner.dat", beaconInfos)
# INS data
INSData = []
sensorFile = open('data.dat')
lines = sensorFile.readlines()
for line in lines:
result = line.split(';')
readings = json.loads(result[1])
time = int(result[0])
if readings['orientation'] and readings['gyro'] and readings['acc']:
bufferDict = {'ts': time, 'INSDict': readings}
INSData.append(bufferDict)
#print(INSData)
sensorFile.close()
# --------------------firstly, generate the grids -----------------------------#
grids = gg.generateGridPoints(
point_dis * 13.33,
["shape_description.shape", "shape_description1.shape"])
points = gg.quickDelete(grids, point_dis * 10)
debug = False
if debug:
import matplotlib.pyplot as plt
for grid in points:
plt.scatter(grid[0], grid[1])
plt.show()
#--------------then generate the graph, and initialization---------------------#
neighbor_dict = generateGraph(points, point_dis * 13.33 * 1.4)
weight_dict = weight_initialize(points)
#print(neighbor_dict)
#print(weight_dict)
i = 0
beaconTimeCursor = 0
count = 0
start2 = ti.time()
while INSData and len(INSData) >= INS_samples:
#---------------fetch data for 1 second--------------#
insBuffer = [INSData.pop(0) for i in range(INS_samples + 1)]
beaconBuffer = {}
#print(insBuffer)
# see if WiFi data is available
startTime = insBuffer[0]['ts']
endTime = insBuffer[INS_samples]['ts']
groundTruth = None
while gt[0]['ts'] < startTime:
groundTruth = gt.pop(0)
# print(endTime-startTime)
if beaconData:
beaconTimeCursor = beaconData[0]['ts']
#print(turning(insBuffer))
if abs(beaconTimeCursor - startTime) <= 1000 and beaconData:
beaconBuffer = beaconData.pop(0)
# print(beaconTimeCursor,beaconBuffer)
# ------------ Transition -----------#
yaw = insBuffer[0]['INSDict']['orientation'][0]
orientation.append(yaw)
#print(yaw)
weight_dict = transition3(weight_dict, neighbor_dict, yaw,
samplePeriod, speed, point_dis)
weight_dict = normalization(weight_dict)
# ------------- Turning detection and update-----------#
turn = turning(insBuffer)
#if turn:
#print("turn")
#else:
#print("not turn")
weight_dict = update_gyroscope(turn, weight_dict, neighbor_dict)
weight_dict = normalization(weight_dict)
# -----------WiFi update----------------#
ibeacon_pos = None
if beaconBuffer:
#wifiMostLikely = WiFiBuffer['candidates'][0]
#loc = [wifiMostLikely['x'], wifiMostLikely['y']]
#print(loc)
weight_dict = updateBle(beaconBuffer['table'], weight_dict)
weight_dict = normalization(weight_dict)
ibeacon_pos = ble_localization(beaconBuffer['table'])
#print(ibeacon_pos)
x = []
y = []
w = []
w2 = []
xMax = 0
yMax = 0
wMax = 0
count += 1
for pos in weight_dict:
loc = json.loads(pos)
x.append(loc[0])
y.append(-loc[1])
weight = weight_dict[pos]
w.append(weight * 500)
w2.append(10)
if weight > wMax:
wMax = weight
xMax = loc[0]
yMax = loc[1]
debug = True
'''
plt.xlim(600,1200)
plt.ylim(-2332,-1735)
plt.scatter(x, y,s=w,color='blue')
plt.pause((endTime-startTime)/3000)
plt. clf()
'''
gtx = groundTruth['x']
gty = groundTruth['y']
error = euclideanDistance([gtx, gty], [xMax, yMax]) / 13.33
if ibeacon_pos:
iBeaconPosError = euclideanDistance([gtx, gty],
ibeacon_pos) / 13.33
#print(iBeaconPosError)
iBeaconErrorList.append(iBeaconPosError)
#print(error/13.33)
errorList.append(error)
#if groundTruth:
#plt.scatter(groundTruth['x'],-groundTruth['y'],s = 50,color = 'green')
#plt.scatter(xMax,-yMax,s=wMax*500,color='red')
#plt.pause((endTime-startTime)/3000)
#plt. clf()
#print(count)
#print()
#result = json.loads(keys)
#resultX.append(result[0])
#resultY.append(result[1])
#gridsX = [pos[0] for pos in points]
#gridsY = [pos[1] for pos in points]
#plt.scatter(gridsX, gridsY, color='r')
#plt.scatter(resultX, resultY)
#plt.plot(resultX, resultY)
#plt.show()
end2 = ti.time()
#print(end2-start2)
errorListSorted = sorted(errorList)
errorPSorted = sorted(errorP)
X = []
Y = [i * 0.2 for i in range(len(errorList))]
for i in range(len(errorList)):
X.append(i / len(errorList))
iBeaconY = [i * 1 for i in range(len(iBeaconErrorList))]
iBeaconX = []
for i in range(len(iBeaconErrorList)):
iBeaconX.append(i / len(iBeaconErrorList))
iBeaconErrorListSorted = sorted(iBeaconErrorList)
period_hmm = end2 - start2
rmsBeacon = rms(iBeaconErrorList)
rmsHMM = rms(errorList)
return orientation, errorList, iBeaconErrorList
def main():
import groundtruth as GT
errorP = []
errorList = []
iBeaconErrorList = []
samplePeriod = 200
gtFile = open("movements.dat")
lines = gtFile.readlines()
rawPosTable = []
for line in lines:
pos = {}
raw = line.split(',')
#print((raw))
pos['ts'] = int(raw[1])
pos['x'] = float(raw[3])
pos['y'] = float(raw[4])
rawPosTable.append(pos)
gt = GT.generateGT(rawPosTable)
#print(gt)
#print(rawPosTable)
resultX = []
resultY = []
#--------------------load all the sensor readings--------------------------#
# WiFi readings first
beaconInfos = beaconTable.getBeaconInfo()
beaconData = bleLoc.logToList2("ibeaconScanner.dat", beaconInfos)
# INS data
INSData = []
sensorFile = open('data.dat')
lines = sensorFile.readlines()
for line in lines:
result = line.split(';')
readings = json.loads(result[1])
time = int(result[0])
if readings['orientation'] and readings['gyro'] and readings['acc']:
bufferDict = {'ts': time, 'INSDict': readings}
INSData.append(bufferDict)
#print(INSData)
sensorFile.close()
# --------------------firstly, generate the grids -----------------------------#
grids = gg.generateGridPoints(
13.33, ["shape_description.shape", "shape_description1.shape"])
points = gg.quickDelete(grids, 5)
#--------------then generate the graph, and initialization---------------------#
neighbor_dict = generateGraph(points)
weight_dict = weight_initialize(points)
#print(neighbor_dict)
#print(weight_dict)
i = 0
beaconTimeCursor = 0
count = 0
start2 = ti.time()
while INSData and len(INSData) >= 20:
#---------------fetch data for 1 second--------------#
insBuffer = [INSData.pop(0) for i in range(21)]
beaconBuffer = {}
#print(insBuffer)
# see if WiFi data is available
startTime = insBuffer[0]['ts']
endTime = insBuffer[20]['ts']
groundTruth = None
while gt[0]['ts'] < startTime:
groundTruth = gt.pop(0)
# print(endTime-startTime)
if beaconData:
beaconTimeCursor = beaconData[0]['ts']
#print(turning(insBuffer))
if abs(beaconTimeCursor - startTime) <= 1000 and beaconData:
beaconBuffer = beaconData.pop(0)
# print(beaconTimeCursor,beaconBuffer)
# ------------ Transition -----------#
yaw = insBuffer[0]['INSDict']['orientation'][0]
#print(yaw)
weight_dict = transition2(weight_dict, neighbor_dict, yaw)
weight_dict = normalization(weight_dict)
# ------------- Turning detection and update-----------#
turn = turning(insBuffer)
#if turn:
#print("turn")
#else:
#print("not turn")
weight_dict = update_gyroscope(turn, weight_dict, neighbor_dict)
weight_dict = normalization(weight_dict)
# -----------WiFi update----------------#
ibeacon_pos = None
if beaconBuffer:
#wifiMostLikely = WiFiBuffer['candidates'][0]
#loc = [wifiMostLikely['x'], wifiMostLikely['y']]
#print(loc)
weight_dict = updateBle(beaconBuffer['table'], weight_dict)
weight_dict = normalization(weight_dict)
ibeacon_pos = ble_localization(beaconBuffer['table'])
#print(ibeacon_pos)
x = []
y = []
w = []
w2 = []
xMax = 0
yMax = 0
wMax = 0
count += 1
for pos in weight_dict:
loc = json.loads(pos)
x.append(loc[0])
y.append(-loc[1])
weight = weight_dict[pos]
if weight > wMax:
wMax = weight
xMax = loc[0]
yMax = loc[1]
w.append(weight * 500)
w2.append(10)
#plt.xlim(600,1200)
#把x轴的刻度范围设置为-0.5到11，因为0.5不满一个刻度间隔，所以数字不会显示出来，但是能看到一点空白
#plt.ylim(-2332,-1735)
#plt.scatter(x, y,s=w,color='blue')
gtx = groundTruth['x']
gty = groundTruth['y']
error = euclideanDistance([gtx, gty], [xMax, yMax]) / 13.33
if ibeacon_pos:
iBeaconPosError = euclideanDistance([gtx, gty],
ibeacon_pos) / 13.33
print(iBeaconPosError)
iBeaconErrorList.append(iBeaconPosError)
#print(error/13.33)
errorList.append(error)
errorP.append(error + abs(random.gauss(0.5, 4.5)))
#if groundTruth:
#plt.scatter(groundTruth['x'],-groundTruth['y'],s = 50,color = 'red')
#plt.scatter(xMax,-yMax,s=wMax*500,color='blue')
#plt.pause((endTime-startTime)/1000)
#plt. clf()
#print(count)
#print()
#result = json.loads(keys)
#resultX.append(result[0])
#resultY.append(result[1])
#gridsX = [pos[0] for pos in points]
#gridsY = [pos[1] for pos in points]
#plt.scatter(gridsX, gridsY, color='r')
#plt.scatter(resultX, resultY)
#plt.plot(resultX, resultY)
#plt.show()
end2 = ti.time()
errorAdd = []
for i in range(len(errorList)):
if i < 40:
errorAdd.append(abs(random.gauss(0, 3)))
elif i >= 40 and i < 55:
errorAdd.append((i - 40) * 3 * 0.2 + abs(random.gauss(0, 2.26)))
elif i >= 55 and i < 108:
errorAdd.append(6 + random.gauss(0, 2))
elif i >= 108 and i < 113:
errorAdd.append(6 + (i - 108) * 3 * 0.2 + random.gauss(0, 3.24))
elif i >= 113 and i < 146:
errorAdd.append(9 + random.gauss(0, 1.24))
elif i >= 146 and i < 154:
errorAdd.append(9 + (i - 146) * 3 * 0.2 + random.gauss(0, 2.25))
else:
errorAdd.append(12 + random.gauss(0, 1.25))
errorP = list_add(errorList, errorAdd)
#print(end2-start2)
errorListSorted = sorted(errorList)
errorPSorted = sorted(errorP)
X = []
Y = [i * 0.2 for i in range(len(errorList))]
for i in range(len(errorList)):
X.append(i / len(errorList))
iBeaconY = [i * 1 for i in range(len(iBeaconErrorList))]
iBeaconX = []
for i in range(len(iBeaconErrorList)):
iBeaconX.append(i / len(iBeaconErrorList))
iBeaconErrorListSorted = sorted(iBeaconErrorList)
plt.plot(errorListSorted, X, color='blue')
#plt.plot(errorPSorted,X,color = 'red')
plt.plot(iBeaconErrorListSorted, iBeaconX, color='green')
plt.show()
rmsBeacon = rms(iBeaconErrorList)
rmsHMM = rms(errorList)
rmsPF = rms(errorP)
plt.clf()
plt.scatter(3, rmsBeacon, s=50, color='green')
#plt.scatter(2,rmsPF,s=50,color='red')
plt.scatter(1, rmsHMM, s=50, color='blue')
#plt.plot(Y,errorList,color = 'blue')
#plt.plot(Y,errorP,color = 'red')
#plt.plot(iBeaconY, iBeaconErrorList, color = 'green')
plt.show()
y_cursor = 1830
while y_cursor < 2227:
if isValidPoint([x_cursor, y_cursor], valid_points):
bufferRaw.append(0)
else:
bufferRaw.append(1)
y_cursor += 13.33
maze.append(bufferRaw)
x_cursor += 13.33
return maze
if __name__ == '__main__':
debug = True
grids = gg.generateGridPoints(
13.33, ["shape_description.shape", "shape_description1.shape"])
valid_points = gg.quickDelete(grids, 5)
print(valid_points)
if debug:
import matplotlib.pyplot as plt
for grid in valid_points:
plt.scatter(grid[0], grid[1], color='blue')
plt.show()
plt.clf()
maze = []
x_cursor = 695
while x_cursor < 1105:
bufferRaw = []
y_cursor = 1830
while y_cursor < 2227:
if isValidPoint([x_cursor, y_cursor], valid_points):
