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
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)
PARTICLE_COUNT = 2000 # 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()