33 matrix_t anchor_pos_matrix[anchor_num][3],
34 matrix_t pseudo_inv_matrix[4][anchor_num],
45 if (anchor_num == 3) {
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);
57 else if (anchor_num > 3) {
59 anchor_pos_matrix, b_arr);
63 pseudo_inv_matrix, b_arr,
70 matrix_t anchor_pos_matrix[anchor_num][3],
78 matrix_t pseudo_inv_matrix[4][anchor_num];
81 if (anchor_num == 3) {
85 anchor_pos_matrix, dist_arr, Xp);
90 anchor_num, anchor_pos_matrix, Xh);
99 puts(
"rank!=3, the equation system is not solvable.");
104 else if (anchor_num > 3) {
109 anchor_pos_matrix, b_arr);
116 pseudo_inv_matrix, b_arr,
129 memset(particular_solu_arr, 0.0, 4 *
sizeof(
matrix_t));
133 particular_solu_arr[i] += pseudo_inv_matrix[i][j]
143 DEBUGF(
"particular_solution_arr[%d] = %f\n", i,
144 (
double)particular_solu_arr[i]);
150 matrix_t anchor_pos_matrix[anchor_num][3],
155 uint8_t m = anchor_num;
170 for (
int i = 0; i < n; i++) {
199 matrix_t a = 0, b = 0, c = 0, t1 = 0, t2 = 0, delta = 0;
211 c = pow(Xp[1], 2) + pow(Xp[2], 2)
212 + pow(Xp[3], 2) - Xp[0];
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);
220 if (solution_x2 != NULL) {
221 t2 = (-b - sqrt(delta)) / (2 * a);
224 else if ((delta == 0) && (a != 0)) {
225 if (solution_x1 != NULL) {
228 if (solution_x2 != NULL) {
233 else if (delta < 0) {
234 if (solution_x1 != NULL) {
237 if (solution_x2 != NULL) {
243 if (solution_x1 != NULL) {
244 solution_x1[i] = Xp[i]
247 if (solution_x2 != NULL) {
248 solution_x2[i] = Xp[i]
262 for (i = 0; i < m; i++) {
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];
276 matrix_t anchor_pos_matrix[anchor_num][3],
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];
292 matrix_t pseudo_inv_matrix[line_num][col_num],
297 memset(sol_vec, 0, col_num *
sizeof(
matrix_t));
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];