RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
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 
24 #include <stdio.h>
25 #include <math.h>
26 
27 #include "matrix.h"
28 #include "vector.h"
29 #include "levenberg_marquardt.h"
30 #include "modified_gauss_newton.h"
31 
39 void optimization_get_J(vector_t x0_vec[], matrix_t J[][3])
40 {
41  uint8_t i;
42  matrix_t denom;
43  uint8_t row_num = 4;
44 
45  matrix_t ref_value_matrix[4][3] = { { 0, 0, 1.67 },
46  { 4.5, 0, 0.75 },
47  { 4.5, 4.45, 0.75 },
48  { 0, 4.92, 0.86 } };
49 
50  for (i = 0; i < row_num; i++) {
51  denom = sqrt(pow(x0_vec[0] - ref_value_matrix[i][0], 2) +
52  pow(x0_vec[1] - ref_value_matrix[i][1], 2) +
53  pow(x0_vec[2] - ref_value_matrix[i][2], 2));
54  if (denom != 0) {
55  J[i][0] = (x0_vec[0] - ref_value_matrix[i][0]) / denom;
56  J[i][1] = (x0_vec[1] - ref_value_matrix[i][1]) / denom;
57  J[i][2] = (x0_vec[2] - ref_value_matrix[i][2]) / denom;
58  }
59  }
60 }
61 
70 void optimization_get_f_error(vector_t x0_vec[], vector_t measured_data_vec[],
71  vector_t f_vec[])
72 {
73  uint8_t i;
74  uint8_t row_num = 4;
75  matrix_t ref_point_mat[4][3] = { { 0, 0, 1.67 },
76  { 4.5, 0, 0.75 },
77  { 4.5, 4.45, 0.75 },
78  { 0, 4.92, 0.86 } };
79 
80  for (i = 0; i < row_num; i++) {
81  f_vec[i] =
82  sqrt(
83  (x0_vec[0] - ref_point_mat[i][0])
84  * (x0_vec[0]
85  - ref_point_mat[i][0])
86  +
87  (x0_vec[1]
88  - ref_point_mat[i][1])
89  * (x0_vec[1]
90  - ref_point_mat[i][1])
91  +
92  (x0_vec[2]
93  - ref_point_mat[i][2])
94  * (x0_vec[2]
95  - ref_point_mat[i][2])
96  )
97  - measured_data_vec[i];
98  }
99 }
100 
102 {
103  puts(
104  "***************** Optimization test *****************");
105 
106  matrix_t true_values_matrix[9][3] = {
107  { 1, 1, 0 }, // T1
108  { 1, 2, 0 }, // T2
109  { 1, 3, 0 }, // T3
110  { 2, 1, 0 }, // T4
111  { 2, 2, 0 }, // T5
112  { 2, 3, 0 }, // T6
113  { 3, 1, 0 }, // T7
114  { 3, 2, 0 }, // T8
115  { 3, 3, 0 }, // T9
116  };
117 
118  matrix_t x0_matrix[9][3] = {
119  { 0.9645166, 0.9894337, -0.1127879 }, // X0_1
120  { 0.9706874, 1.9937843, -0.0767517 }, // X0_2
121  { 0.9759567, 2.9980380, -0.0421651 }, // X0_3
122  { 1.9666907, 0.9892634, -0.1184331 }, // X0_4
123  { 1.9734193, 1.9945307, -0.0776960 }, // X0_5
124  { 1.9791861, 2.9997600, -0.0385668 }, // X0_6
125  { 2.9676093, 0.9886335, -0.1290797 }, // X0_7
126  { 2.9749588, 1.9950495, -0.0824135 }, // X0_8
127  { 2.9814874, 3.0015139, -0.0370967 }, // X0_9
128  };
129 
130  matrix_t measured_values_matrix[9][4] = {
131  { 2.189355547, 3.732117187, 4.992518309, 4.126940038 }, // M1
132  { 2.791860082, 4.115904867, 4.358726079, 3.195059925 }, // M2
133  { 3.577157155, 4.685985423, 3.883094303, 2.320377599 }, // M3
134  { 2.791860082, 2.810684972, 4.347183679, 4.474971454 }, // M4
135  { 3.285646100, 3.303836609, 3.600904464, 3.633252051 }, // M5
136  { 3.974524884, 3.992093430, 3.006899725, 2.893757310 }, // M6
137  { 3.577157155, 1.968162419, 3.857113556, 5.001588788 }, // M7
138  { 3.974524884, 2.625676627, 2.990106568, 4.264874121 }, // M8
139  { 4.560484620, 3.452531771, 2.238079928, 3.655150652 }, // M9
140  };
141 
142  uint8_t vector_values_num = 9;
143  uint8_t f_length = 4;
144  uint8_t i;
145 
146  //GN & LVM-parameters
147  matrix_t fmin = 1e-11;
148  matrix_t eps = 1e-3;
149  matrix_t beta0 = 0.2;
150  matrix_t beta1 = 0.8;
151  matrix_t tau = 1e-6;
152  uint8_t max_iter_num = 100;
153  uint8_t iter_num;
154  matrix_t est_values_vec[3];
155 
156  printf("\nseeked_values_matrix = ");
157  matrix_flex_print(vector_values_num, 3, true_values_matrix, 7, 3);
158  puts("");
159 
160  puts("############## PARAMETERS #################");
161  printf("eps = %.0e\ntau = %.0e\nfmin = %.0e\n", eps, tau, fmin);
162 
163  for (i = 0; i < vector_values_num; i++) {
164  /************************************** Gauss-Newton **************************************/
165 
166  printf(
167  "\n----------------------------------------- T%u ----------------------------------\n",
168  i + 1);
169  printf("true vector value = {%.4f, %.4f, %.4f}\n",
170  true_values_matrix[i][0],
171  true_values_matrix[i][1],
172  true_values_matrix[i][2]);
173  printf("start vector value = ");
174  vector_flex_print(3, x0_matrix[i], 5, 7);
175  puts("");
176 
177  iter_num = modified_gauss_newton(f_length, 3, x0_matrix[i],
178  measured_values_matrix[i], eps, fmin,
179  max_iter_num, est_values_vec,
181  printf("Gauss-Newton solution = ");
182  vector_flex_print(3, est_values_vec, 5, 7);
183  puts("");
184  printf("iteration number = %u\n", iter_num);
185 
186  vector_clear((uint8_t)3, est_values_vec);
187  iter_num = opt_levenberg_marquardt(f_length, 3, x0_matrix[i],
188  measured_values_matrix[i], eps, tau,
189  beta0, beta1, max_iter_num,
190  est_values_vec, &optimization_get_f_error,
192  printf("Levenberg-Marquardt solution = ");
193  vector_flex_print(3, est_values_vec, 5, 7);
194  puts("");
195  printf("iteration number = %u", iter_num);
196  }
197 }
198 
199 
215 void optimization_get_exp_f(vector_t x_vec[], vector_t data_vec[],
216  vector_t f_vec[])
217 {
218  vector_t x1, x2;
219  uint8_t i;
220 
221  x1 = x_vec[0];
222  x2 = x_vec[1];
223  for (i = 1; i < 9; i++) {
224  f_vec[i - 1] = x1 * exp(i * x2) - data_vec[i - 1];
225  }
226 }
227 
252 {
253  vector_t x1, x2;
254  uint8_t i;
255 
256  x1 = x_vec[0];
257  x2 = x_vec[1];
258  for (i = 1; i < 9; i++) {
259  J[i - 1][0] = exp(i * x2);
260  J[i - 1][1] = i * x1 * exp(i * x2);
261  }
262 }
263 
265 {
266  vector_t d_vec[] = { 8.3, 11.0, 14.7, 19.7, 26.7, 35.2, 44.4, 55.9 };
267  vector_t start_x_vec[] = { 6, .3 };
268  vector_t f_vec[8];
269  matrix_t J[8][2];
270 
271  // Parameters of GNM & LVM
272  matrix_t beta0 = 0.2;
273  matrix_t beta1 = 0.8;
274  matrix_t eps = 1e-3;
275  matrix_t tau = 1e-6;
276  matrix_t f_min = 1e-11;
277  uint8_t max_it_num = 3;
278  uint8_t iter_num = 0;
279  matrix_t est_x_vec[2];
280 
281  optimization_get_exp_f(start_x_vec, d_vec, f_vec);
282  optimization_get_exp_Jacobian(start_x_vec, J);
283 
284  puts(
285  "\n\n############### Test Gauss-Newton & LVM algorithms ###############");
286  puts("\nExponential Data:");
287  iter_num = modified_gauss_newton(8, 2, start_x_vec, d_vec, eps, f_min,
288  max_it_num, est_x_vec,
291  printf("start value = {%.6f, %.6f}\n", start_x_vec[0], start_x_vec[1]);
292  printf("Gauss-Newton solution = {%.6f, %.6f}\n", est_x_vec[0], est_x_vec[1]);
293  printf("iteration number = %d\n", iter_num);
294 
295  iter_num = 0;
296  vector_clear(2, est_x_vec);
297 
298  iter_num = opt_levenberg_marquardt(8, 2, start_x_vec, d_vec, eps, tau,
299  beta0, beta1, max_it_num,
300  est_x_vec, &optimization_get_exp_f,
302  printf("Levenberg-Marquardt solution = {%.6f, %.6f}\n", est_x_vec[0],
303  est_x_vec[1]);
304  printf("iteration number = %d\n", iter_num);
305 
306 }
307 
325 void optimization_get_sin_f(vector_t x_vec[], vector_t data_vec[],
326  vector_t f_vec[])
327 {
328  vector_t x1, x2, x3, x4;
329  uint8_t i;
330 
331  x1 = x_vec[0];
332  x2 = x_vec[1];
333  x3 = x_vec[2];
334  x4 = x_vec[3];
335  for (i = 1; i < 13; i++) {
336  f_vec[i - 1] = x1 * sin(x2 * i + x3) + x4 - data_vec[i - 1];
337  }
338 }
339 
361 {
362  vector_t x1, x2, x3;
363  uint8_t i;
364 
365  x1 = x_vec[0];
366  x2 = x_vec[1];
367  x3 = x_vec[2];
368  for (i = 1; i < 13; i++) {
369  J[i - 1][0] = sin(i * x2 + x3);
370  J[i - 1][1] = i * x1 * cos(i * x2 + x3);
371  J[i - 1][2] = x1 * cos(i * x2 + x3);
372  J[i - 1][3] = 1;
373  }
374 }
375 
377 {
378  vector_t d_vec[] = { 61, 65, 72, 78, 85, 90, 92, 92, 88, 81, 72, 63 };
379  vector_t start_x_vec[] = { 17, 0.5, 10.5, 77 };
380  vector_t f_vec[12];
381  matrix_t J[12][4];
382 
383  // Parameters of GNM & LVM
384  matrix_t beta0 = 0.2;
385  matrix_t beta1 = 0.8;
386  matrix_t eps = 1e-3;
387  matrix_t tau = 1e-6;
388  matrix_t f_min = 1e-11;
389  uint8_t max_it_num = 2;
390  uint8_t iter_num = 0;
391  matrix_t est_x_vec[4];
392 
393  optimization_get_sin_f(start_x_vec, d_vec, f_vec);
394  optimization_get_sin_Jacobian(start_x_vec, J);
395 
396 // puts("############### Test Gauss-Newton & LVM algorithms ###############");
397  puts("\nSinusoidal Data:");
398  iter_num = modified_gauss_newton(12, 4, start_x_vec, d_vec, eps, f_min,
399  max_it_num, est_x_vec,
402  printf("start value = {%.6f, %.6f, %.6f, %.6f}\n", start_x_vec[0],
403  start_x_vec[1],
404  start_x_vec[2], start_x_vec[3]);
405  printf("Gauss-Newton solution = {%.6f, %.6f, %.6f, %.6f}\n", est_x_vec[0],
406  est_x_vec[1],
407  est_x_vec[2], est_x_vec[3]);
408  printf("iteration number = %d\n", iter_num);
409 
410  iter_num = 0;
411  vector_clear(4, est_x_vec);
412 
413  iter_num = opt_levenberg_marquardt(12, 4, start_x_vec, d_vec, eps, tau,
414  beta0, beta1, max_it_num,
415  est_x_vec, &optimization_get_sin_f,
417  printf("Levenberg-Marquardt solution = {%.6f, %.6f, %.6f, %.6f}\n", est_x_vec[0],
418  est_x_vec[1],
419  est_x_vec[2], est_x_vec[3]);
420  printf("iteration number = %d\n", iter_num);
421 }
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
optimization_test
void optimization_test(void)
Examples of optimization algorithms using the LVM and GN algorithms.
Definition: optimization_test.c:101
modified_gauss_newton
uint8_t modified_gauss_newton(uint8_t f_length, uint8_t n, vector_t x0_vec[n], vector_t data_vec[f_length], matrix_t eps, matrix_t fmin, uint8_t max_iter_num, vector_t est_x_vec[n], void(*get_f_error)(vector_t x0_vec[], vector_t data_vec[], vector_t f_vec[]), void(*get_jacobian)(vector_t x0_vec[], matrix_t J[][n]))
Implements the modified Gauss–Newton algorithm.
Definition: modified_gauss_newton.c:32
vector_t
#define vector_t
Define the data type of the vector elements.
Definition: vector.h:33
optimization_get_exp_f
void optimization_get_exp_f(vector_t x_vec[], vector_t data_vec[], vector_t f_vec[])
Calculate the error vector using exponential data.
Definition: optimization_test.c:215
matrix.h
Matrix computations.
optimization_get_sin_Jacobian
void optimization_get_sin_Jacobian(vector_t x_vec[], matrix_t J[][4])
Calculate the Jacobian matrix using sinusoidal data.
Definition: optimization_test.c:360
matrix_flex_print
void matrix_flex_print(uint8_t m, uint8_t n, matrix_t matrix[m][n], uint8_t before_dec, uint8_t after_dec)
Display the values of the matrix elements.
Definition: matrix.c:220
levenberg_marquardt.h
Implement the Levenberg–Marquardt (LVM) algorithm.
vector_clear
void vector_clear(uint8_t size, vector_t arr[])
Clear all the elements of the vector.
Definition: vector.c:32
optimization_get_sin_f
void optimization_get_sin_f(vector_t x_vec[], vector_t data_vec[], vector_t f_vec[])
Calculate the error vector using sinusoidal data.
Definition: optimization_test.c:325
optimization_get_exp_Jacobian
void optimization_get_exp_Jacobian(vector_t x_vec[], matrix_t J[][2])
Calculate the Jacobian matrix using exponential data.
Definition: optimization_test.c:251
optimization_get_f_error
void optimization_get_f_error(vector_t x0_vec[], vector_t measured_data_vec[], vector_t f_vec[])
Calculate the error vector of the approximation.
Definition: optimization_test.c:70
optimization_get_J
void optimization_get_J(vector_t x0_vec[], matrix_t J[][3])
Calculate the Jacobian matrix of the function optimization_get_f_error.
Definition: optimization_test.c:39
opt_levenberg_marquardt
uint8_t opt_levenberg_marquardt(uint8_t f_length, uint8_t n, vector_t x0_vec[n], vector_t data_vec[f_length], matrix_t eps, matrix_t tau, matrix_t beta0, matrix_t beta1, uint8_t max_iter_num, vector_t est_x_vec[n], void(*get_f_error)(vector_t x0_vec[], vector_t data_vec[], vector_t f_vec[]), void(*get_jacobian)(vector_t x0_vec[], matrix_t J[][n]))
Implements the Levenberg–Marquardt (LVM) algorithm.
Definition: levenberg_marquardt.c:143
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
modified_gauss_newton.h
Implement the Gauss–Newton algorithm.
vector.h
Vector computations.
optimization_exponential_data_test
void optimization_exponential_data_test(void)
Examples of optimization algorithms using exponential data.
Definition: optimization_test.c:264
optimization_sinusoidal_data_test
void optimization_sinusoidal_data_test(void)
Examples of optimization algorithms using sinusoidal data.
Definition: optimization_test.c:376