def selectiveLogDistanceParameters(channel, rssi):
if USE_ALT_METHOD:
X = altMethodParams[channel]["X"]
n = altMethodParams[channel]["n"]
return distance.altMethod(rssi=rssi, X=X, n=n)
else:
A = logDistanceParams[channel]["A"]
n = logDistanceParams[channel]["n"]
return distance.logDistancePathLoss(rssi=rssi, rssi_d0=A, d0=1.0, n=n, stDev=log_d_st_dev)
def main(args):
sql = "SELECT * FROM `positioning`.`rssi_data` WHERE CRC = 1 AND Address = '00:ce:11:52:a6:0f' ORDER BY `Date` DESC LIMIT 10"
cursor.execute(sql)
row = cursor.fetchone()
while row:
rssi = row['RSSI']
channel = row['Channel']
f = distance.bleChannelToFrequency(channel)
# Log distance
rssi_d0 = -40.0
d0 = 1.0
n = 2.6
xo = 14.1
dist_1 = round(distance.logDistancePathLoss(rssi, rssi_d0, d0, n, xo),
2)
# ITU distance model
N = 30
Lf = 14
n = 1
dist_2 = round(distance.ituDistance(rssi, f, N, Lf, n), 2)
# Empirical model
n = 4
A = -45.0
dist_3 = round(distance.empiricalDistance(rssi, n, A), 2)
print("RSSI: ", rssi, "\t Channel: ", channel, "\t Log-dist: ", dist_1,
"\t ITU: ", dist_2, "\t Empirical: ", dist_3)
row = cursor.fetchone()
def main(argv):
global GRAPH_ENABLED
global DB_ENABLED
label = None
totalCounter = 0
allPositions = dict()
setupEnable = True
truePosition = Position(0, 0, 0)
tagColor = ["b", "y", "c"]
opts, args = getopt.getopt(argv, "cdfghilsxyz:o", [
"channel=", "database=", "filter=", "graph=", "ip=", "label=",
"setup=", "x=", "y=", "z="
])
del (args)
if len(opts) == 0:
print(
"A label must be set for the test to start: filter_test.py --label '<label>'"
)
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print(
"A label must be set for the test to start: filter_test.py --label '<label>'"
)
sys.exit()
elif opt in ("-i", "--ip"):
ip = arg
print(ip)
elif opt in ("-l", "--label"):
label = arg
print("Label for test: ", label)
elif opt in ("-g", "--graph"):
GRAPH_ENABLED = True
elif opt in ("-d", "--database"):
DB_ENABLED = True
elif opt in ("-s", "--setup"):
if str(arg) == "true":
setupEnable = True
else:
configFile = str(arg)
elif opt in ("-x", "--trueX"):
print("x: ", arg)
truePosition.x = float(arg)
elif opt in ("-y", "--trueY"):
truePosition.y = float(arg)
elif opt in ("-z", "--trueZ"):
truePosition.z = float(arg)
else:
print(
"A label must be set for the test to start: filter_test.py --label '<label>'"
)
sys.exit(2)
if truePosition.x == 0 and truePosition.y == 0 and truePosition.z == 0:
truePosition = "NULL"
else:
truePosition = (truePosition.x, truePosition.y, truePosition.z)
print(truePosition)
if not label:
print(
"A label must be set for the test to start: filter_test.py --label '<label>'"
)
sys.exit(2)
interval = Interval.Interval(2, sendServerInfo, args=[
ip,
])
print("Starting positioning test... Press CTRL + C to stop.")
interval.start()
if setupEnable:
# Run configuration of node setup
setupConfig = dict()
setupConfig["listenSocket"] = listenSocket
setupConfig["broadcastSocket"] = broadcastSocket
nodes = setup.setupNodes(setupConfig, fromFile=False)
configFile = "nodeConfig.txt"
print(nodes)
else:
nodes = setup.setupNodes(None, fromFile=True, fileName=configFile)
tags = dict()
if GRAPH_ENABLED:
import matplotlib.pyplot as plt
plt.ion()
fig = plt.figure()
ax = fig.add_subplot(111)
plt.axis([-3, 10, -3, 10])
ax.set_aspect(1)
ax.grid(color='#cccccc', linestyle='-', linewidth=1)
for _, node in nodes.items():
node.setActiveStatus(False)
while True:
try:
sql = ''
rawData, addr = listenSocket.recvfrom(1024)
data = json.loads(rawData)
if not "nodeID" in data:
continue
ip = addr[0]
nodeID = data['nodeID']
timestamp = data['timestamp']
address = data['address']
if "txPower" in data:
txPower = data['txPower']
else:
txPower = defaultTxPower
rssi = data['RSSI']
crc = data['CRC']
lpe = data['LPE']
counter = data['counter']
syncController = data['syncController']
channel = data['channel']
# Get true node position from config file or setup
nodePosition = (nodes[nodeID].position.x, nodes[nodeID].position.y,
nodes[nodeID].position.z)
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, NULL, %d, NULL, %d, %d, %d, '%s', '%s', NULL, NULL, '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address, channel, counter,
txPower, rssi, 0, crc, lpe, syncController, label, configFile,
nodePosition, settings)
# If CRC or LPE fails, store to database and move on to next iteration
if crc == 0 or lpe == 1:
if DB_ENABLED:
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, NULL, %d, NULL, %d, %d, %d, '%s', '%s', NULL, NULL, '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address, channel,
counter, txPower, rssi, 0, crc, lpe, syncController,
label, configFile, nodePosition, settings)
cursor.execute(sql)
db.commit()
continue
if nodeID not in nodes:
continue
if address not in nodes[nodeID].tags:
nodes[nodeID].tags[address] = Tag(address)
nodes[nodeID].tags[address].currentCounter = counter
nodes[nodeID].tags[address].currentCounterAdvCount = 0
if address not in tags:
tags[address] = (1, 1, 1)
allPositions[address] = list()
nodes[nodeID].setActiveStatus(True)
nodes[nodeID].tags[address].rssi.append(rssi)
if nodes[nodeID].tags[address].currentCounter == counter:
nodes[nodeID].tags[address].currentCounterAdvCount += 1
if DB_ENABLED:
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, NULL, %d, NULL, %d, %d, %d, '%s', '%s', NULL, '%s', '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address, channel,
counter, txPower, rssi, 0, crc, lpe, syncController,
label, configFile, truePosition, nodePosition,
settings)
else:
nodes[nodeID].tags[address].currentCounter = counter
nodes[nodeID].tags[address].currentCounterAdvCount = 1
selectedChannelRssi = max(nodes[nodeID].tags[address].rssi)
nodes[nodeID].tags[address].kalman.predict()
nodes[nodeID].tags[address].kalman.update(selectedChannelRssi)
filteredRssi = nodes[nodeID].tags[address].kalman.x[0]
nodes[nodeID].tags[address].filteredRssi = filteredRssi
nodes[nodeID].tags[address].rssi = list()
if DB_ENABLED:
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, %f, %d, NULL, %d, %d, %d, '%s', '%s', NULL, '%s', '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address, channel,
counter, txPower, rssi, filteredRssi, 0, crc, lpe,
syncController, label, configFile, truePosition,
nodePosition, settings)
if totalCounter > 0 and totalCounter % 6 == 0:
if GRAPH_ENABLED:
ax.cla()
ax.set_xlim((-5, 10))
ax.set_ylim((-5, 15))
plt.grid()
tagKey = -1
for tagAddress in tags:
tagKey += 1
positions = list()
color = 'k'
for _, node in nodes.items():
if node.getActiveStatus(
) == False or tagAddress not in node.tags:
continue
x = node.position.x
y = node.position.y
z = node.position.z
# Log-distance path loss model
filteredRssi = node.tags[tagAddress].filteredRssi
node.tags[
tagAddress].distance = distance.logDistancePathLoss(
filteredRssi,
rssi_d0=LOG_DISTANCE_RSSI_D0,
d0=LOG_DISTANCE_D0,
n=LOG_DISTANCE_N,
stDev=LOG_DISTANCE_ST_DEV)
radius = node.tags[tagAddress].distance
positions.append((x, y, z, radius))
if DB_ENABLED:
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, %f, NULL, %f, %d, %d, %d, '%s', '%s', NULL, '%s', '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address,
channel, counter, txPower, rssi, filteredRssi,
radius, crc, lpe, syncController, label,
configFile, truePosition, nodePosition,
settings)
if GRAPH_ENABLED:
circle = plt.Circle((x, y),
radius=radius,
color=color,
alpha=0.1)
center = plt.Circle((x, y),
radius=0.1,
color='r',
alpha=1)
ax.add_patch(circle)
ax.add_patch(center)
if len(positions) >= NUMBER_OF_NODES_TO_USE:
sortedPositions = sorted(positions, key=lambda x: x[3])
estimatedPosition = multi.multilateration(
sortedPositions[:NUMBER_OF_NODES_TO_USE],
dimensions=POSITION_DIMENSIONS)
allPositions[tagAddress].append(estimatedPosition)
tags[tagAddress] = estimatedPosition
if truePosition != "NULL":
error_3d = np.sqrt(
np.square(estimatedPosition[0] -
truePosition[0]) +
np.square(estimatedPosition[1] -
truePosition[1]) +
np.square(estimatedPosition[2] -
truePosition[2]))
error_2d = np.sqrt(
np.square(estimatedPosition[0] -
truePosition[0]) +
np.square(estimatedPosition[1] -
truePosition[1]))
ax.text(
11, 3, ''.join(
("Error 2D: ", str(round(error_2d, 2)))))
ax.text(
11, 1, ''.join(
("Error 3D: ", str(round(error_3d, 2)))))
print("x: ", round(estimatedPosition[0],
2), "\ty: ",
round(estimatedPosition[1], 2), "\tz: ",
round(estimatedPosition[2], 2),
"\t Error 3D: ", round(error_3d,
2), "\tError 2D: ",
round(error_2d, 2))
if DB_ENABLED:
sql = "INSERT INTO `%s` VALUES(NULL, NULL, '%s', '%s', %d, '%s', %d, %d, %d, %d, %f, NULL, %f, %d, %d, %d, '%s', '%s', '%s', '%s', '%s', '%s')" % (
DB_TABLE, nodeID, ip, timestamp, address,
channel, counter, txPower, rssi, filteredRssi,
radius, crc, lpe, syncController, label,
configFile, estimatedPosition, truePosition,
nodePosition, settings)
if GRAPH_ENABLED:
positionIndicator = plt.Circle(
estimatedPosition,
radius=0.20,
color=tagColor[tagKey],
alpha=1)
ax.add_patch(positionIndicator)
if truePosition != "NULL":
truePositionIndicator = plt.Circle(
truePosition,
radius=0.20,
color="g",
alpha=1)
ax.add_patch(truePositionIndicator)
if GRAPH_ENABLED:
plt.draw()
plt.pause(0.0001)
if DB_ENABLED:
cursor.execute(sql)
db.commit()
totalCounter += 1
except KeyboardInterrupt:
print("Shutting down interval...")
interval.stop()
if GRAPH_ENABLED:
ax.cla()
ax.set_xlim((-5, 10))
ax.set_ylim((-5, 15))
plt.grid()
for _, node in nodes.items():
x = node.position.x
y = node.position.y
z = node.position.z
center = plt.Circle((x, y), radius=0.1, color='r', alpha=1)
ax.add_patch(center)
tagKey = -1
for tagAddress in tags:
x = list()
y = list()
tagKey += 1
for position in allPositions[tagAddress][30:]:
x.append(position[0])
y.append(position[1])
plt.plot(x, y, '-', color=tagColor[tagKey], alpha=0.1)
if truePosition != "NULL":
truePositionIndicator = plt.Circle(truePosition,
radius=0.20,
color="g",
alpha=1)
ax.add_patch(truePositionIndicator)
plt.draw()
plt.pause(0.0001)
input("Press enter to exit")
break
continue
# Estimate distances and multilateration
if numberOfNodesToUse < maxNumberOfNodes:
pData = [(nodePositions[i][0], nodePositions[i][1], nodePositions[i][2], rssis[i], channels[i]) for i in range(0, len(rssis))]
pData.sort(key=itemgetter(3), reverse=True)
rssis = [pData[i][3] for i in range(len(rssis))]
channels = [pData[i][4] for i in range(len(channels))]
if SELECTIVE_PL_PARAMS:
distanceEstimates = [selectiveLogDistanceParameters(channels[i], rssis[i]) for i in range(len(rssis))]
else:
if USE_ALT_METHOD:
distanceEstimates = [distance.altMethod(rssi=rssi, X=altMethodX, n=altMethodN) for rssi in rssis]
else:
distanceEstimates = [distance.logDistancePathLoss(rssi, rssi_d0, d0, log_d_n, log_d_st_dev) for rssi in rssis]
if numberOfNodesToUse < maxNumberOfNodes:
pData = [(pData[i][0], pData[i][1], pData[i][2], distanceEstimates[i]) for i in range(0, len(distanceEstimates))]
else:
pData = [(nodePositions[i][0], nodePositions[i][1], nodePositions[i][2], distanceEstimates[i]) for i in range(0, len(distanceEstimates))]
positionEstimate = multi.multilateration(pData[:numberOfNodesToUse], startingPoint=multiStartingPoint, plotEnabled=False, dimensions=3, bounds=multiBounds)
if not ONLY_FILTERED:
rawDistanceEstimates = [distance.logDistancePathLoss(rssi, rssi_d0, d0, log_d_n, log_d_st_dev) for rssi in rawRssis]
rawPData = [(nodePositions[i][0], nodePositions[i][1], nodePositions[i][2], rawDistanceEstimates[i]) for i in range(0, len(rawDistanceEstimates))]
rawPositionEstimate = multi.multilateration(rawPData, plotEnabled=False, dimensions=3, bounds=multiBounds, startingPoint=multiStartingPoint)
if initial:
initial = False
def main(argv):
global nodes
opts, args = getopt.getopt(
argv, "cghi:o", ["channel=", "graph=", "ip=", "addr=", "address="])
del (args)
totalCounter = 0
if len(opts) == 0:
print("udp_listener.py -i <server IP address>")
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print("udp_listener.py -i <server IP address>")
sys.exit()
elif opt in ("-i", "-a", "--ip", "--address"):
ip = arg
print(ip)
else:
print("udp_listener.py -i <server IP address>")
sys.exit(2)
interval = Interval.Interval(2, sendServerInfo, args=[
ip,
])
print("Starting Interval, press CTRL+C to stop.")
interval.start()
# Initiate figure
fig = plt.figure()
plt.ion()
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim((-5, 5))
ax.set_ylim((-5, 5))
x0, x1 = ax.get_xlim()
y0, y1 = ax.get_ylim()
ax.set_aspect(abs(x1 - x0) / abs(y1 - y0))
while True:
try:
rawData, addr = listenSocket.recvfrom(1024)
data = json.loads(rawData)
ip = addr[0]
nodeID = data['nodeID']
timestamp = data['timestamp']
address = data['address']
rssi = data['RSSI']
crc = data['CRC']
lpe = data['LPE']
counter = data['counter']
times[ip] = timestamp
if crc == 1 and lpe == 0:
if nodeID not in nodes:
nodes[nodeID] = Node(nodeID)
if address not in nodes[nodeID].tags:
nodes[nodeID].tags[address] = Tag(address)
nodes[nodeID].tags[address].currentCounter = counter
nodes[nodeID].tags[address].currentCounterAdvCount = 0
if address not in tags:
tags[address] = 1
if nodes[nodeID].tags[address].currentCounter == counter:
nodes[nodeID].tags[address].currentCounterAdvCount += 1
else:
nodes[nodeID].tags[address].currentCounter = counter
nodes[nodeID].tags[address].currentCounterAdvCount = 1
if nodes[nodeID].tags[address].currentCounterAdvCount == 3:
nodes[nodeID].tags[address].rssi.append(rssi)
selectedChannelRssi = max(nodes[nodeID].tags[address].rssi)
nodes[nodeID].tags[address].kalman.predict()
nodes[nodeID].tags[address].kalman.update(
selectedChannelRssi)
filteredRssi = nodes[nodeID].tags[address].kalman.x[0]
nodes[nodeID].tags[address].filteredRssi = filteredRssi
# Log-distance path loss model
nodes[nodeID].tags[address].distance = round(
distance.logDistancePathLoss(filteredRssi,
rssi_d0=-40.0,
d0=1.0,
n=2.6,
xo=1.1),
ndigits=2)
print("node ID: ", nodeID, "\tIP: ", ip, "\tAddress: ",
address, "\tFiltered RSSI: ",
nodes[nodeID].tags[address].filteredRssi,
"\tDistance: ", nodes[nodeID].tags[address].distance)
nodes[nodeID].tags[address].rssi = list()
totalCounter += 1
if totalCounter > 0 and totalCounter % 20 == 0:
ax.cla()
ax.set_xlim((-3, 5))
ax.set_ylim((-3, 5))
plt.grid()
color = "b"
for tagAddress in tags:
positions = list()
if color == "b":
color = "k"
else:
color = "b"
for _, node in nodes.items():
x = node.position.x
y = node.position.y
radius = node.tags[tagAddress].distance
positions.append((x, y, radius))
circle = plt.Circle((x, y),
radius=radius,
color=color,
alpha=0.1)
center = plt.Circle((x, y),
radius=0.1,
color='r',
alpha=1)
ax.add_patch(circle)
ax.add_patch(center)
if len(positions) > 2:
position = multi.multilateration(positions)
tags[tagAddress] = position
positionIndicator = plt.Circle(tags[tagAddress],
radius=0.15,
color=color,
alpha=1)
ax.add_patch(positionIndicator)
plt.draw()
plt.pause(0.0001)
except KeyboardInterrupt:
print("Shutting down interval...")
interval.stop()
break