-
Notifications
You must be signed in to change notification settings - Fork 3
/
tdoa_montecarlo.py
119 lines (84 loc) · 3.18 KB
/
tdoa_montecarlo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
'''
MONTE-CARLO ANALYSIS OF TDOA LOCALISATION
COMPARING MLE, BLUE WITH CRLB
AUTHOR: ABIJITH J KAMATH
'''
# %% IMPORT PACKAGES
import os
import numpy as np
from tqdm import tqdm
from scipy.io import loadmat
from matplotlib import pyplot as plt
from matplotlib import style
from matplotlib import rcParams
import utils
# %% PLOT SETTINGS
plt.style.use(['science','ieee'])
plt.rcParams.update({
"font.family": "serif",
"font.serif": ["cm"],
"mathtext.fontset": "cm",
"font.size": 11})
# %% LOAD DATA
data = loadmat('./data/TDOA_data.mat')
anchor_location = data['anchor_location'].astype(np.float)
target_location = data['target_location'].astype(np.float)
true_distances = data['true_distances']
noisy_distances = data['noisy_distances']
var_vec = data['sigma2']
# %%
_, num_iter, var_len = noisy_distances.shape
init_pos = np.array([0.0,0.0])[:,np.newaxis]
min_x_var = np.zeros(var_len)
min_y_var = np.zeros(var_len)
mle_x_mse = np.zeros(var_len)
mle_y_mse = np.zeros(var_len)
blue_x_mse = np.zeros(var_len)
blue_y_mse = np.zeros(var_len)
for noise_iter in tqdm(range(var_len)):
noise = var_vec[noise_iter]
# Compute CRLB
fim = utils.crlb_tdoa(target_location, anchor_location, noise)
cov_est = np.linalg.inv(fim)
min_x_var[noise_iter] = cov_est[0,0]
min_y_var[noise_iter] = cov_est[1,1]
# Compute error in MLE and BLUE estimates
for avg_iter in range(num_iter):
mle_est = utils.mle_tdoa(noisy_distances[:,avg_iter,noise_iter],
anchor_location, init_pos, var=noise, step_size=noise/2.)
blue_est = utils.blue_tdoa(noisy_distances[:,avg_iter,noise_iter],
anchor_location, var=noise)
mle_x_mse[noise_iter] += (mle_est[0]-target_location[0])**2
mle_y_mse[noise_iter] += (mle_est[1]-target_location[1])**2
blue_x_mse[noise_iter] += (blue_est[0]-target_location[0])**2
blue_y_mse[noise_iter] += (blue_est[1]-target_location[1])**2
mle_x_mse[noise_iter] /= num_iter
mle_y_mse[noise_iter] /= num_iter
blue_x_mse[noise_iter] /= num_iter
blue_y_mse[noise_iter] /= num_iter
# %% PLOT MSE
os.makedirs('./results', exist_ok=True)
path = './results/'
# Plotting for x-coordinate
plt.figure(figsize=(12,6))
ax = plt.gca()
utils.plot_curve(var_vec, mle_x_mse, ax=ax, plot_colour='red',
legend_label=r'MLE', line_width=4)
utils.plot_curve(var_vec, blue_x_mse, ax=ax, plot_colour='blue',
legend_label=r'BLUE', line_width=4)
utils.plot_curve(var_vec, min_x_var, ax=ax, plot_colour='green',
legend_label=r'CRLB', line_width=4, xaxis_label=r'$\sigma^2$',
title_text=r'MSE in $x$-Coordinate', xlimits=[0,10], ylimits=[0,10],
show=True, save=path+'crlb_x')
# Plotting for y-coordinate
plt.figure(figsize=(12,6))
ax = plt.gca()
utils.plot_curve(var_vec, mle_y_mse, ax=ax, plot_colour='red',
legend_label=r'MLE', line_width=4)
utils.plot_curve(var_vec, blue_y_mse, ax=ax, plot_colour='blue',
legend_label=r'BLUE', line_width=4)
utils.plot_curve(var_vec, min_y_var, ax=ax, plot_colour='green',
legend_label=r'CRLB', line_width=4, xaxis_label=r'$\sigma^2$',
title_text=r'MSE in $y$-Coordinate', xlimits=[0,10], ylimits=[0,10],
show=True, save=path+'crlb_y')