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];