RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
position_optimization_test.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 
27 #include <stdio.h>
28 #include <stdint.h>
29 
31 #include "loc_gauss_newton.h"
32 #include "dist_based_fi.h"
33 #include "dist_based_position.h"
34 #include "dist_based_jacobian.h"
35 #include "vector.h"
36 
38 {
39  matrix_t ref_pos_matrix[4][3] = { { 0, 0, 1.67 }, // P1
40  { 4.5, 0, 0.75 }, // P2
41  { 4.5, 4.45, 0.75 }, // P3
42  { 0, 4.92, 0.86 } // P4
43  };
44 
45  uint8_t ref_num = 4;
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 };
49  double est_pos[3];
50 
51  uint8_t iter_num;
52  matrix_t fmin = 1e-11;
53  matrix_t eps = 1e-3;
54  matrix_t beta0 = 0.2;
55  matrix_t beta1 = 0.8;
56  matrix_t tau = 1e-6;
57  uint8_t max_iter_num = 100;
58 
59  iter_num = loc_gauss_newton(ref_num, ref_pos_matrix, start_pos,
60  measured_dist_arr, eps, fmin, max_iter_num, est_pos,
63  printf("True position = ");
64  vector_flex_print(3, true_pos, 3, 4);
65  puts("");
66  printf("Optimized position with Gauss-Newton algorithm = ");
67  vector_flex_print(3, est_pos, 5, 7);
68  puts("");
69  printf("iteration number = %u\n", iter_num);
70 
71  vector_clear((uint8_t) 3, est_pos);
72 
73  iter_num = loc_levenberg_marquardt(ref_num, ref_pos_matrix, start_pos,
74  measured_dist_arr, eps, tau, beta0, beta1, max_iter_num,
75  est_pos,
79  printf("Optimized position with Levenberg-Marquardt algorithm = ");
80  vector_flex_print(3, est_pos, 5, 7);
81  puts("");
82  printf("iteration number = %u\n", iter_num);
83 
84 }
vector_flex_print
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.
Definition: vector.c:284
loc_levenberg_marquardt
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.
Definition: loc_levenberg_marquardt.c:126
dist_based_jacobian.h
Jacobian function of distance-based localization systems.
loc_levenberg_marquardt.h
Implement the Levenberg–Marquardt (LVM) algorithm for position optimization.
dist_based_position.h
Functions of distance-based localization systems.
vector_t
#define vector_t
Define the data type of the vector elements.
Definition: vector.h:33
dist_based_jacobian_get_J_mul_s
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.
Definition: dist_based_jacobian.c:132
dist_based_f_i
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.
Definition: dist_based_fi.c:47
dist_based_jacobian_get_JTJ
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.
Definition: dist_based_jacobian.c:56
vector_clear
void vector_clear(uint8_t size, vector_t arr[])
Clear all the elements of the vector.
Definition: vector.c:32
position_optimization_test
void position_optimization_test(void)
Examples of optimization algorithms of a localization system.
Definition: position_optimization_test.c:37
loc_gauss_newton.h
Implement the Gauss–Newton algorithm for position optimization.
loc_gauss_newton
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.
Definition: loc_gauss_newton.c:30
dist_based_fi.h
Error function of distance-based localization systems.
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector.h
Vector computations.
dist_based_jacobian_get_JTf
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.
Definition: dist_based_jacobian.c:28