|
RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
|
Go to the documentation of this file.
39 matrix_t ref_pos_matrix[4][3] = { { 0, 0, 1.67 },
46 double true_pos[] = { 1.0000, 1.0000, 0.0000 };
47 double measured_dist_arr[] = { 2.1893555, 3.7321172, 4.9925183, 4.1269400 };
48 vector_t start_pos[] = { 0.9645166, 0.9894337, -0.1127879 };
57 uint8_t max_iter_num = 100;
60 measured_dist_arr, eps, fmin, max_iter_num, est_pos,
63 printf(
"True position = ");
66 printf(
"Optimized position with Gauss-Newton algorithm = ");
69 printf(
"iteration number = %u\n", iter_num);
74 measured_dist_arr, eps, tau, beta0, beta1, max_iter_num,
79 printf(
"Optimized position with Levenberg-Marquardt algorithm = ");
82 printf(
"iteration number = %u\n", iter_num);
void vector_flex_print(uint32_t length, vector_t arr[], uint8_t before_dot, uint8_t after_dot)
Display the values of the vector's elements.
uint8_t loc_levenberg_marquardt(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t start_pos[3], matrix_t measured_data_vec[ref_points_num], matrix_t eps, matrix_t tau, matrix_t beta0, matrix_t beta1, uint8_t max_iter_num, matrix_t est_pos[3], void(*f_i)(uint8_t ref_points_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[]), void(*jacobian_get_JTJ)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTJ[3][3]), void(*jacobian_get_JTf)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTf[3]), void(*jacobian_get_J_mul_s)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t s[3], matrix_t J_s[ref_points_num]))
Implements the Levenberg–Marquardt (LVM) algorithm.
Jacobian function of distance-based localization systems.
Implement the Levenberg–Marquardt (LVM) algorithm for position optimization.
Functions of distance-based localization systems.
#define vector_t
Define the data type of the vector elements.
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.
void dist_based_f_i(uint8_t ref_points_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[])
Defines the error function of a distance-based localization system.
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.
void vector_clear(uint8_t size, vector_t arr[])
Clear all the elements of the vector.
void position_optimization_test(void)
Examples of optimization algorithms of a localization system.
Implement the Gauss–Newton algorithm for position optimization.
uint8_t loc_gauss_newton(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], vector_t start_pos[3], matrix_t measured_data_vec[ref_points_num], matrix_t eps, matrix_t fmin, uint8_t max_iter_num, vector_t est_pos[3], void(*f_i)(uint8_t ref_point_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[]), void(*jacobian_get_JTJ)(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTJ[3][3]), void(*jacobian_get_JTf)(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTf[3]))
Implements the modified Gauss–Newton algorithm.
Error function of distance-based localization systems.
#define matrix_t
Define the data type of the matrix elements.
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.