Trilateration and Min-Max algorithms implementation for indoor localization problems.

minmax.py

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd 
import os



# def main():
# path = 'D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Pengukuran Variasi Jarak Ulangan/'
path = 'D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Pengukuran Variasi Jarak dan Manusia/'
cases = os.listdir(path)
# cases = ['4D4.csv'] # for testing purpose
print(path)
for case in cases:
	# read data
	df = pd.read_csv(path+case)

	# drop time column
	df = df.drop(columns=["Time"])

	# drop NaN values
	df = df.dropna()

	# take only data from column 1-2
	df = df.iloc[:,0:3]

	# Rename columns to AP1, AP2, and AP3
	df.columns = ["AP1", "AP2", "AP3"]

	#~~~~~~~~~~~~~~~~ Distance calculation from rssi using ple data ~~~~~~~~~~~~~~~#

	# Add path loss exponent and rssi at d0 parameters
	# Create a lambda function to convert RSSI data into distance
	dist = lambda k, n, rssi: 10**((k - rssi) / (10*n))

	# path loss parameters
	rssi_d0 = -49
	ple = 2.255

	# Add distance columns in df 
	for i in range(1,4):
		df["dist%d"%i] = dist(rssi_d0, ple, df["AP%d"%i])


	# Determining AP coordinate from case 
	# 1Dx means d = 1, 2Dx means d = 2, cont..

	if case[0] == "1":
		d = 1
	elif case[0] == "2":
		d = 2
	elif case[0] == "3":
		d = 3
	else:
		d = 4

	# Setting APs coordinates
	# AP1
	x1 = 0
	y1 = 0

	# AP2
	x2 = 0
	y2 = 1 * d

	# AP3
	x3 = 1 * d
	y3 = 1 * d

	xcoords = [x1, x2, x3]
	ycoords = [y1, y2, y3]

	# Target coordinates
	case = case.upper()
	if case[1:3] == "D1":
		xreal = (1/2) * d
		yreal = 1*d
	elif case[1:3] == "D2":
		xreal = (1/4) * d
		yreal = (3/4) * d
	elif case[1:3] == "D3":
		xreal = (1/2) * d
		yreal = (1/2) * d
	elif case[1:3] == "D4":
		xreal = (1/2) * d
		yreal = 0 * d

	#~~~~~~~~~~~~~~~~~~~~~ Min and Max coordinates calculations ~~~~~~~~~~~~~~~~~~~~#
	# Add xmax, xmin, ymin, and ymax of all APs into df
	for i in range(1,4):
		df["x%d_max"%i] = xcoords[i-1] + df["dist%d"%i]
		df["x%d_min"%i] = xcoords[i-1] - df["dist%d"%i]
		df["y%d_max"%i] = ycoords[i-1] + df["dist%d"%i]
		df["y%d_min"%i] = ycoords[i-1] - df["dist%d"%i]

	#print(df.head())

	#~~~~~~~~~~~~~~~~~~~~~ Min and Max coordinates calculations ~~~~~~~~~~~~~~~~~~~~#

	# Concantenate all the max and min coordinates
	xmax = np.array(pd.concat([df["x1_max"], df["x2_max"], df["x3_max"]]).sort_values(ascending=True))
	xmin = np.array(pd.concat([df["x1_min"], df["x2_min"], df["x3_min"]]).sort_values(ascending=False))
	ymax = np.array(pd.concat([df["y1_max"], df["y2_max"], df["y3_max"]]).sort_values(ascending=True))
	ymin = np.array(pd.concat([df["y1_min"], df["y2_min"], df["y3_min"]]).sort_values(ascending=False))

	# assign 100 values for each minmax and maxmin values
	# minmax = minimum value from a set of maximum values
	# maxmin = maximum value from a set of minimum values 

	xminmax = xmax[:100]
	xmaxmin = xmin[:100]
	yminmax = ymax[:100]
	ymaxmin = ymin[:100]

	#~~~~~~~~~~~~~~~~~~~~~~~~~  Min-Max Target calculations ~~~~~~~~~~~~~~~~~~~~~~~#
	x_pred = (xminmax + xmaxmin)/2
	y_pred = (yminmax + ymaxmin)/2

	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Accuracy, Precision, Resolution #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

	# Calculating Accuracy 
	sqe = np.sqrt((x_pred - xreal)**2 + (y_pred - yreal)**2)
	mse = np.mean(sqe)
	acc = mse + 0

	# Calculating Precision
	prc = np.mean([np.std(x_pred), np.std(y_pred)])

	# Calculating Resolution
	xco = np.array(x_pred)
	yco = np.array(y_pred)

	xco.sort()
	yco.sort()

	rslx = np.mean(xco[90:100]) - np.mean(xco[0:10])
	rsly = np.mean(yco[90:100]) - np.mean(yco[0:10])
	rsl = np.mean([rslx, rsly])


	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Print Results #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
	# print("\n")
	# print("Localization using ESP32 %s" %case[0:3])
	# print("Akurasi  : ", round(acc, 2))
	# print("Presisi  : ", round(prc, 2))
	# print("Resolusi : ", round(rsl, 2))

	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Graphing the Results #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
	#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
	if case[0] == "1":
		limits = [-0.5, 1.5]
		ticks  = np.arange(-0.5, 1.51, 0.5)
	elif case[0] == "2":
		limits = [-1, 3.0]
		ticks  = np.arange(-1, 3.1, 0.5)
	elif case[0] == "3":
		limits = [-1, 4]
		ticks  = np.arange(-1, 4.1, 0.5)
	else:
		limits = [-1, 5]
		ticks  = np.arange(-1, 5.1, 0.5)
	pos = 0.05

	plt.figure("%s"%case[0:3],figsize = [8,6])
	# plt.title("Min-Max Kasus %s\n Variasi Jarak tanpa Manusia" %case[0:3], fontsize = 14, fontweight = "bold")
	plt.title("Min-Max Kasus %s\n Variasi Jarak dengan Manusia" %case[0:3], fontsize = 14, fontweight = "bold")
	plt.scatter(x_pred, y_pred, label = "minmax", c = "blue")
	plt.plot(xreal, yreal, "D", label = "real", markersize = 9, markeredgewidth=1.5, markeredgecolor = "black", c = "red")
	plt.plot(np.mean(x_pred), np.mean(y_pred), "X", label = "$avg_{minmax}$", markersize = 12, markeredgewidth=1.5, markeredgecolor = "black", c = "lime")
	plt.plot(x1, y1, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
	plt.plot(x2, y2, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
	plt.plot(x3, y3, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
	plt.text(x1+pos, y1+pos, "AP1", fontsize = 15)
	plt.text(x2+pos, y2+pos, "AP2", fontsize = 15)
	plt.text(x3+pos, y3+pos, "AP3", fontsize = 15)
	plt.ylim(limits)
	plt.xlim(limits)
	plt.xticks(ticks, fontsize = 14)
	plt.yticks(ticks, fontsize = 14)
	plt.xlabel("\nx (m)", fontsize = 12)
	plt.ylabel("y (m)", fontsize = 12)
	plt.legend(loc = "lower right", fontsize = 14)
	plt.grid()
	# plt.savefig(fname="D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Grafik PNG/minmax/Variasi Jarak/%s"%case[0:3], dpi=100)
	plt.savefig(fname="D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Grafik PNG/minmax/Variasi Jarak dan Manusia/%s"%case[0:3], dpi=100)
	plt.show()
	plt.close()

#~~~~~~~~~~~~~~~~~~~~~ Run Program ~~~~~~~~~~~~~~~~~~~~#
# if __name__ == '__main__':
# 	main()

trilateration.py

"""
Author	: Muhammad Arifin
Date	: 7th May, 2020
Subject	: Implementation of trilateration calculation
"""

import os
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

"""
Function to calculate trilateration parameters
"""

def trilat_params(xi,yi,xj,yj,ri,rj):
	a = -2*xi + 2*xj
	b = -2*yi + 2*yj
	c = ri**2 - rj**2 - xi**2 + xj**2 - yi**2 + yj**2

	return a,b,c

"""
Implementation of trilateration calculation process
# Function of Trilateration Calculation Process
# Data to be processed is stored as csv file. 
# Data is processed entirely using pandas dataframe
"""

def trilateration_process(path, case, ple, rssi_d0):
	# Read csv file as panda data frame
	df = pd.read_csv(path+case)

	# Drop 'Time' column
	df = df.drop(columns=["Time"])

	# Drop NaN values
	df = df.dropna()

	# Take only column 0-3 which is AP1, AP2, and AP3
	# This step is important as sometimes the csv data
	# has other column aside Time, and RSSI values of 
	# AP1, AP2, and AP3

	df = df.iloc[:,0:3]

	# Rename columns to AP1, AP2, and AP3
	df.columns = ["AP1", "AP2", "AP3"]


	# Add path loss exponent and rssi at d0 parameters
	# Create a lambda function to convert RSSI data into distance
	dist = lambda k, n, rssi: 10**((k - rssi) / (10*n))

	# Add distance columns in df 
	for i in range(1,4):
		df["dist%d"%i] = dist(rssi_d0, ple, df["AP%d"%i])


	# Determining what is the distance case 
	# 1Dx means d = 1, 2Dx means d = 2, cont..

	if case[0] == "1":
		d = 1
	elif case[0] == "2":
		d = 2
	elif case[0] == "3":
		d = 3
	else:
		d = 4


	# Setting APs coordinates
	# AP1
	x1 = 0
	y1 = 0

	# AP2
	x2 = 0
	y2 = 1 * d

	# AP3
	x3 = 1 * d
	y3 = 1 * d

	ap_coordinates = [[x1,y1], [x2, y2], [x3, y3]]

	# Calculation target Coordinates 
	# check which STA position case is being analysed
	# Also check whether the entered case is in capital or not

	case = case.upper()
	if case[1:3] == "D1":
		xreal = (1/2) * d
		yreal = 1*d
	elif case[1:3] == "D2":
		xreal = (1/4) * d
		yreal = (3/4) * d
	elif case[1:3] == "D3":
		xreal = (1/2) * d
		yreal = (1/2) * d
	elif case[1:3] == "D4":
		xreal = (1/2) * d
		yreal = 0 * d

	# Trilateration parameters calculation
	# Calculate A, B, C, D, E, and F 
	A, B, C = trilat_params(x1, y1, x2, y2, df["dist1"], df["dist2"])
	D, E, F = trilat_params(x2, y2, x3, y3, df["dist2"], df["dist3"])

	# Calculate x and y using trilateration
	# x = CE - BF / AE - BD; y = CD - AF / BD - AE
	xtr = (C*E - B*F) / (A*E - B*D)
	ytr = (C*D - A*F) / (B*D - A*E)

	# Calculate the Squared Root Error and the Mean Squared Error 
	sqe = np.sqrt((xtr - xreal)**2 + (ytr - yreal)**2)
	mse = np.mean(sqe)

	# Calculate standard deviation
	stdev = np.mean([np.std(xtr), np.std(ytr)])

	# Calculate the resolution parameter's value

	# Add target coordinates,
	# predicted coordinates by trilateration
	# also the SQRT error in df

	df["xreal"]   = xreal  		
	df["yreal"]   = yreal

	df["xtrilat"] = xtr	
	df["ytrilat"] = ytr

	df["SQE"]	  = sqe
	df["MSE"] 	  = mse
	df["stdev"]   = stdev

	# Return the trilateration df and MSE
	return (df, ap_coordinates)

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Main Program #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
def main():

	""" Run the program """
	# Data Paths

	path = 'D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Pengukuran Variasi Jarak Ulangan/'
	# path = 'D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Pengukuran Variasi Jarak dan Manusia/'
	cases = os.listdir(path)
	# cases = ['3D1.csv']

	# RSSI @d0 and path loss exponent (gamma)
	gamma = 2.255
	K = -49

	# Run all cases in the directory
	print(path)
	for x in cases:
		df, ap_coords = trilateration_process(path, x, gamma, K)

		# Assign APs coordinates
		x1 = ap_coords[0][0]
		y1 = ap_coords[0][1]

		x2 = ap_coords[1][0]
		y2 = ap_coords[1][1]

		x3 = ap_coords[2][0]
		y3 = ap_coords[2][1]

		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Accuracy, Precision, Resolution #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

		# Resolution
		xco = np.array(df["xtrilat"])
		yco = np.array(df["ytrilat"])
		xco.sort()
		yco.sort()
		rslx = np.mean(xco[90:100]) - np.mean(xco[0:10])
		rsly = np.mean(yco[90:100]) - np.mean(yco[0:10])
		rsl = np.mean([rslx, rsly])

		# Accuracy 
		acc = np.mean(df["MSE"])

		# Precision
		prc = np.mean(df["stdev"])
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Print Results #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

		# print("\n")
		# print("Localization using ESP32 %s \n" %x)
		# print("Akurasi  : ", round(acc, 2))
		# print("Presisi  : ", round(prc, 2))
		# print("Resolusi : ", round(rsl, 2))

		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# Graphing the Results #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 
		#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#


		# Graph limits and ticks based on case
		if x[0] == "1":
			limits = [-0.5, 1.5]
			ticks  = np.arange(-0.5, 1.51, 0.5)
		elif x[0] == "2":
			limits = [-1, 3.0]
			ticks  = np.arange(-1, 3.1, 0.5)
		elif x[0] == "3":
			limits = [-1, 4]
			ticks  = np.arange(-1, 4.1, 0.5)
		else:
			limits = [-1, 5]
			ticks  = np.arange(-1, 5.1, 0.5)
		
		pos = 0.05

		plt.figure("%s" %x[0:3], figsize = [8,6])
		plt.title("Trilaterasi Kasus %s\n Variasi Jarak tanpa Manusia" %x[0:3], fontsize = 14, fontweight = "bold")
		# plt.title("Trilaterasi Kasus %s\n Variasi Jarak dengan Manusia" %x[0:3], fontsize = 14, fontweight = "bold")
		plt.scatter(df["xtrilat"], df["ytrilat"], label = "tri", c = "blue")
		plt.plot(df["xreal"], df["yreal"], "D", label = "real", markersize = 9, markeredgewidth=1.5, markeredgecolor = "black", c = "red")
		plt.plot(np.mean(df["xtrilat"]), np.mean(df["ytrilat"]), "X", label = "$avg_{tri}$", markersize = 12, markeredgewidth=1.5, markeredgecolor = "black", c = "lime")
		plt.plot(x1, y1, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
		plt.plot(x2, y2, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
		plt.plot(x3, y3, "^", markersize = 12, c = "darkorange", markeredgewidth=1.0, markeredgecolor = "black")
		plt.text(x1+pos, y1+pos, "AP1", fontsize = 15)
		plt.text(x2+pos, y2+pos, "AP2", fontsize = 15)
		plt.text(x3+pos, y3+pos, "AP3", fontsize = 15)
		plt.ylim(limits)
		plt.xlim(limits)
		plt.xticks(ticks, fontsize = 14)
		plt.yticks(ticks, fontsize = 14)
		plt.xlabel("\nx (m)", fontsize = 12)
		plt.ylabel("y (m)", fontsize = 12)
		plt.legend(loc = "lower right", fontsize = 14)
		plt.grid()
		plt.savefig(fname="D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Grafik PNG/trilaterasi/Variasi Jarak/%s"%x[0:3], dpi=100)
		# plt.savefig(fname="D:/Skripsweetku/FILE INTI SKRIPSI/Raw Data/Grafik PNG/trilaterasi/Variasi Jarak dan Manusia/%s"%x[0:3], dpi=100)
		plt.show()
		plt.close(fig=None)

if __name__ == '__main__':
	main()