-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathestimateReceiverPosition.m
106 lines (87 loc) · 4.94 KB
/
estimateReceiverPosition.m
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
function [x_eval, y_eval, z_eval] = estimateReceiverPosition(LED1, LED2, LED3, LED4, PDect_pos, p, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of, R_pd, q_0, k_B, T_k, eta, I_2, I_3, Gamma, g_m, I_bg, G_0, B, K_0)
% calcoli presi da [SAA+2022], %[c2022]
% parameters: LED1, LED2, LED3, LED4, PDect_pos, p, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of, R_pd, q_0, k_B, T_k, eta, I_2, I_3, Gamma, g_m, I_bg, G_0, B
debug_enabled = 0;
kalman_filter_enabled = 0;
m = -(log(2)/log(cosd(Psi)));
G = (a^2)/((sind(Phi_FoV)^2));
%% Received power from each entity
% kalman filter initialization
if kalman_filter_enabled
x_LED1 = 0;
x_LED2 = 0;
x_LED3 = 0;
x_LED4 = 0;
P_LED1 = 1e-9;
P_LED2 = 1e-9;
P_LED3 = 1e-9;
P_LED4 = 1e-9;
end
received_current_LED1_array = nan(1, K_0);
received_current_LED2_array = nan(1, K_0);
received_current_LED3_array = nan(1, K_0);
received_current_LED4_array = nan(1, K_0);
%par
for sampling_idx = 1:K_0
% LED1 contribution
received_current_LED1_array(sampling_idx) = R_pd * p * singleEntityContribution(LED1, PDect_pos, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of);
[n_shoot_LED1, n_thermal_LED1, ~, ~] = noiseEstimation(received_current_LED1_array(sampling_idx), q_0, R_pd, k_B, T_k, eta, I_2, I_3, Gamma, A_pd, g_m, I_bg, G_0, B);
received_current_LED1_array(sampling_idx) = received_current_LED1_array(sampling_idx) + n_shoot_LED1 + n_thermal_LED1;
% LED2 contribution
received_current_LED2_array(sampling_idx) = R_pd * p * singleEntityContribution(LED2, PDect_pos, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of);
[n_shoot_LED2, n_thermal_LED2, ~, ~] = noiseEstimation(received_current_LED2_array(sampling_idx), q_0, R_pd, k_B, T_k, eta, I_2, I_3, Gamma, A_pd, g_m, I_bg, G_0, B);
received_current_LED2_array(sampling_idx) = received_current_LED2_array(sampling_idx) + n_shoot_LED2 + n_thermal_LED2;
% LED3 contribution
received_current_LED3_array(sampling_idx) = R_pd * p * singleEntityContribution(LED3, PDect_pos, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of);
[n_shoot_LED3, n_thermal_LED3, ~, ~] = noiseEstimation(received_current_LED3_array(sampling_idx), q_0, R_pd, k_B, T_k, eta, I_2, I_3, Gamma, A_pd, g_m, I_bg, G_0, B);
received_current_LED3_array(sampling_idx) = received_current_LED3_array(sampling_idx) + n_shoot_LED3 + n_thermal_LED3;
% LED4 contribution
received_current_LED4_array(sampling_idx) = R_pd * p * singleEntityContribution(LED4, PDect_pos, alpha, beta, Phi_FoV, a, Psi, A_pd, T_of);
[n_shoot_LED4, n_thermal_LED4, ~, ~] = noiseEstimation(received_current_LED4_array(sampling_idx), q_0, R_pd, k_B, T_k, eta, I_2, I_3, Gamma, A_pd, g_m, I_bg, G_0, B);
received_current_LED4_array(sampling_idx) = received_current_LED4_array(sampling_idx) + n_shoot_LED4 + n_thermal_LED4;
end
received_current_LED1 = sum(received_current_LED1_array)/K_0;
received_current_LED2 = sum(received_current_LED2_array)/K_0;
received_current_LED3 = sum(received_current_LED3_array)/K_0;
received_current_LED4 = sum(received_current_LED4_array)/K_0;
%% testONLY => forzo la correttezza delle distanze
dLED1_real = calculateDistance(LED1, PDect_pos);
dLED2_real = calculateDistance(LED2, PDect_pos);
dLED3_real = calculateDistance(LED3, PDect_pos);
dLED4_real = calculateDistance(LED4, PDect_pos);
% d1 = dLED1_real;
% d2 = dLED2_real;
% d3 = dLED3_real;
% d4 = dLED4_real;
%% LSE method
d1 = lseDistanceEstimator(LED1, PDect_pos, R_pd, A_pd, p, m, T_of, G, received_current_LED1_array);
d2 = lseDistanceEstimator(LED2, PDect_pos, R_pd, A_pd, p, m, T_of, G, received_current_LED2_array);
d3 = lseDistanceEstimator(LED3, PDect_pos, R_pd, A_pd, p, m, T_of, G, received_current_LED3_array);
d4 = lseDistanceEstimator(LED4, PDect_pos, R_pd, A_pd, p, m, T_of, G, received_current_LED4_array);
rec_power_vect = [
received_current_LED1
received_current_LED2
received_current_LED3
received_current_LED4
];
distance_vect = [d1 d2 d3 d4];
entity_vect(1).position = LED1;
entity_vect(2).position = LED2;
entity_vect(3).position = LED3;
entity_vect(4).position = LED4;
[~, orderedIdx] = sort(rec_power_vect, 'descend');
distance_vect = distance_vect(orderedIdx);
entity_vect = entity_vect(orderedIdx);
distance_vect = distance_vect(~isnan(distance_vect) & ~isinf(distance_vect));
entity_vect = entity_vect(~isnan(distance_vect) & ~isinf(distance_vect));
pos_estimated = leastSquareMethod(entity_vect, distance_vect, PDect_pos(3));
if isnan(pos_estimated)
x_eval = NaN;
y_eval = NaN;
z_eval = NaN;
else
x_eval = pos_estimated(1);
y_eval = pos_estimated(2);
z_eval = pos_estimated(3);
end
end