RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
dist_based_jacobian.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 
22 #include <math.h>
23 
24 #include "matrix.h"
25 #include "vector.h"
26 #include "dist_based_fi.h"
27 
28 void dist_based_jacobian_get_JTf(uint8_t ref_points_num,
29  matrix_t ref_point_matrix[ref_points_num][3],
30  matrix_t point[3],
31  matrix_t dist_vec[ref_points_num],
32  vector_t JTf[3])
33 {
34  uint8_t i;
35  matrix_t f_i = 0.0;
36  matrix_t denom = 0.0;
37 
38  vector_clear(3, JTf);
39  for (i = 0; i < ref_points_num; i++) {
40  f_i =
41  dist_based_fi(point,
42  (matrix_t *)&ref_point_matrix[i],
43  dist_vec[i]);
44  denom = f_i + dist_vec[i];
45  if (denom != 0) {
46  JTf[0] += (point[0] - ref_point_matrix[i][0]) * f_i
47  / denom;
48  JTf[1] += (point[1] - ref_point_matrix[i][1]) * f_i
49  / denom;
50  JTf[2] += (point[2] - ref_point_matrix[i][2]) * f_i
51  / denom;
52  }
53  }
54 }
55 
56 void dist_based_jacobian_get_JTJ(uint8_t ref_points_num,
57  matrix_t ref_point_matrix[ref_points_num][3],
58  matrix_t point[3],
59  matrix_t dist_vec[ref_points_num],
60  matrix_t JTJ[3][3])
61 {
62  uint8_t i;
63  matrix_t denom = 0.0;
64  matrix_t f_i = 0.0;
65 
66  matrix_clear(3, 3, JTJ);
67  for (i = 0; i < ref_points_num; i++) {
68  f_i =
69  dist_based_fi(point,
70  (matrix_t *)&ref_point_matrix[i],
71  dist_vec[i]) + dist_vec[i];
72  denom = f_i * f_i;
73  if (denom != 0) {
74  JTJ[0][0] += (point[0] - ref_point_matrix[i][0]) *
75  (point[0] - ref_point_matrix[i][0])
76  / denom;
77  JTJ[1][1] += (point[1] - ref_point_matrix[i][1]) *
78  (point[1] - ref_point_matrix[i][1])
79  / denom;
80  JTJ[2][2] += (point[2] - ref_point_matrix[i][2]) *
81  (point[2] - ref_point_matrix[i][2])
82  / denom;
83  JTJ[0][1] += (point[0] - ref_point_matrix[i][0]) *
84  (point[1] - ref_point_matrix[i][1])
85  / denom;
86  JTJ[0][2] += (point[0] - ref_point_matrix[i][0]) *
87  (point[2] - ref_point_matrix[i][2])
88  / denom;
89  JTJ[1][2] += (point[1] - ref_point_matrix[i][1]) *
90  (point[2] - ref_point_matrix[i][2])
91  / denom;
92  }
93  }
94 
95  JTJ[1][0] = JTJ[0][1];
96  JTJ[2][0] = JTJ[0][2];
97  JTJ[2][1] = JTJ[1][2];
98 }
99 
100 void dist_based_jacobian_get_J(uint8_t ref_points_num, matrix_t point[3],
101  matrix_t ref_point_matrix[ref_points_num][3],
102  matrix_t J[ref_points_num][3])
103 {
104  uint8_t i;
105  matrix_t denom;
106 
107  for (i = 0; i < ref_points_num; i++) {
108  denom =
109  sqrt(
110  pow(
111  point[0]
112  - ref_point_matrix[i][0],
113  2)
114  +
115  pow(
116  point[1]
117  - ref_point_matrix[i][1],
118  2)
119  + pow(
120  point[2]
121  - ref_point_matrix[i][2],
122  2));
123  if (denom != 0) {
124  J[i][0] = (point[0] - ref_point_matrix[i][0]) / denom;
125  J[i][1] = (point[1] - ref_point_matrix[i][1]) / denom;
126  J[i][2] = (point[2] - ref_point_matrix[i][2]) / denom;
127  }
128  }
129 }
130 
131 //compute: J(x)*s
132 void dist_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
133  matrix_t ref_point_matrix[ref_points_num][3],
134  matrix_t point[3], matrix_t s[3],
135  matrix_t J_s[ref_points_num])
136 {
137  uint8_t i;
138  matrix_t denom;
139  matrix_t j_i_0 = 0;
140  matrix_t j_i_1 = 0;
141  matrix_t j_i_2 = 0;
142 
143  for (i = 0; i < ref_points_num; i++) {
144  denom =
145  sqrt(
146  pow(
147  point[0]
148  - ref_point_matrix[i][0],
149  2)
150  +
151  pow(
152  point[1]
153  - ref_point_matrix[i][1],
154  2)
155  + pow(
156  point[2]
157  - ref_point_matrix[i][2],
158  2));
159  if (denom != 0) {
160  j_i_0 = (point[0] - ref_point_matrix[i][0]) / denom;
161  j_i_1 = (point[1] - ref_point_matrix[i][1]) / denom;
162  j_i_2 = (point[2] - ref_point_matrix[i][2]) / denom;
163  }
164  J_s[i] = j_i_0 * s[0] + j_i_1 * s[1] + j_i_2 * s[2];
165  }
166 }
matrix_clear
void matrix_clear(uint8_t m, uint8_t n, matrix_t matrix[m][n])
Clear all the elements of the vector.
Definition: matrix.c:46
vector_t
#define vector_t
Define the data type of the vector elements.
Definition: vector.h:33
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
matrix.h
Matrix computations.
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_clear
void vector_clear(uint8_t size, vector_t arr[])
Clear all the elements of the vector.
Definition: vector.c:32
dist_based_jacobian_get_J
void dist_based_jacobian_get_J(uint8_t ref_points_num, matrix_t point[3], matrix_t ref_point_matrix[ref_points_num][3], matrix_t J[ref_points_num][3])
Computes the Jacobian matrix of distance-based localization system.
Definition: dist_based_jacobian.c:100
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.
dist_based_fi
matrix_t dist_based_fi(matrix_t point[3], matrix_t ref_point[3], matrix_t ri)
Defines the error function of a distance-based localization system.
Definition: dist_based_fi.c:26
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