RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
multipath_dist_detection_mitigation.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 <math.h>
25 #include <stdbool.h>
26 #include <stdio.h>
27 #include <float.h>
28 #include <stdlib.h>
29 #include <stdint.h>
30 #include <string.h>
31 
32 #include "matrix.h"
33 #include "vector.h"
34 #include "combinatorics.h"
37 #include "shell_sort.h"
38 #include "dist_based_position.h"
39 #include "DOP.h"
40 #include "trilateration.h"
41 
43  uint32_t exact_point[])
44 {
45  matrix_t d = sqrt(
46  pow((exact_point[0] - ref_point[0]), 2)
47  + pow((exact_point[1] - ref_point[1]),
48  2)
49  + pow((exact_point[2] - ref_point[2]),
50  2));
51 
52  return d;
53 }
54 
55 bool is_member(matrix_t vector, uint8_t n, matrix_t multipath[n])
56 {
57  for (int i = 0; i < n; i++) {
58  if (vector == multipath[i]) {
59  return true;
60  }
61  }
62  return false;
63 }
64 
65 bool is_anchor(uint8_t m, matrix_t ref_matr[m][3], uint32_t point[3])
66 {
67 
68  for (uint8_t i = 0; i < m; i++) {
69  if (point[0] == ref_matr[i][0] && point[1] == ref_matr[i][1]
70  && point[2] == ref_matr[i][2]) {
71  return true;
72  }
73 
74  }
75  return false;
76 }
77 
78 void sim_UWB_dist(uint8_t m, matrix_t ref_matrix[m][3], uint32_t exact_point[],
79  matrix_t sigma, uint8_t n, matrix_t multipath[n], int seed,
80  matrix_t r_noised_vec[])
81 {
82  matrix_t M_UW = 0.88;
83  matrix_t sigma_UW = 1.522;
84  matrix_t R = 0.0;
85 
86  // set seed
87  get_rand_num(seed);
88 
89  for (int i = 0; i < m; i++) {
90  matrix_t d = get_exact_distance_to_anchor(ref_matrix[i],
91  exact_point);
92 
93  if (is_member(i + 1, n, multipath) == true) {
94 
95  R = log(1 + d)
97  sigma_UW);
98 
99  }
100  else {
101 
102  R = sigma * get_norm_distr_rand_num(0, 1);
103  }
104  r_noised_vec[i] = R + d;
105  }
106 }
107 
108 void get_optimal_partial_ref_matrix(uint8_t anchors_num,
109  matrix_t ref_matrix[anchors_num][3],
110  uint8_t k,
111  uint8_t optimal_anchors_comb[k],
112  matrix_t opt_partial_ref_matrix[k][3])
113 {
114  for (int i = 0; i < k; i++) {
115  for (int j = 0; j < 3; j++) {
116  opt_partial_ref_matrix[i][j] =
117  ref_matrix[optimal_anchors_comb[i]][j];
118 
119  }
120  }
121 }
122 
123 void get_optimal_partial_r_noised_vec(uint8_t k, matrix_t r_noised_vec[],
124  uint8_t optimal_anchors_comb[k],
125  matrix_t opt_sub_r_noised_vec[k])
126 {
127  for (int i = 0; i < k; i++) {
128  opt_sub_r_noised_vec[i] = r_noised_vec[optimal_anchors_comb[i]];
129 
130  }
131 }
132 
133 void recog_mitigate_multipath(uint8_t k, uint8_t m, matrix_t ref_matrix[m][3],
134  matrix_t noised_r_vec[m],
135  uint8_t anchors_optimal_combi[k],
136  matrix_t start_optimal_pos[3])
137 {
138  int8_t status_num;
139  uint8_t combi_arr[k];
140  matrix_t r_vec[3];
141  matrix_t part_opt_ref_matrix[k][3];
142  matrix_t opt_part_r_noised_vec[k];
143  uint8_t index_vector[k];
144  matrix_t qres_optimal = FLT_MAX;
145  matrix_t pos_solution_x1[4];
146 
147  // Initialize the combination generator
148  status_num = combinatorics_init(m, k, combi_arr);
149 
150  if (status_num == COMBI_ERROR) {
151  fprintf(stderr,
152  "Error: couldn't initialize the combination generator\n");
153 
154  return;
155  }
156 
157  while (status_num == COMBI_SUCCESS) {
158  get_optimal_partial_ref_matrix(m, ref_matrix, k, combi_arr,
159  part_opt_ref_matrix);
160 
161  get_optimal_partial_r_noised_vec(k, noised_r_vec, combi_arr,
162  opt_part_r_noised_vec);
163  // Trilateration
164  trilateration2(k, part_opt_ref_matrix, opt_part_r_noised_vec,
165  pos_solution_x1,
166  NULL);
167 
168  matrix_t residual_vec[m];
169  matrix_t residual_vec_copy[m];
170 
171  for (int i = 0; i < m; i++) {
172 
173  vector_sub(3, pos_solution_x1 + 1, ref_matrix[i],
174  r_vec);
175  residual_vec[i] = vector_get_norm2(3, r_vec)
176  - noised_r_vec[i];
177  }
178 
179  vector_square(m, residual_vec, residual_vec);
180 
181  vector_copy(m, residual_vec, residual_vec_copy);
182 
183  shell_sort(residual_vec_copy, m);
184 
185  // find index
186  vector_get_index_vector(k, m, residual_vec, residual_vec_copy,
187  index_vector);
188 
189  matrix_t res_vec_copy_sum = 0;
190 
191  for (int i = 0; i < k; i++) {
192  res_vec_copy_sum = res_vec_copy_sum
193  + residual_vec_copy[i];
194  }
195 
196  if (res_vec_copy_sum <= qres_optimal) {
197  qres_optimal = res_vec_copy_sum;
198  memcpy(anchors_optimal_combi, index_vector,
199  k * sizeof(uint8_t));
200  vector_copy(3, pos_solution_x1 + 1, start_optimal_pos);
201  }
202  status_num = combinatorics_get_next_without_rep(m, k,
203  combi_arr);
204  } //while
205 }
vector_get_norm2
vector_t vector_get_norm2(uint8_t length, vector_t arr[])
Compute the 2-norm norm of a vector.
Definition: vector.c:42
trilateration.h
Implement the trilateration algorithm.
is_member
bool is_member(matrix_t vector, uint8_t n, matrix_t multipath[n])
Determine if a candidate is a multipath or not.
Definition: multipath_dist_detection_mitigation.c:55
shell_sort
void shell_sort(vector_t *arr, uint8_t length)
Sort a data set of type utils_t by using the Shell sort algorithm.
Definition: shell_sort.c:49
vector_square
void vector_square(uint8_t n, vector_t vec[n], vector_t square_vec[n])
Compute the square of a vector.
Definition: vector.c:124
dist_based_position.h
Functions of distance-based localization systems.
get_rand_num
double get_rand_num(int seed)
Generate uniform (0.0, 1.0) random numbers by using the Linear Congruential Generator (LGC) algorithm...
Definition: norm_dist_rnd_generator.c:80
multipath_dist_detection_mitigation.h
Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
vector_get_index_vector
void vector_get_index_vector(uint8_t k, uint8_t n, vector_t unsorted_vector[n], vector_t sorted_vector[n], uint8_t index_vector[n])
Determine the index of the vector elements before sorting.
Definition: vector.c:214
matrix.h
Matrix computations.
recog_mitigate_multipath
void recog_mitigate_multipath(uint8_t k, uint8_t m, matrix_t ref_matrix[m][3], matrix_t noised_r_vec[m], uint8_t anchors_optimal_combi[k], matrix_t start_optimal_pos[3])
Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
Definition: multipath_dist_detection_mitigation.c:133
get_exact_distance_to_anchor
matrix_t get_exact_distance_to_anchor(matrix_t ref_point[], uint32_t exact_point[])
Computes the exact distance between a mobile station and a reference station.
Definition: multipath_dist_detection_mitigation.c:42
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
COMBI_ERROR
#define COMBI_ERROR
Case of an error.
Definition: combinatorics.h:31
combinatorics.h
Calculate possible without repetition in ascending order.
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
get_norm_distr_rand_num
double get_norm_distr_rand_num(double mean, double std_dev)
Get a normally distributed random number by applying the Box–Muller method.
Definition: norm_dist_rnd_generator.c:40
trilateration2
void trilateration2(uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t dist_arr[], matrix_t solution_x1[], matrix_t solution_x2[])
Implement the trilateration algorithm.
Definition: trilateration.c:69
COMBI_SUCCESS
#define COMBI_SUCCESS
Case of successfully calculated combination set.
Definition: combinatorics.h:41
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
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
vector.h
Vector computations.
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
norm_dist_rnd_generator.h
Generating normally distributed random numbers.
shell_sort.h
Implement the Shell sort algorithm.
combinatorics_init
uint8_t combinatorics_init(uint8_t n, uint8_t k, uint8_t comb_arr[])
Initialize the combinations generator.
Definition: combinatorics.c:29
DOP.h
Compute the Position Dilution of Precision (PDOP).
combinatorics_get_next_without_rep
uint8_t combinatorics_get_next_without_rep(uint8_t n, uint8_t k, uint8_t comb_arr[])
Generate the next combination.
Definition: combinatorics.c:48
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_sub
void vector_sub(uint8_t size, vector_t a_vec[], vector_t b_vec[], vector_t a_minus_b[])
Compute the subtraction of two vectors.
Definition: vector.c:94