35                                             matrix_t ref_points_matrix[ref_points_num][3],
 
   39                                             void (*f_i)(uint8_t ref_point_num,
 
   40                                                         matrix_t ref_point_mat[ref_points_num][3],
 
   43                                             void (*jacobian_get_JTJ)(
 
   44                                                 uint8_t ref_points_num,
 
   45                                                 matrix_t ref_point_matrix[ref_points_num][3],
 
   49                                             void (*jacobian_get_JTf)(
 
   50                                                 uint8_t ref_points_num,
 
   51                                                 matrix_t ref_point_matrix[ref_points_num][3],
 
   55                                             void (*jacobian_get_J_mul_s)(
 
   56                                                 uint8_t ref_points_num,
 
   57                                                 matrix_t ref_point_matrix[ref_points_num][3],
 
   70     matrix_t Fx_plus_J_mul_s[ref_points_num];
 
   78                                           measured_data_vec, mu, JTJ_mu2_I,
 
   82     jacobian_get_JTf(ref_points_num, ref_points_matrix, point,
 
   83                      measured_data_vec, JTF);
 
   89     f_i(ref_points_num, ref_points_matrix, point, measured_data_vec, Fx);
 
   97     f_i(ref_points_num, ref_points_matrix, point_plus_s, measured_data_vec,
 
  104     jacobian_get_J_mul_s(ref_points_num, ref_points_matrix, point, s,
 
  107     vector_add(ref_points_num, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);
 
  114     denom = Fx_square - Fx_plus_J_mul_s_square;
 
  116         ro_mu = (Fx_square - Fx_plus_s_square) / denom;
 
  119         puts(
"ro_mu is infinite !!!");
 
  127                                 matrix_t ref_points_matrix[ref_points_num][3],
 
  129                                 matrix_t measured_data_vec[ref_points_num],
 
  132                                 uint8_t max_iter_num,
 
  134                                 void (*f_i)(uint8_t ref_points_num,
 
  135                                             matrix_t ref_point_mat[ref_points_num][3],
 
  138                                 void (*jacobian_get_JTJ)(uint8_t ref_points_num,
 
  143                                 void (*jacobian_get_JTf)(uint8_t ref_points_num,
 
  148                                 void (*jacobian_get_J_mul_s)(uint8_t ref_points_num,
 
  166     jacobian_get_JTJ(ref_points_num, ref_points_matrix, start_pos,
 
  167                      measured_data_vec, JTJ_mu2_I);
 
  172                                           start_pos, measured_data_vec, mu,
 
  179     jacobian_get_JTf(ref_points_num, ref_points_matrix, start_pos,
 
  180                      measured_data_vec, JTF);
 
  189            && (it < max_iter_num)) {   
 
  192                                                    ref_points_matrix, est_pos,
 
  194                                                    mu, s, f_i, jacobian_get_JTJ,
 
  196                                                    jacobian_get_J_mul_s);
 
  199             if (ro_mu <= beta0) {
 
  203                     ref_points_matrix, est_pos,
 
  204                     measured_data_vec, mu, s, f_i,
 
  207                     jacobian_get_J_mul_s);
 
  209             else if (ro_mu >= beta1) {
 
  227     for (uint8_t i = 1; i < 3; i++) {
 
  228         if (JTJ[i][i] > max_diag_JTJ) {
 
  229             max_diag_JTJ = JTJ[i][i];
 
  232     return tau * max_diag_JTJ;
 
  237                                            matrix_t ref_points_matrix[ref_points_num][3],
 
  239                                            matrix_t measured_data_vec[ref_points_num],
 
  242                                            void (*jacobian_get_JTJ)(
 
  243                                                uint8_t ref_points_num,
 
  244                                                matrix_t ref_point_matrix[ref_points_num][3],
 
  252     jacobian_get_JTJ(ref_points_num, ref_points_matrix, point,
 
  253                      measured_data_vec, JTJ_mu2_I); 
 
  255     for (i = 0; i < 3; i++) {
 
  256         JTJ_mu2_I[i][i] += pow(mu, 2);