RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
trilateration.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 <stdio.h>
25 #include <string.h>
26 
28 #include "matrix.h"
29 #include "svd.h"
30 #include "trilateration.h"
31 
32 void trilateration1(uint8_t anchor_num,
33  matrix_t anchor_pos_matrix[anchor_num][3],
34  matrix_t pseudo_inv_matrix[4][anchor_num],
35  matrix_t homog_sol_arr[],
36  matrix_t dist_arr[],
37  matrix_t solution_x1[],
38  matrix_t solution_x2[]
39  )
40 {
41 
42  matrix_t b_arr[anchor_num];
43  matrix_t particular_solution_arr[4];
44 
45  if (anchor_num == 3) {
46  trilateration_get_b_vector(anchor_num, dist_arr,
47  anchor_pos_matrix, b_arr);
49  pseudo_inv_matrix, b_arr,
50  particular_solution_arr);
52  particular_solution_arr,
53  homog_sol_arr, solution_x1, solution_x2);
54 
55  }
56 
57  else if (anchor_num > 3) {
58  trilateration_get_b_vector(anchor_num, dist_arr,
59  anchor_pos_matrix, b_arr);
60 
61  //compute the solution
62  trilateration_solve_linear_equation(anchor_num, anchor_num,
63  pseudo_inv_matrix, b_arr,
64  solution_x1);
65  }
66 }
67 
68 //On MCU Processing
69 void trilateration2(uint8_t anchor_num,
70  matrix_t anchor_pos_matrix[anchor_num][3],
71  matrix_t dist_arr[],
72  matrix_t solution_x1[],
73  matrix_t solution_x2[])
74 {
75 
76  matrix_t b_arr[anchor_num];
77  matrix_t A_matrix[anchor_num][4];
78  matrix_t pseudo_inv_matrix[4][anchor_num];
79  uint8_t rank = 0;
80 
81  if (anchor_num == 3) {
82  //2nd compute the particular solution
83  matrix_t Xp[4];
84  trilateration_get_particular_solution(anchor_num, anchor_num,
85  anchor_pos_matrix, dist_arr, Xp);
86 
87  //3th compute the homogeneous solution
88  matrix_t Xh[4];
90  anchor_num, anchor_pos_matrix, Xh);
91 
92  //solve the quadratic equation
93  if (rank == 3) {
95  solution_x1,
96  solution_x2);
97  }
98  else {
99  puts("rank!=3, the equation system is not solvable.");
100  }
101 
102  }
103 
104  else if (anchor_num > 3) {
105  //compute A-matrix and b_vector
106  trilateration_get_A_matrix(anchor_num, anchor_pos_matrix,
107  A_matrix);
108  trilateration_get_b_vector(anchor_num, dist_arr,
109  anchor_pos_matrix, b_arr);
110 
111  //compute the pseudo inverse
112  moore_penrose_get_pinv(anchor_num, 4, A_matrix,
113  pseudo_inv_matrix);
114  //compute the solution
116  pseudo_inv_matrix, b_arr,
117  solution_x1);
118  }
119 }
120 
122  matrix_t pseudo_inv_matrix[4][3],
123  matrix_t b_arr[],
124  matrix_t particular_solu_arr[])
125 {
126  int i = 0;
127  int j = 0;
128 
129  memset(particular_solu_arr, 0.0, 4 * sizeof(matrix_t));
130 
131  for (; i < 4; i++) {
132  for (; j < 3; j++) {
133  particular_solu_arr[i] += pseudo_inv_matrix[i][j]
134  * b_arr[j];
135  //DEBUGF("pseudo_inv_matrix[%d][%d] = %f\n b_arr[%d] = %f\n", i, j, (double) pseudo_inv_matrix[i][j], j, (double) b_arr[j]);
136  //printf("pseudo_inv_matrix[%d][%d] = %f\n b_arr[%d] = %f\n", i, j, (double) pseudo_inv_matrix[i][j], j, (double) b_arr[j]);
137  }
138  j = 0;
139  }
140 #ifdef DEBUG_ENABLED
141  i = 0;
142  for (; i < 4; i++) {
143  DEBUGF("particular_solution_arr[%d] = %f\n", i,
144  (double)particular_solu_arr[i]);
145  }
146 #endif
147 }
148 
150  matrix_t anchor_pos_matrix[anchor_num][3],
151  matrix_t Xh[])
152 {
153  uint8_t rank = 0;
154  matrix_t A[anchor_num][4];
155  uint8_t m = anchor_num;
156  uint8_t n = 4;
157  uint8_t s_length = svd_get_single_values_num(m, n);
158  matrix_t s[s_length];
159  matrix_dim_t u_dim;
160 
161  svd_get_U_dim(m, n, &u_dim);
162  matrix_t U[u_dim.row_num][u_dim.col_num];
163  matrix_t S[u_dim.col_num][n];
164  matrix_t V[n][n];
165 
166  trilateration_get_A_matrix(anchor_num, anchor_pos_matrix, A);
167  svd(m, n, A, u_dim.row_num, u_dim.col_num, U, S, V, s_length, s);
168 
169  //the forth column of the V matix
170  for (int i = 0; i < n; i++) {
171  Xh[i] = V[i][3];
172  }
173 
174  rank = matrix_get_rank(m, n, s, s_length);
175 
176  return rank;
177 }
178 
179 void trilateration_get_particular_solution(uint8_t m, uint8_t n,
180  matrix_t anchor_pos_mat[m][n],
181  matrix_t dist_arr[], matrix_t Xp[])
182 {
183  matrix_t A[m][4];
184  matrix_t pinvA[4][m];
185  matrix_t b[m];
186 
187  trilateration_get_b_vector(m, dist_arr, anchor_pos_mat, b);
188  trilateration_get_A_matrix(m, anchor_pos_mat, A);
189  moore_penrose_get_pinv(m, 4, A, pinvA);
190  matrix_mul_vec(4, m, pinvA, b, Xp);
191 }
192 
194  matrix_t Xh[],
195  matrix_t solution_x1[],
196  matrix_t solution_x2[])
197 {
198 
199  matrix_t a = 0, b = 0, c = 0, t1 = 0, t2 = 0, delta = 0;
200  int i = 0;
201 
202  //compute polynomial coefficients: a*t^2 + b*t^+ c
203  a = pow(Xh[1], 2)
204  + pow(Xh[2], 2)
205  + pow(Xh[3], 2);
206  b = 2
207  * (Xh[1] * Xp[1]
208  + Xh[2] * Xp[2]
209  + Xh[3] * Xp[3])
210  - Xh[0];
211  c = pow(Xp[1], 2) + pow(Xp[2], 2)
212  + pow(Xp[3], 2) - Xp[0];
213 
214  //compute the quadratic equation a*t^2 + b*t+ c
215  delta = b * b - 4 * a * c;
216  if ((delta > 0) && (a != 0)) {
217  if (solution_x1 != NULL) {
218  t1 = (-b + sqrt(delta)) / (2 * a);
219  }
220  if (solution_x2 != NULL) {
221  t2 = (-b - sqrt(delta)) / (2 * a);
222  }
223  }
224  else if ((delta == 0) && (a != 0)) {
225  if (solution_x1 != NULL) {
226  t1 = -b / (2 * a);
227  }
228  if (solution_x2 != NULL) {
229  t2 = t1;
230  }
231 
232  }
233  else if (delta < 0) { // take only a real part
234  if (solution_x1 != NULL) {
235  t1 = -b / (2 * a);
236  }
237  if (solution_x2 != NULL) {
238  t2 = -b / (2 * a);
239  }
240  }
241 
242  for (; i < 4; i++) {
243  if (solution_x1 != NULL) {
244  solution_x1[i] = Xp[i]
245  + t1 * Xh[i];
246  }
247  if (solution_x2 != NULL) {
248  solution_x2[i] = Xp[i]
249  + t2 * Xh[i];
250  }
251  }
252 
253  //DEBUGF("a=%f, b=%f, c=%f, t1=%f, t2=%f\n", (double)a, (double)b, (double)c, (double)t1, (double)t2);
254 }
255 
256 void trilateration_get_A_matrix(uint8_t m, matrix_t anchor_pos_matrix[m][3],
257  matrix_t A_matrix[m][4])
258 {
259  int i, j;
260 
261  //the first column is equal 1
262  for (i = 0; i < m; i++) {
263  A_matrix[i][0] = 1;
264  }
265 
266  //the second, third and firth columns are equal: -2x, -2y, -2z respectively
267  for (i = 0; i < m; i++) {
268  for (j = 1; j < 4; j++) {
269  A_matrix[i][j] = -2 * anchor_pos_matrix[i][j - 1];
270 
271  }
272  }
273 }
274 
275 void trilateration_get_b_vector(uint8_t anchor_num, matrix_t dist_arr[],
276  matrix_t anchor_pos_matrix[anchor_num][3],
277  matrix_t b_vec[])
278 {
279 
280  uint8_t i, j;
281 
282  for (i = 0; i < anchor_num; i++) {
283  b_vec[i] = dist_arr[i] * dist_arr[i];
284  for (j = 0; j < 3; j++) {
285  b_vec[i] -= anchor_pos_matrix[i][j]
286  * anchor_pos_matrix[i][j];
287  }
288  }
289 }
290 
291 void trilateration_solve_linear_equation(uint8_t line_num, uint8_t col_num,
292  matrix_t pseudo_inv_matrix[line_num][col_num],
293  matrix_t b_vec[], matrix_t sol_vec[])
294 {
295  int i, j;
296 
297  memset(sol_vec, 0, col_num * sizeof(matrix_t));
298 
299  for (i = 0; i < line_num; i++) {
300  for (j = 0; j < col_num; j++) {
301  sol_vec[i] += pseudo_inv_matrix[i][j] * b_vec[j];
302  }
303  }
304 }
moore_penrose_get_pinv
int8_t moore_penrose_get_pinv(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t pinv_A[n][m])
Calculate the Moore–Penrose inverse of a rectangular matrix.
Definition: moore_penrose_pseudo_inverse.c:38
trilateration_get_quadratic_equation_solution
void trilateration_get_quadratic_equation_solution(matrix_t Xp[], matrix_t Xh[], matrix_t solution_x1[], matrix_t solution_x2[])
Solve a quadratic equation.
Definition: trilateration.c:193
trilateration_get_b_vector
void trilateration_get_b_vector(uint8_t anchor_num, matrix_t dist_arr[], matrix_t anchor_pos_matrix[anchor_num][3], matrix_t b_vec[])
Computes the vector of the equation system: .
Definition: trilateration.c:275
matrix_mul_vec
void matrix_mul_vec(uint8_t m, uint8_t n, matrix_t matrix[m][n], matrix_t vec[n], matrix_t dst_arr[m])
Compute the multiplication of a matrix with a column vector.
Definition: matrix.c:434
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
trilateration.h
Implement the trilateration algorithm.
matrix_dim_t::row_num
uint8_t row_num
the row number
Definition: matrix.h:61
svd
void svd(uint8_t m, uint8_t n, matrix_t A[m][n], uint8_t u_m, uint8_t u_n, matrix_t U[u_m][u_n], matrix_t S[u_n][n], matrix_t V[n][n], uint8_t sing_vec_length, matrix_t singl_values_vec[])
Compute the Singular-Value Decomposition (SVD) of a matrix.
Definition: svd.c:119
matrix_get_rank
uint8_t matrix_get_rank(uint8_t m, uint8_t n, matrix_t singl_values_arr[], uint8_t length)
Compute the rank of a matrix.
Definition: matrix.c:312
matrix.h
Matrix computations.
moore_penrose_pseudo_inverse.h
Moore–Penrose algorithm to compute the pseudo-inverse of a matrix.
trilateration_get_rank_and_homogeneous_solution
uint8_t trilateration_get_rank_and_homogeneous_solution(uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t Xh[])
Compute the rank and the solution of the homogeneous system .
Definition: trilateration.c:149
trilateration_solve_linear_equation
void trilateration_solve_linear_equation(uint8_t line_num, uint8_t col_num, matrix_t pseudo_inv_matrix[line_num][col_num], matrix_t b_vec[], matrix_t sol_vec[])
Solve a linear equation.
Definition: trilateration.c:291
matrix_dim_t
A structure to define the row and column number of a matrix.
Definition: matrix.h:60
svd.h
Algorithm for the Singular Value Decomposition (SVD).
svd_get_single_values_num
uint8_t svd_get_single_values_num(uint8_t m, uint8_t n)
Calculate the number of the singular values.
Definition: svd.c:114
trilateration_preprocessed_get_particular_solution
void trilateration_preprocessed_get_particular_solution(matrix_t pseudo_inv_matrix[4][3], matrix_t b_arr[], matrix_t particular_solu_arr[])
Compute the particular solution, which is the general solution of .
Definition: trilateration.c:121
trilateration_get_A_matrix
void trilateration_get_A_matrix(uint8_t m, matrix_t anchor_pos_matrix[m][3], matrix_t A_matrix[m][4])
Computes the matrix of the equation system: .
Definition: trilateration.c:256
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
trilateration_get_particular_solution
void trilateration_get_particular_solution(uint8_t m, uint8_t n, matrix_t anchor_pos_mat[m][n], matrix_t dist_arr[], matrix_t Xp[])
Compute the particular solution, which is the general solution of .
Definition: trilateration.c:179
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
svd_get_U_dim
void svd_get_U_dim(uint8_t m, uint8_t n, matrix_dim_t *u_dim)
Calculate the dimension of the matrix U.
Definition: svd.c:96
matrix_dim_t::col_num
uint8_t col_num
the column number
Definition: matrix.h:62