RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
magnetic_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 
23 #include <math.h>
24 #include <string.h>
25 #include <stdio.h>
26 #include <stdlib.h>
27 
28 #include "magnetic_based_fi.h"
30 #include "matrix.h"
31 
32 void magnetic_based_jacobian_get_J(uint8_t ref_point_num,
33  matrix_t ref_point_matrix[ref_point_num][3],
34  matrix_t point[],
35  matrix_t J[ref_point_num][3])
36 {
37 
38  for (int i = 0; i < ref_point_num; i++) {
39 
40  matrix_t X_Xi = point[0] - ref_point_matrix[i][0];
41  matrix_t Y_Yi = point[1] - ref_point_matrix[i][1];
42  matrix_t Z_Zi = point[2] - ref_point_matrix[i][2];
43 
44  matrix_t X_Xi_sq = pow(X_Xi, 2);
45  matrix_t Y_Yi_sq = pow(Y_Yi, 2);
46  matrix_t Z_Zi_sq = pow(Z_Zi, 2);
47 
48  J[i][0] =
49  K_T * (X_Xi)
50  * ((-3.0 * X_Xi_sq
51  - 3.0 * Y_Yi_sq
52  - 15.0 * Z_Zi_sq)
53  /
54  (sqrt(
55  X_Xi_sq
56  + Y_Yi_sq
57  + 4.0
58  * Z_Zi_sq)
59  * pow(
60  (X_Xi_sq
61  + Y_Yi_sq
62  + Z_Zi_sq),
63  3)));
64 
65  J[i][1] =
66  K_T * (Y_Yi)
67  * ((-3.0 * X_Xi_sq
68  - 3.0 * Y_Yi_sq
69  - 15.0 * Z_Zi_sq)
70  /
71  (sqrt(
72  X_Xi_sq
73  + Y_Yi_sq
74  + 4.0
75  * Z_Zi_sq)
76  * pow(
77  (X_Xi_sq
78  + Y_Yi_sq
79  + Z_Zi_sq),
80  3)));
81 
82  J[i][2] =
83  4.0 * K_T * (Z_Zi)
84  * ((-3.0 * Z_Zi_sq)
85  /
86  (sqrt(
87  X_Xi_sq
88  + Y_Yi_sq
89  + 4.0
90  * Z_Zi_sq)
91  * pow(
92  (X_Xi_sq
93  + Y_Yi_sq
94  + Z_Zi_sq),
95  3)));
96  }
97 }
98 
99 void magnetic_based_jacobian_get_JTJ(uint8_t ref_points_num,
100  matrix_t ref_point_matrix[ref_points_num][3],
101  matrix_t point[3], matrix_t *unused, matrix_t JTJ[3][3])
102 {
103  matrix_t J[ref_points_num][3];
104 
105  magnetic_based_jacobian_get_J(ref_points_num, ref_point_matrix, point,
106  J);
107  matrix_trans_mul_itself(ref_points_num, 3, J, JTJ);
108 
109  (void)unused;
110 }
111 
112 void magnetic_based_jacobian_get_JTf(uint8_t ref_points_num,
113  matrix_t ref_points_matrix[ref_points_num][3],
114  matrix_t point[3], matrix_t Bi_vec[ref_points_num],
115  matrix_t JTf[3])
116 {
117 
118  matrix_t J[ref_points_num][3];
119  matrix_t f_vec[ref_points_num];
120 
121  magnetic_based_f_i(ref_points_num, ref_points_matrix, point, Bi_vec,
122  f_vec);
123 
124  magnetic_based_jacobian_get_J(ref_points_num, ref_points_matrix, point,
125  J);
126  matrix_trans_mul_vec(ref_points_num, 3, J, ref_points_num, f_vec, JTf);
127 }
128 
129 //compute: J(x)*s
130 void magnetic_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
131  matrix_t ref_point_matrix[ref_points_num][3],
132  matrix_t point[3],
133  matrix_t s[3],
134  matrix_t J_s[ref_points_num])
135 {
136  uint8_t i;
137  matrix_t X_Xi, Y_Yi, Z_Zi;
138  matrix_t X_Xi_sq, Y_Yi_sq, Z_Zi_sq;
139  matrix_t j_i_0, j_i_1, j_i_2;
140 
141  for (i = 0; i < ref_points_num; i++) {
142  X_Xi = point[0] - ref_point_matrix[i][0];
143  Y_Yi = point[1] - ref_point_matrix[i][1];
144  Z_Zi = point[2] - ref_point_matrix[i][2];
145 
146  X_Xi_sq = pow(X_Xi, 2);
147  Y_Yi_sq = pow(Y_Yi, 2);
148  Z_Zi_sq = pow(Z_Zi, 2);
149 
150  j_i_0 =
151  K_T * (X_Xi)
152  * ((-3.0 * X_Xi_sq
153  - 3.0 * Y_Yi_sq
154  - 15.0 * Z_Zi_sq)
155  /
156  (sqrt(
157  X_Xi_sq
158  + Y_Yi_sq
159  + 4.0
160  * Z_Zi_sq)
161  * pow(
162  (X_Xi_sq
163  + Y_Yi_sq
164  + Z_Zi_sq),
165  3)));
166  j_i_1 =
167  K_T * (Y_Yi)
168  * ((-3.0 * X_Xi_sq
169  - 3.0 * Y_Yi_sq
170  - 15.0 * Z_Zi_sq)
171  /
172  (sqrt(
173  X_Xi_sq
174  + Y_Yi_sq
175  + 4.0
176  * Z_Zi_sq)
177  * pow(
178  (X_Xi_sq
179  + Y_Yi_sq
180  + Z_Zi_sq),
181  3)));
182  j_i_2 =
183  4.0 * K_T * (Z_Zi)
184  * ((-3.0 * Z_Zi_sq)
185  /
186  (sqrt(
187  X_Xi_sq
188  + Y_Yi_sq
189  + 4.0
190  * Z_Zi_sq)
191  * pow(
192  (X_Xi_sq
193  + Y_Yi_sq
194  + Z_Zi_sq),
195  3)));
196 
197  J_s[i] = j_i_0 * s[0] + j_i_1 * s[1] + j_i_2 * s[2];
198  }
199 }
matrix_trans_mul_itself
void matrix_trans_mul_itself(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t AT_mul_A[n][n])
Compute the multiplication of the transpose of a matrix with itself.
Definition: matrix.c:545
magnetic_based_position.h
Functions of of DC-pulsed, magnetic localization system.
matrix_trans_mul_vec
void matrix_trans_mul_vec(uint8_t m, uint8_t n, matrix_t A[m][n], uint8_t b_size, matrix_t b_vec[m], matrix_t c_vec[n])
Compute the multiplication of transposed matrix with column vector.
Definition: matrix.c:511
magnetic_based_fi.h
Error function of DC-pulsed, magnetic localization system.
matrix.h
Matrix computations.
magnetic_based_jacobian_get_JTJ
void magnetic_based_jacobian_get_JTJ(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t *unused, matrix_t JTJ[3][3])
Defines of magnetic-based localization system.
Definition: magnetic_based_jacobian.c:99
K_T
#define K_T
The number of turns of the wire.
Definition: magnetic_based_position.h:75
magnetic_based_jacobian_get_J
void magnetic_based_jacobian_get_J(uint8_t ref_point_num, matrix_t ref_point_matrix[ref_point_num][3], matrix_t point[], matrix_t J[ref_point_num][3])
Computes the Jacobian matrix of magnetic-based localization system.
Definition: magnetic_based_jacobian.c:32
magnetic_based_jacobian_get_J_mul_s
void magnetic_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 magnetic-based localization system.
Definition: magnetic_based_jacobian.c:130
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
magnetic_based_f_i
void magnetic_based_f_i(uint8_t ref_points_num, matrix_t ref_points_matrix[][3], matrix_t point[], matrix_t Bi_vec[], matrix_t f_vec[])
Defines the error function of a magnetic-based localization system.
Definition: magnetic_based_fi.c:29
magnetic_based_jacobian_get_JTf
void magnetic_based_jacobian_get_JTf(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t point[3], matrix_t Bi_vec[ref_points_num], matrix_t JTf[3])
Defines of magnetic-based localization system.
Definition: magnetic_based_jacobian.c:112