RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
multipath_algo_own_norm_distr_test.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
3  * 2020 Freie Universität Berlin
4  *
5  * This file is subject to the terms and conditions of the GNU Lesser General
6  * Public License v2.1. See the file LICENSE in the top level directory for more
7  * details.
8  */
9 
24 #include <float.h>
25 #include <stdio.h>
26 #include <unistd.h>
27 #include <inttypes.h>
28 
32 #include "dist_based_jacobian.h"
33 #include "dist_based_fi.h"
34 #include "shell_sort.h"
35 #include "vector.h"
36 #include "DOP.h"
37 
39 {
40  //to get the console output in the debugging mode.
41  setvbuf(stdout, NULL, _IONBF, 0);
42  setvbuf(stderr, NULL, _IONBF, 0);
43  uint8_t n = 8; //length of alphabet
44  uint8_t k = 4; //length of figures
45 
46  matrix_t ref_matrix[][3] = { { 1, 1, 1 },
47  { 20, 1, 1 },
48  { 20, 20, 1 },
49  { 1, 20, 1 },
50  { 1, 1, 20 },
51  { 20, 1, 20 },
52  { 20, 20, 20 },
53  { 1, 20, 20 } };
54 
55  matrix_t opt_noised_r_vec[k];
56 
57  matrix_t sol_x[3];
58  matrix_t optimal_start_pos[3];
59  matrix_t tmp_start_optimal[3];
60  uint8_t optimal_anchor_combi_arr[k];
61  uint8_t PDOP_Threshold = 8;
62  uint8_t maxit = 100;
63  uint8_t it = 0;
64  matrix_t tau = 1e-6;
65  matrix_t eps = 1e-5;
66  matrix_t beta0 = 0.2;
67  matrix_t beta1 = 0.8;
68  uint8_t d_max = 19;
69  uint8_t grid_cell_length = 2;
70  uint8_t axis_point_num = d_max / grid_cell_length;
71  uint32_t exact_point[3];
72  //matrix_t multipath[] = {5,3};
73  //matrix_t multipath[] = {4,3}; // for 4 anchors
74  matrix_t multipath[] = { 3 }; // for 4 anchors
75  uint8_t anchor_num = n;
76  uint32_t room_point_num;
77  matrix_t noised_r_vec[anchor_num];
78  uint32_t counter = 0;
79  matrix_t mean_lvm_call_num = 0;
80  matrix_t sigma = 0.1;
81  int32_t initial_seed_val = 3;
82  int32_t seed = 0;
83 
84  /************************************************/
85 
86  puts("################ START ################");
87 
88  // initialize seed's value
89  seed = initial_seed_val;
90 
91  //Sampling the room
92  for (uint32_t z = 0; z <= axis_point_num; z++) {
93  exact_point[2] = ref_matrix[0][2] + grid_cell_length * z;
94 
95  for (uint32_t y = 0; y <= axis_point_num; y++) {
96  exact_point[1] = ref_matrix[0][1]
97  + grid_cell_length * y;
98 
99  for (uint32_t x = 0; x <= axis_point_num; x++) {
100  exact_point[0] = ref_matrix[0][0]
101  + grid_cell_length * x;
102 
103  if (!is_anchor(anchor_num, ref_matrix,
104  exact_point)) {
105  printf("seed = %ld\n", (long) seed);
106  sim_UWB_dist(n, ref_matrix, exact_point,
107  sigma, 2, multipath,
108  seed, noised_r_vec);
109  seed = 2 * initial_seed_val + 1;
110  initial_seed_val++;
111 
113  ref_matrix,
114  noised_r_vec,
115  optimal_anchor_combi_arr,
116  optimal_start_pos);
117 
118  // PDOP & Levenberg
119  matrix_t opt_ref_matrix_comb_matrix[k][3];
120 
122  ref_matrix, k,
123  optimal_anchor_combi_arr,
124  opt_ref_matrix_comb_matrix);
125  matrix_t PDOP =
126  get_PDOP(k,
127  opt_ref_matrix_comb_matrix,
128  optimal_start_pos);
129 
131  noised_r_vec,
132  optimal_anchor_combi_arr,
133  opt_noised_r_vec);
134 
135  if (PDOP > PDOP_Threshold) {
137  noised_r_vec, k,
138  optimal_anchor_combi_arr,
139  opt_noised_r_vec);
140  vector_copy(3,
141  optimal_start_pos,
142  tmp_start_optimal);
143  it =
145  3,
146  opt_ref_matrix_comb_matrix,
147  optimal_start_pos,
148  opt_noised_r_vec,
149  eps,
150  tau,
151  beta0,
152  beta1,
153  maxit,
154  sol_x,
159  mean_lvm_call_num++;
160 
161  }
162  else {
163  it = 0;
164  vector_copy(3,
165  optimal_start_pos,
166  sol_x);
167  }
168 
169  puts(
170  "------------------------------------------------------------");
171  printf("PDOP = %f | it = %d\n", PDOP,
172  it);
173 
174  printf("r_noised_vec = ");
175  //matrix_print_array(r_noised_vec, n, 7);
176  vector_flex_print(n, noised_r_vec, 5,
177  7);
178  puts("");
179  printf(
180  "anchors_optimal = {%u, %u, %u, %u}\n",
181  optimal_anchor_combi_arr[0],
182  optimal_anchor_combi_arr[1],
183  optimal_anchor_combi_arr[2],
184  optimal_anchor_combi_arr[3]);
185  printf(
186  "exact point = {%lu, %lu, %lu}\n",
187  (unsigned long)exact_point[0],
188  (unsigned long)exact_point[1],
189  (unsigned long)exact_point[2]);
190  if (it != 0) {
191  printf(
192  "start_optimal = {%f, %f, %f}\n",
193  tmp_start_optimal[0],
194  tmp_start_optimal[1],
195  tmp_start_optimal[2]);
196  }
197 
198  printf("x_sol = {%.7f, %.7f, %.7f}\n",
199  sol_x[0],
200  sol_x[1],
201  sol_x[2]);
202  counter++;
203  }//x
204  }//if
205  }//y
206  }//Z
207 
208  room_point_num = counter;
209  puts(
210  "----------------------------- End Simulation -------------------------------");
211  printf("Counter = %lu\n", (unsigned long) counter);
212  printf("room point num = %lu\n", (unsigned long) room_point_num);
213  printf("anchors num = %u\n", n);
214  printf("LVM call number = %f\n", mean_lvm_call_num);
215  printf("mean LVM call number = %f\n",
216  mean_lvm_call_num / room_point_num);
217 }
is_anchor
bool is_anchor(uint8_t m, matrix_t ref_matr[m][3], uint32_t point[3])
Determine if a point is an anchor or not.
Definition: multipath_dist_detection_mitigation.c:65
get_optimal_partial_r_noised_vec
void get_optimal_partial_r_noised_vec(uint8_t k, matrix_t r_noised_vec[], uint8_t optimal_anchors_comb[k], matrix_t opt_sub_r_noised_vec[k])
Compute noised distances corresponding to the optimal partial matrix.
Definition: multipath_dist_detection_mitigation.c:123
vector_flex_print
void vector_flex_print(uint32_t length, vector_t arr[], uint8_t before_dot, uint8_t after_dot)
Display the values of the vector's elements.
Definition: vector.c:284
loc_levenberg_marquardt
uint8_t loc_levenberg_marquardt(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t start_pos[3], matrix_t measured_data_vec[ref_points_num], matrix_t eps, matrix_t tau, matrix_t beta0, matrix_t beta1, uint8_t max_iter_num, matrix_t est_pos[3], void(*f_i)(uint8_t ref_points_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[]), void(*jacobian_get_JTJ)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTJ[3][3]), void(*jacobian_get_JTf)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTf[3]), void(*jacobian_get_J_mul_s)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t s[3], matrix_t J_s[ref_points_num]))
Implements the Levenberg–Marquardt (LVM) algorithm.
Definition: loc_levenberg_marquardt.c:126
recog_mitigate_multipath
void recog_mitigate_multipath(uint8_t k, uint8_t m, matrix_t ref_Matrix[m][3], matrix_t r_noised_vec[m], uint8_t anchors_optimal[k], matrix_t start_optimal[3])
Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
Definition: multipath_dist_detection_mitigation.c:133
dist_based_jacobian.h
Jacobian function of distance-based localization systems.
loc_levenberg_marquardt.h
Implement the Levenberg–Marquardt (LVM) algorithm for position optimization.
dist_based_jacobian_get_J_mul_s
void dist_based_jacobian_get_J_mul_s(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t s[3], matrix_t J_s[ref_points_num])
Computes of distance-based localization system.
Definition: dist_based_jacobian.c:132
dist_based_f_i
void dist_based_f_i(uint8_t ref_points_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[])
Defines the error function of a distance-based localization system.
Definition: dist_based_fi.c:47
multipath_dist_detection_mitigation.h
Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
get_PDOP
matrix_t get_PDOP(uint8_t m, matrix_t ref_Matrix[m][3], matrix_t true_pos[m])
Compute the Position Dilution of Precision (PDOP).
Definition: DOP.c:30
multipath_algo_own_norm_distr_test
void multipath_algo_own_norm_distr_test(void)
Example of the algorithm for the recognition and mitigation of multipath effects.
Definition: multipath_algo_own_norm_distr_test.c:38
get_optimal_partial_ref_matrix
void get_optimal_partial_ref_matrix(uint8_t anchors_num, matrix_t ref_matrix[anchors_num][3], uint8_t k, uint8_t optimal_anchors_comb[k], matrix_t opt_partial_ref_matrix[k][3])
Compute the optimal partial matrix including reference points.
Definition: multipath_dist_detection_mitigation.c:108
vector_get_elements
void vector_get_elements(vector_t src_vec[], uint8_t k, uint8_t index_vec[], vector_t dst_vec[])
Get the elements of the vector by an index vector.
Definition: vector.c:235
dist_based_jacobian_get_JTJ
void dist_based_jacobian_get_JTJ(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t dist_vec[ref_points_num], matrix_t JTJ[3][3])
Defines of distance-based localization system.
Definition: dist_based_jacobian.c:56
vector_copy
void vector_copy(uint8_t size, vector_t src_arr[], vector_t dest_arr[])
Copy the elements of the source vector to the destination vector.
Definition: vector.c:37
dist_based_fi.h
Error function of distance-based localization systems.
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector.h
Vector computations.
norm_dist_rnd_generator.h
Generating normally distributed random numbers.
shell_sort.h
Implement the Shell sort algorithm.
DOP.h
Compute the Position Dilution of Precision (PDOP).
dist_based_jacobian_get_JTf
void dist_based_jacobian_get_JTf(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t dist_vec[ref_points_num], vector_t JTf[3])
Defines of distance-based localization system.
Definition: dist_based_jacobian.c:28
sim_UWB_dist
void sim_UWB_dist(uint8_t m, matrix_t ref_matrix[m][3], uint32_t exact_point[], matrix_t sigma, uint8_t n, matrix_t multipath[n], int seed, matrix_t r_noised_vec[])
Simulate an UWB-based localization system.
Definition: multipath_dist_detection_mitigation.c:78