RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
magnetic_based_position.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 
23 #include <math.h>
24 #include <complex.h>
26 #include <stdint.h>
27 #include <string.h>
28 #include <stdio.h>
29 
30 #include "matrix.h"
31 #include "vector.h"
32 #include "trilateration.h"
33 
35  matrix_t approx_value_arr[],
36  matrix_t absolute_error_arr[],
37  uint8_t length)
38 {
39  uint8_t i;
40 
41  if ((value_arr != NULL) && (approx_value_arr != NULL)
42  && (absolute_error_arr != NULL)) {
43  for (i = 0; i < length; i++) {
44  absolute_error_arr[i] = fabs(
45  value_arr[i] - approx_value_arr[i]);
46  }
47  }
48 }
49 
51  matrix_t point[3])
52 {
53  matrix_t dist = 0.0;
54  matrix_t diff_vec[3];
55 
56  vector_sub(3, ref_point, point, diff_vec);
57  dist = vector_get_norm2(3, diff_vec);
58 
59  return dist;
60 }
61 
63  matrix_t target_point[3], matrix_t k)
64 {
65  matrix_t magn_field = 0.0;
66  matrix_t r_square = 0.0;
67  uint8_t i;
68 
69  printf("ref_arr = ");
70  vector_flex_print(3, ref_point, 5, 4);
71  puts("");
72  for (i = 0; i < 3; i++) {
73  r_square += pow(target_point[i] - ref_point[i], 2);
74  }
75 
76  printf("r_sq = %f\n", r_square);
77  magn_field =
78  k
79  * sqrt(
80  3
81  * pow(
82  target_point[2]
83  - ref_point[2],
84  2)
85  + r_square)
86  / pow(r_square, 2);
87 
88  return magn_field;
89 }
90 
92  matrix_t ref_point_matrix[m][3],
93  matrix_t target_point[],
94  matrix_t k,
95  matrix_t magn_field_vec[])
96 {
97 
98  matrix_t r_square = 0.0;
99  uint8_t i, j;
100 
101  for (i = 0; i < m; i++) {
102  for (j = 0; i < 3; j++) {
103  r_square +=
104  pow(
105  target_point[i]
106  - ref_point_matrix[i][j],
107  2);
108  }
109 
110  magn_field_vec[i] =
111  k
112  * sqrt(
113  3
114  * pow(
115  target_point[2]
116  - ref_point_matrix[i][2],
117  2)
118  + r_square)
119  / pow(r_square, 2);
120  r_square = 0.0;
121  }
122 }
123 
124 
125 void magnetic_based_get_distances(matrix_t magnetic_field_strength_arr[],
126  matrix_t angular_theta_arr[],
127  matrix_t distance_arr[], uint8_t length,
128  matrix_t k)
129 {
130  int i;
131 
132  for (i = 0; i < length; i++) {
133  if (magnetic_field_strength_arr[i] == 0) {
134  distance_arr[i] = MILPS_MAX_DIST;
135  }
136  else {
137  if (angular_theta_arr[i] == 0) {
138  distance_arr[i] =
139  pow(
140  k
141  / magnetic_field_strength_arr[i],
142  (matrix_t)1
143  / 3);
144  }
145  else {
146 
147  distance_arr[i] =
148  pow(
149  k
150  * sqrt(
151  1
152  + 3
153  * pow(
154  sin(
155  angular_theta_arr[i]),
156  2))
157  / magnetic_field_strength_arr[i],
158  (matrix_t)1
159  / 3);
160  }
161  }
162  }
163 }
164 
165 
167 {
168  matrix_t r = 0;
169 
170  if (B == 0) {
171  return MILPS_MAX_DIST;
172  }
173  else {
174  if (theta == 0) {
175  r = pow(k / B, (matrix_t)1 / 3);
176  }
177  else {
178 
179  r = pow(k * sqrt(1 + 3 * pow(sin(theta), 2)) / B,
180  (matrix_t)1 / 3);
181  }
182  }
183 
184  return r;
185 }
186 
187 // PC proprocessing
189  matrix_t anchor_pos_matrix[anchor_num][3],
190  matrix_t pseudo_inv_matrix[4][anchor_num],
191  matrix_t homog_sol_arr[],
192  matrix_t solution_x1[],
193  matrix_t solution_x2[])
194 {
195 
196  matrix_t dist_arr[anchor_num];
197 
198  trilateration1(anchor_num, anchor_pos_matrix, pseudo_inv_matrix,
199  homog_sol_arr, dist_arr, solution_x1, solution_x2);
200 
201 }
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
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
MILPS_MAX_DIST
#define MILPS_MAX_DIST
The number of turns of the wire.
Definition: magnetic_based_position.h:80
trilateration.h
Implement the trilateration algorithm.
magnetic_based_get_r
matrix_t magnetic_based_get_r(matrix_t B, matrix_t theta, matrix_t k)
Computes the distance between a mobile station and a reference stations of a magnet-based localizatio...
Definition: magnetic_based_position.c:166
magnetic_based_position.h
Functions of of DC-pulsed, magnetic localization system.
matrix.h
Matrix computations.
magnetic_based_get_distances
void magnetic_based_get_distances(matrix_t magnetic_field_strength_arr[], matrix_t angular_theta_arr[], matrix_t distance_arr[], uint8_t length, matrix_t k)
Computes the distances between a mobile station and the reference stations of a magnet-based localiza...
Definition: magnetic_based_position.c:125
magnetic_based_get_magnetic_field_vec
void magnetic_based_get_magnetic_field_vec(uint8_t m, matrix_t ref_point_matrix[m][3], matrix_t target_point[], matrix_t k, matrix_t magn_field_vec[])
Computes the magnetic field strengths from a mobile station to various reference stations.
Definition: magnetic_based_position.c:91
magnetic_based_preprocessing_get_position
void magnetic_based_preprocessing_get_position(uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t pseudo_inv_matrix[4][anchor_num], matrix_t homog_sol_arr[], matrix_t solution_x1[], matrix_t solution_x2[])
Computes the position of a mobile station by a magnetic-based localization system.
Definition: magnetic_based_position.c:188
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector.h
Vector computations.
trilateration1
void trilateration1(uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t pseudo_inv_matrix[4][anchor_num], matrix_t homog_sol_arr[], matrix_t dist_arr[], matrix_t solution_x1[], matrix_t solution_x2[])
Implement the trilateration algorithm using the pre-processed pseudo-inverse matrix.
Definition: trilateration.c:32
magnetic_based_get_distances_to_anchors
matrix_t magnetic_based_get_distances_to_anchors(matrix_t ref_point[3], matrix_t point[3])
Computes the distance between a mobile station and a reference station of magnetic-based localization...
Definition: magnetic_based_position.c:50
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
magnetic_based_get_absolute_error
void magnetic_based_get_absolute_error(matrix_t value_arr[], matrix_t approx_value_arr[], matrix_t absolute_error_arr[], uint8_t length)
Computes the absolute error of a position of magnet-based localization system.
Definition: magnetic_based_position.c:34
magnetic_based_get_magnetic_field
matrix_t magnetic_based_get_magnetic_field(matrix_t ref_point[3], matrix_t target_point[3], matrix_t k)
Computes the magnetic field strength from a mobile station to a reference station.
Definition: magnetic_based_position.c:62