diff --git a/lab_3/.DS_Store b/lab_3/.DS_Store index 15d76a4..ca1933c 100644 Binary files a/lab_3/.DS_Store and b/lab_3/.DS_Store differ diff --git a/lab_3/p3_localisation_solutions/fingerprinting.py b/lab_3/p3_localisation_solutions/fingerprinting_horus.py similarity index 100% rename from lab_3/p3_localisation_solutions/fingerprinting.py rename to lab_3/p3_localisation_solutions/fingerprinting_horus.py diff --git a/lab_3/p3_localisation_solutions/fingerprinting_radar.py b/lab_3/p3_localisation_solutions/fingerprinting_radar.py new file mode 100644 index 0000000..e048656 --- /dev/null +++ b/lab_3/p3_localisation_solutions/fingerprinting_radar.py @@ -0,0 +1,399 @@ +import numpy as np +import os +import glob +import sys +import matplotlib.pyplot as plt +from statsmodels.distributions.empirical_distribution import ECDF +from scipy.stats import norm + + +# +# Implements the Radar WiFi Localization system +# ( Mobile and Sensor Networks course - University of Oxford ) +# April 2016 +# + +def init(): + print("") + print ('*' * 60) + print (' The Radar Localization System') + print (" Mobile and Sensor Networks course - University of Oxford") + print ('*' * 60) + print ("") + print ("") + return + + +# Function to load the wifi RSS data +def load_wifi_data(data_folder, n_samples, n_ap): + """ + :param data_folder: Location of WiFi RSS data + :param n_samples: Number of RSS samples collected for each location + :param n_ap: Number of access points + :return: train and test wifi databases + """ + + sys.stdout.write("-> Loading files...") + + train_path = data_folder + "/" + "wifiData/" + test_path = data_folder + "/" + "testWifiData/" + + # We are going to examine two cases with 3 and 5 Access Points (APs) + if n_ap == 3: + ap_index = [1, 2, 3] + else: + ap_index = [1, 2, 3, 4, 5] + + # We read the wifi RSS collected in the training phase to built the Wifi database + os.chdir(train_path) + train_files = glob.glob("*.txt") + n_locations = len(train_files) + wifi_database = np.zeros((n_locations, (n_ap * n_samples) + 3)) + ap_name = "AP_" + + file_count = -1 + for f in train_files: + file_count += 1 + + if n_ap == 3: + ap_counter = [0, 0, 0] + else: + ap_counter = [0, 0, 0, 0, 0] + + fid = open(f, 'r') + wifi_database[file_count, 0] = file_count + first_line = 0 + for line in fid: + data = line.split() + if first_line == 0: + first_line += 1 + wifi_database[file_count, 1] = int(data[0]) + wifi_database[file_count, 2] = int(data[1]) + else: + for idx in range(1, n_ap + 1): + if data[1] == (ap_name + str(idx)): + ap_counter[idx - 1] += 1 + db_index = int((n_samples * (ap_index[idx - 1] - 1)) + 2 + ap_counter[idx - 1]) + wifi_database[file_count, db_index] = float(data[2]) + os.chdir("../..") + + # Now we load the test points. Wifi data collected at unknown locations. + os.chdir(test_path) + test_files = glob.glob("*.txt") + test_files.sort() + n_locations = len(test_files) + test_db = np.zeros((n_locations, (n_ap * (n_samples - 10)) + 3)) + + file_count = -1 + for f in test_files: + file_count += 1 + + if n_ap == 3: + ap_counter = [0, 0, 0] + else: + ap_counter = [0, 0, 0, 0, 0] + + fid = open(f, 'r') + test_db[file_count, 0] = file_count + first_line = 0 + for line in fid: + data = line.split() + if first_line == 0: + first_line += 1 + test_db[file_count, 1] = int(data[0]) + test_db[file_count, 2] = int(data[1]) + else: + for idx in range(1, n_ap + 1): + if data[1] == (ap_name + str(idx)): + ap_counter[idx - 1] += 1 + db_index = int(((n_samples - 10) * (ap_index[idx - 1] - 1)) + 2 + ap_counter[idx - 1]) + test_db[file_count, db_index] = float(data[2]) + + sys.stdout.write("done\n") + return wifi_database, test_db + + +# Visualize fingerprint locations and locations of APs +def show_fingerprints(train_db, n_ap): + """ + :param train_db: RSS points collected at known locations + :param n_ap: Number of APs + :return: null + """ + sys.stdout.write("-> Displaying fingerprints...") + + if n_ap == 3: + ap_loc = np.array([[6, 6], [8, 16], [18, 14]]) + else: + ap_loc = np.array([[6, 6], [8, 16], [18, 14], [16, 4], [12, 10]]) + + plt.figure() + plt.plot(train_db[:, 1], train_db[:, 2], 'co', markersize=17) + plt.plot(ap_loc[:, 0], ap_loc[:, 1], 'b*', markersize=10) + major_ticks = np.arange(-2, 22, 2) + axes = plt.gca() + axes.set_xlim([-2, 22]) + axes.set_ylim([-2, 22]) + axes.set_xticks(major_ticks) + axes.set_yticks(major_ticks) + plt.grid(True) + plt.xlabel("X -> m") + plt.ylabel("Y -> m") + plt.title("Cyan points: fingerprint locations, Blue points: Access Points") + plt.show(block=False) + sys.stdout.write("done\n") + + return + + +# Fit a normal distribution to the RSS data +def fit_data(train_db, n_samples, n_ap): + """ + :param train_db: RSS points collected at known locations + :param n_samples: Number of RSS samples per location + :param n_ap: Number of access points + :return: Wifi fingerprint database; We approximate the RSS at each location with a Gaussian + """ + + sys.stdout.write("-> Modeling RSS with Gaussian dist...") + + n_loc = len(train_db) + + # Initialize Wifi database + wifi_db = np.zeros((n_loc, 3 + (n_ap * 2))) + + for i in range(0, n_loc): + wifi_db[i, 0] = i + wifi_db[i, 1] = train_db[i, 1] + wifi_db[i, 2] = train_db[i, 2] + for j in range(0, n_ap): + ind_start = int((n_samples * j) + 3) + ind_end = int((j + 1) * n_samples + 3) + dat = train_db[i, ind_start:ind_end] + [mu, sigma] = norm.fit(dat) + wifi_db[i, (j * 2) + 3] = mu + wifi_db[i, (j * 2) + 4] = sigma + + sys.stdout.write("done\n") + return wifi_db + + +# Plot Wifi RSS histogram per location - Fit a Gaussian Dist to the data +def plot_histogram(train_db, location, n_ap, n_samples): + """ + :param train_db: RSS points collected at known locations + :param location: Location to plot the RSS histogram + :param n_ap: Number of access points + :param n_samples: Number of RSS samples per location + :return: null + """ + + sys.stdout.write("-> Visualizing histograms...") + + if (location < 0) or (location >= len(train_db)): + location = 0 + + # Extract the RSS data from :param location + for i in range(0, n_ap): + ind_start = int((n_samples * i) + 3) + ind_end = int((i + 1) * n_samples + 3) + dat = train_db[location, ind_start:ind_end] + + # Plot the RSS histogram + plt.figure() + n_bins = round(np.sqrt(len(dat))) + plt.hist(dat, bins=n_bins, normed=True, alpha=0.6, color='g') + + # Approximate the histogram with a Gaussian distribution + [mu, sigma] = norm.fit(dat) + x = np.linspace(mu - 4 * sigma, mu + 4 * sigma, 500) + y = norm.pdf(x, mu, sigma) + plt.plot(x, y, 'r-', linewidth=2) + plt.xlabel('Wifi RSS -> dBm') + plt.ylabel('Frequency') + plt.title('RSS histogram of AP=%d , Norm(%.2f,%.2f)' % (i + 1, mu, sigma)) + plt.grid(True) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# The Radar localization system +def predict(wifi_db, test_db, n_ap, n_samples): + """ + :param wifi_db: The Wifi database + :param test_db: Wifi RSS points an the unknown locations + :param n_ap: Number of APs + :param n_samples: Samples per location + :return: The estimated locations + """ + + sys.stdout.write("-> Running Radar...") + + predicted_loc = np.zeros((len(test_db), 2)) + actual_loc = test_db[:, 1:3] + + for i in range(0, len(test_db)): + + # Keep track of the likelihood being in each location + loglik = np.zeros((1, len(wifi_db))) + + + # Go through each location in the wifi_db + for j in range(0, len(wifi_db)): + + # Take account all access points + for k in range(0, n_ap): + # Get the RSS data at the unknown location + ind_start = int(((n_samples - 10) * k) + 3) + ind_end = int((k + 1) * (n_samples - 10) + 3) + rssdat = test_db[i, ind_start:ind_end] + + # Calculate the mean (test point) + test_point = np.mean(rssdat) + + # Calculate the mean of wifi_db location (training point) + mu = wifi_db[j, (k * 2) + 3] + + # Calculate the Euclidian distance between test points and training points + loglik[0, j] += (test_point-mu)*(test_point-mu) + + # Find the top 3 nearest locations + min_k=[] + for k in range(3): + min_lik=float('Inf') + tmp=0 + for j in range(0,len(wifi_db)): + if (loglik[0,j] Visualizing estimated trajectory...") + + if n_ap == 3: + ap_loc = np.array([[6, 6], [8, 16], [18, 14]]) + else: + ap_loc = np.array([[6, 6], [8, 16], [18, 14], [16, 4], [12, 10]]) + + plt.figure() + plt.plot(ap_loc[:, 0], ap_loc[:, 1], 'b*', markersize=15) + plt.plot(actual_loc[:, 0], actual_loc[:, 1], 'go-', linewidth=2) + plt.plot(predicted_loc[:, 0], predicted_loc[:, 1], 'ro-', linewidth=2) + major_ticks = np.arange(-2, 22, 2) + + axes = plt.gca() + axes.set_xlim([-2, 22]) + axes.set_ylim([-2, 22]) + axes.set_xticks(major_ticks) + axes.set_yticks(major_ticks) + plt.xlabel('X -> m') + plt.ylabel('Y -> m') + plt.title('Green line = actual path , Red line = estimated path, Blue points = AP loc') + plt.grid(True) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# Calculate and plot the localization error +def plot_error(actual_loc, predicted_loc): + """ + :param actual_loc: Actual locations + :param predicted_loc: Estimated Locations + :return: null + """ + + sys.stdout.write("-> Calculating localization error...") + + err = np.zeros((1, len(actual_loc))) + for i in range(0, len(actual_loc)): + err[0, i] = np.sqrt((actual_loc[i, 0] - predicted_loc[i, 0]) ** 2 + + (actual_loc[i, 1] - predicted_loc[i, 1]) ** 2) + + mean_err = np.mean(err) + + ecdf = ECDF(err[0, :]) + plt.figure() + plt.plot(ecdf.x, ecdf.y) + plt.grid(True) + plt.xlabel('error -> m') + plt.ylabel('Probability') + plt.title('Error CDF , Mean error = %.3f' % mean_err) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# -------------------------------------------------------------- +# MAIN CODE +# -------------------------------------------------------------- + +# ----- Constants ----- +# Number of measurements per location +nSamples = 60 + +# ---- Configure Parameters ----- +# Select Dataset (set1 or set2) +data_set = "set1" + +# Visualize the RSS in location = loc +loc = 2 + +# ----- Run System ------ + +# set1 contains 3 access points and set2 5 access points +if data_set == "set1": + nAP = 3 +else: + nAP = 5 + +init() + +# Load the RSS measurements from file +(trainDB, testDB) = load_wifi_data(data_set, nSamples, nAP) + +# Show fingerprint locations +show_fingerprints(trainDB, nAP) + +# Plot the RSS histogram and approximate it with a Gaussian +plot_histogram(trainDB, loc, nAP, nSamples) + +# Fit Gaussian distribution to the RSS measurements +wifiDB = fit_data(trainDB, nSamples, nAP) + +# Run the localization algorithm +# Estimate the unknown location given RSS measurements at that location +(actualLoc, predictedLoc) = predict(wifiDB, testDB, nAP, nSamples) + +# Visualize the actual and estimated locations +plot_path(actualLoc, predictedLoc, nAP) + +# Plot the mean localization error and error CDF +plot_error(actualLoc, predictedLoc) + +ans = raw_input("Press any key to exit") diff --git a/lab_3/results/simulation/fingerprinting_radar.py b/lab_3/results/simulation/fingerprinting_radar.py new file mode 100644 index 0000000..e76f688 --- /dev/null +++ b/lab_3/results/simulation/fingerprinting_radar.py @@ -0,0 +1,399 @@ +import numpy as np +import os +import glob +import sys +import matplotlib.pyplot as plt +from statsmodels.distributions.empirical_distribution import ECDF +from scipy.stats import norm + + +# +# Implements the Radar WiFi Localization system +# ( Mobile and Sensor Networks course - University of Oxford ) +# April 2016 +# + +def init(): + print("") + print ('*' * 60) + print (' The Radar Localization System') + print (" Mobile and Sensor Networks course - University of Oxford") + print ('*' * 60) + print ("") + print ("") + return + + +# Function to load the wifi RSS data +def load_wifi_data(data_folder, n_samples, n_ap): + """ + :param data_folder: Location of WiFi RSS data + :param n_samples: Number of RSS samples collected for each location + :param n_ap: Number of access points + :return: train and test wifi databases + """ + + sys.stdout.write("-> Loading files...") + + train_path = data_folder + "/" + "wifiData/" + test_path = data_folder + "/" + "testWifiData/" + + # We are going to examine two cases with 3 and 5 Access Points (APs) + if n_ap == 3: + ap_index = [1, 2, 3] + else: + ap_index = [1, 2, 3, 4, 5] + + # We read the wifi RSS collected in the training phase to built the Wifi database + os.chdir(train_path) + train_files = glob.glob("*.txt") + n_locations = len(train_files) + wifi_database = np.zeros((n_locations, (n_ap * n_samples) + 3)) + ap_name = "AP_" + + file_count = -1 + for f in train_files: + file_count += 1 + + if n_ap == 3: + ap_counter = [0, 0, 0] + else: + ap_counter = [0, 0, 0, 0, 0] + + fid = open(f, 'r') + wifi_database[file_count, 0] = file_count + first_line = 0 + for line in fid: + data = line.split() + if first_line == 0: + first_line += 1 + wifi_database[file_count, 1] = int(data[0]) + wifi_database[file_count, 2] = int(data[1]) + else: + for idx in range(1, n_ap + 1): + if data[1] == (ap_name + str(idx)): + ap_counter[idx - 1] += 1 + db_index = int((n_samples * (ap_index[idx - 1] - 1)) + 2 + ap_counter[idx - 1]) + wifi_database[file_count, db_index] = float(data[2]) + os.chdir("../..") + + # Now we load the test points. Wifi data collected at unknown locations. + os.chdir(test_path) + test_files = glob.glob("*.txt") + test_files.sort() + n_locations = len(test_files) + test_db = np.zeros((n_locations, (n_ap * (n_samples - 10)) + 3)) + + file_count = -1 + for f in test_files: + file_count += 1 + + if n_ap == 3: + ap_counter = [0, 0, 0] + else: + ap_counter = [0, 0, 0, 0, 0] + + fid = open(f, 'r') + test_db[file_count, 0] = file_count + first_line = 0 + for line in fid: + data = line.split() + if first_line == 0: + first_line += 1 + test_db[file_count, 1] = int(data[0]) + test_db[file_count, 2] = int(data[1]) + else: + for idx in range(1, n_ap + 1): + if data[1] == (ap_name + str(idx)): + ap_counter[idx - 1] += 1 + db_index = int(((n_samples - 10) * (ap_index[idx - 1] - 1)) + 2 + ap_counter[idx - 1]) + test_db[file_count, db_index] = float(data[2]) + + sys.stdout.write("done\n") + return wifi_database, test_db + + +# Visualize fingerprint locations and locations of APs +def show_fingerprints(train_db, n_ap): + """ + :param train_db: RSS points collected at known locations + :param n_ap: Number of APs + :return: null + """ + sys.stdout.write("-> Displaying fingerprints...") + + if n_ap == 3: + ap_loc = np.array([[6, 6], [8, 16], [18, 14]]) + else: + ap_loc = np.array([[6, 6], [8, 16], [18, 14], [16, 4], [12, 10]]) + + plt.figure() + plt.plot(train_db[:, 1], train_db[:, 2], 'co', markersize=17) + plt.plot(ap_loc[:, 0], ap_loc[:, 1], 'b*', markersize=10) + major_ticks = np.arange(-2, 22, 2) + axes = plt.gca() + axes.set_xlim([-2, 22]) + axes.set_ylim([-2, 22]) + axes.set_xticks(major_ticks) + axes.set_yticks(major_ticks) + plt.grid(True) + plt.xlabel("X -> m") + plt.ylabel("Y -> m") + plt.title("Cyan points: fingerprint locations, Blue points: Access Points") + plt.show(block=False) + sys.stdout.write("done\n") + + return + + +# Fit a normal distribution to the RSS data +def fit_data(train_db, n_samples, n_ap): + """ + :param train_db: RSS points collected at known locations + :param n_samples: Number of RSS samples per location + :param n_ap: Number of access points + :return: Wifi fingerprint database; We approximate the RSS at each location with a Gaussian + """ + + sys.stdout.write("-> Modeling RSS with Gaussian dist...") + + n_loc = len(train_db) + + # Initialize Wifi database + wifi_db = np.zeros((n_loc, 3 + (n_ap * 2))) + + for i in range(0, n_loc): + wifi_db[i, 0] = i + wifi_db[i, 1] = train_db[i, 1] + wifi_db[i, 2] = train_db[i, 2] + for j in range(0, n_ap): + ind_start = int((n_samples * j) + 3) + ind_end = int((j + 1) * n_samples + 3) + dat = train_db[i, ind_start:ind_end] + [mu, sigma] = norm.fit(dat) + wifi_db[i, (j * 2) + 3] = mu + wifi_db[i, (j * 2) + 4] = sigma + + sys.stdout.write("done\n") + return wifi_db + + +# Plot Wifi RSS histogram per location - Fit a Gaussian Dist to the data +def plot_histogram(train_db, location, n_ap, n_samples): + """ + :param train_db: RSS points collected at known locations + :param location: Location to plot the RSS histogram + :param n_ap: Number of access points + :param n_samples: Number of RSS samples per location + :return: null + """ + + sys.stdout.write("-> Visualizing histograms...") + + if (location < 0) or (location >= len(train_db)): + location = 0 + + # Extract the RSS data from :param location + for i in range(0, n_ap): + ind_start = int((n_samples * i) + 3) + ind_end = int((i + 1) * n_samples + 3) + dat = train_db[location, ind_start:ind_end] + + # Plot the RSS histogram + plt.figure() + n_bins = round(np.sqrt(len(dat))) + plt.hist(dat, bins=n_bins, normed=True, alpha=0.6, color='g') + + # Approximate the histogram with a Gaussian distribution + [mu, sigma] = norm.fit(dat) + x = np.linspace(mu - 4 * sigma, mu + 4 * sigma, 500) + y = norm.pdf(x, mu, sigma) + plt.plot(x, y, 'r-', linewidth=2) + plt.xlabel('Wifi RSS -> dBm') + plt.ylabel('Frequency') + plt.title('RSS histogram of AP=%d , Norm(%.2f,%.2f)' % (i + 1, mu, sigma)) + plt.grid(True) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# The Radar localization system +def predict(wifi_db, test_db, n_ap, n_samples): + """ + :param wifi_db: The Wifi database + :param test_db: Wifi RSS points an the unknown locations + :param n_ap: Number of APs + :param n_samples: Samples per location + :return: The estimated locations + """ + + sys.stdout.write("-> Running Radar...") + + predicted_loc = np.zeros((len(test_db), 2)) + actual_loc = test_db[:, 1:3] + + for i in range(0, len(test_db)): + + # Keep track of the likelihood being in each location + loglik = np.zeros((1, len(wifi_db))) + + + # Go through each location in the wifi_db + for j in range(0, len(wifi_db)): + + # Take account all access points + for k in range(0, n_ap): + # Get the RSS data at the unknown location + ind_start = int(((n_samples - 10) * k) + 3) + ind_end = int((k + 1) * (n_samples - 10) + 3) + rssdat = test_db[i, ind_start:ind_end] + + # Calculate the mean (test point) + test_point = np.mean(rssdat) + + # Calculate the mean of wifi_db location (training point) + mu = wifi_db[j, (k * 2) + 3] + + # Calculate the Euclidian distance between test points and training points + loglik[0, j] += (test_point-mu)*(test_point-mu) + + # Find the top 3 nearest locations + min_k=[] + for k in range(3): + min_lik=1000000 + tmp=0 + for j in range(0,len(wifi_db)): + if (loglik[0,j] Visualizing estimated trajectory...") + + if n_ap == 3: + ap_loc = np.array([[6, 6], [8, 16], [18, 14]]) + else: + ap_loc = np.array([[6, 6], [8, 16], [18, 14], [16, 4], [12, 10]]) + + plt.figure() + plt.plot(ap_loc[:, 0], ap_loc[:, 1], 'b*', markersize=15) + plt.plot(actual_loc[:, 0], actual_loc[:, 1], 'go-', linewidth=2) + plt.plot(predicted_loc[:, 0], predicted_loc[:, 1], 'ro-', linewidth=2) + major_ticks = np.arange(-2, 22, 2) + + axes = plt.gca() + axes.set_xlim([-2, 22]) + axes.set_ylim([-2, 22]) + axes.set_xticks(major_ticks) + axes.set_yticks(major_ticks) + plt.xlabel('X -> m') + plt.ylabel('Y -> m') + plt.title('Green line = actual path , Red line = estimated path, Blue points = AP loc') + plt.grid(True) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# Calculate and plot the localization error +def plot_error(actual_loc, predicted_loc): + """ + :param actual_loc: Actual locations + :param predicted_loc: Estimated Locations + :return: null + """ + + sys.stdout.write("-> Calculating localization error...") + + err = np.zeros((1, len(actual_loc))) + for i in range(0, len(actual_loc)): + err[0, i] = np.sqrt((actual_loc[i, 0] - predicted_loc[i, 0]) ** 2 + + (actual_loc[i, 1] - predicted_loc[i, 1]) ** 2) + + mean_err = np.mean(err) + + ecdf = ECDF(err[0, :]) + plt.figure() + plt.plot(ecdf.x, ecdf.y) + plt.grid(True) + plt.xlabel('error -> m') + plt.ylabel('Probability') + plt.title('Error CDF , Mean error = %.3f' % mean_err) + plt.show(block=False) + + sys.stdout.write("done\n") + return + + +# -------------------------------------------------------------- +# MAIN CODE +# -------------------------------------------------------------- + +# ----- Constants ----- +# Number of measurements per location +nSamples = 60 + +# ---- Configure Parameters ----- +# Select Dataset (set1 or set2) +data_set = "set1" + +# Visualize the RSS in location = loc +loc = 2 + +# ----- Run System ------ + +# set1 contains 3 access points and set2 5 access points +if data_set == "set1": + nAP = 3 +else: + nAP = 5 + +init() + +# Load the RSS measurements from file +(trainDB, testDB) = load_wifi_data(data_set, nSamples, nAP) + +# Show fingerprint locations +show_fingerprints(trainDB, nAP) + +# Plot the RSS histogram and approximate it with a Gaussian +plot_histogram(trainDB, loc, nAP, nSamples) + +# Fit Gaussian distribution to the RSS measurements +wifiDB = fit_data(trainDB, nSamples, nAP) + +# Run the localization algorithm +# Estimate the unknown location given RSS measurements at that location +(actualLoc, predictedLoc) = predict(wifiDB, testDB, nAP, nSamples) + +# Visualize the actual and estimated locations +plot_path(actualLoc, predictedLoc, nAP) + +# Plot the mean localization error and error CDF +plot_error(actualLoc, predictedLoc) + +ans = raw_input("Press any key to exit")