def pf_process(particleNum): car_speed = 3 import groundtruth as GT import beaconLoc as bleLoc import beaconTable errorP = [] 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, 0.2) PARTICLE_COUNT = particleNum # Total number of particles 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() # initial distribution assigns each particle an equal probability particles = Particle.create_random(PARTICLE_COUNT, world) robbie = Robot(world) errorList = [] start_time_pf = ti.time() while INSData and len(INSData) > 20: insBuffer = [INSData.pop(0) for i in range(21)] beaconBuffer = {} startTime = insBuffer[0]['ts'] endTime = insBuffer[20]['ts'] groundTruth = None while gt[0]['ts'] < startTime: groundTruth = gt.pop(0) #"here we need to show the ground truth of car" # Read robbie's sensor if groundTruth: gtMaze = mg.pixelToMaze(groundTruth['x'], groundTruth['y']) robbie.position(gtMaze[0], gtMaze[1]) if beaconData: beaconTimeCursor = beaconData[0]['ts'] if abs(beaconTimeCursor - startTime) <= 1000 and beaconData: beaconBuffer = beaconData.pop(0) yaw = -insBuffer[0]['INSDict']['orientation'][0] + 280 #r_d = robbie.read_sensor(world) # Update particle weight according to how good every particle matches # robbie's sensor reading if beaconBuffer: beaconTable = beaconBuffer['table'] for p in particles: #print(p.x,p.y) weight = 0 for bleSignal in beaconTable: blePos = [bleSignal['x'], bleSignal['y']] rssi = bleSignal['rssi'] Loss = (abs(rssi) + 40) / 20 likelyhood = 1000 * (10**(-Loss)) particlePixelPos = mg.mazeGridToPixel(p.x, p.y) #print("hello ", blePos) dis = euclideanDistance(blePos, particlePixelPos) #print(dis) #weight += huber_gauss(dis,80)*likelyhood weight += huber_gauss(dis, 80) * likelyhood p.w = weight #print(p.w) for p in particles: if not world.is_free(*p.xy): p.w = 0 # ---------- Try to find current best estimate for display ---------- m_x, m_y, m_confident = compute_mean_point(particles, PARTICLE_COUNT) #print(m_x,m_y) # ---------- Show current state ---------- #world.show_particles(particles) #world.show_mean(m_x, m_y, m_confident) #world.show_robot(robbie) # ---------- Shuffle particles ---------- new_particles = [] # Normalise weights nu = sum(p.w for p in particles) if nu: for p in particles: p.w = p.w / nu # create a weighted distribution, for fast picking dist = WeightedDistribution(particles) for _ in particles: p = dist.pick() if p is None: # No pick b/c all totally improbable new_particle = Particle.create_random(1, world)[0] else: new_particle = Particle(p.x, p.y, heading=yaw, noisy=True) new_particles.append(new_particle) particles = new_particles # ---------- Move things ---------- #robbie.move(world) # Move particles according to my belief of movement (this may # be different than the real movement, but it's all I got) for p in particles: p.h = yaw # in case robot changed heading, swirl particle heading too p.advance_by(3) error = euclideanDistance(gtMaze, [m_x, m_y]) errorList.append(error) errorListSorted = sorted(errorList) X = [] for i in range(len(errorList)): X.append(i / len(errorList)) Y = [i * 0.2 for i in range(len(errorList))] #plt.plot(Y,errorList,color = 'red') rmsParticle = rms(errorList) #plt.xticks([]) #plt.ylim(0,0.07) #plt.xlim(0,3) #plt.plot([0,2],[rmsParticle,rmsParticle],linestyle='--',color='red') #plt.scatter(2,rmsParticle,s=50,color='red',marker='*') #plt.annotate("Particle Filter", xy=(2, rmsParticle), xytext = (1.8, rmsParticle+0.5)) #plt.show() period_pf = (ti.time() - start_time_pf) / len(errorList) #plt.scatter(2,period_pf/len(errorList),s=50,color='red',marker='*') #plt.annotate("Particle Filter", xy=(2, period_pf/len(errorList)), xytext = (1.8,period_pf/len(errorList)+0.002)) #plt.plot([0,2],[period_pf/len(errorList),period_pf/len(errorList)],linestyle='--',color='red') return errorList, period_pf
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()
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
beaconList.append(newBuffer) beaconDic = {} beaconDic['ts'] = timeStamp beaconDic['table'] = beaconList if beaconList: beaconLog.append(beaconDic) return beaconLog if __name__ == '__main__': import matplotlib.pyplot as plt beaconLog = logToList("ibeaconScanner.dat") for beacon in beaconLog: print(beacon) beaconInfos = beaconTable.getBeaconInfo() ts_pre = 0 for rssiTable in beaconLog: ts, locx, locy = beacon_locs(beaconInfos, rssiTable) if locx != -10000: print(ts, locx, locy) plt.xlim(600, 1100) #把x轴的刻度范围设置为-0.5到11,因为0.5不满一个刻度间隔,所以数字不会显示出来,但是能看到一点空白 plt.ylim(-2332, -1835) plt.scatter(locx, -locy) else: print(ts, {}) continue if ts_pre != 0: plt.pause((ts - ts_pre) / 1000)