|
RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
|
Go to the documentation of this file.
41 setvbuf(stdout, NULL, _IONBF, 0);
42 setvbuf(stderr, NULL, _IONBF, 0);
46 matrix_t ref_matrix[][3] = { { 1, 1, 1 },
60 uint8_t optimal_anchor_combi_arr[k];
61 uint8_t PDOP_Threshold = 8;
69 uint8_t grid_cell_length = 2;
70 uint8_t axis_point_num = d_max / grid_cell_length;
71 uint32_t exact_point[3];
75 uint8_t anchor_num = n;
76 uint32_t room_point_num;
81 int32_t initial_seed_val = 3;
86 puts(
"################ START ################");
89 seed = initial_seed_val;
92 for (uint32_t z = 0; z <= axis_point_num; z++) {
93 exact_point[2] = ref_matrix[0][2] + grid_cell_length * z;
95 for (uint32_t y = 0; y <= axis_point_num; y++) {
96 exact_point[1] = ref_matrix[0][1]
97 + grid_cell_length * y;
99 for (uint32_t x = 0; x <= axis_point_num; x++) {
100 exact_point[0] = ref_matrix[0][0]
101 + grid_cell_length * x;
105 printf(
"seed = %ld\n", (
long) seed);
109 seed = 2 * initial_seed_val + 1;
115 optimal_anchor_combi_arr,
119 matrix_t opt_ref_matrix_comb_matrix[k][3];
123 optimal_anchor_combi_arr,
124 opt_ref_matrix_comb_matrix);
127 opt_ref_matrix_comb_matrix,
132 optimal_anchor_combi_arr,
135 if (PDOP > PDOP_Threshold) {
138 optimal_anchor_combi_arr,
146 opt_ref_matrix_comb_matrix,
170 "------------------------------------------------------------");
171 printf(
"PDOP = %f | it = %d\n", PDOP,
174 printf(
"r_noised_vec = ");
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]);
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]);
192 "start_optimal = {%f, %f, %f}\n",
193 tmp_start_optimal[0],
194 tmp_start_optimal[1],
195 tmp_start_optimal[2]);
198 printf(
"x_sol = {%.7f, %.7f, %.7f}\n",
208 room_point_num = counter;
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);
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.
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.
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.
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.
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.
Jacobian function of distance-based localization systems.
Implement the Levenberg–Marquardt (LVM) algorithm for position optimization.
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.
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.
Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
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).
void multipath_algo_own_norm_distr_test(void)
Example of the algorithm for the recognition and mitigation of multipath effects.
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.
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.
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.
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.
Error function of distance-based localization systems.
#define matrix_t
Define the data type of the matrix elements.
Generating normally distributed random numbers.
Implement the Shell sort algorithm.
Compute the Position Dilution of Precision (PDOP).
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.
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.