RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
modified_gauss_newton.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 
23 #include <stdio.h>
24 #include <math.h>
25 
26 #include "utils.h"
27 #include "matrix.h"
28 #include "vector.h"
30 
31 //n is the size of x0 that is equal to the column number of J
32 uint8_t modified_gauss_newton(uint8_t f_length, uint8_t n,
33  vector_t x0_vec[n],
34  vector_t data_vec[f_length],
35  matrix_t eps, matrix_t fmin, uint8_t max_iter_num,
36  vector_t est_x_vec[n],
37  void (*get_f_error)(vector_t x0_vec[],
38  vector_t data_vec[],
39  vector_t f_vec[]),
40  void (*get_jacobian)(vector_t x0_vec[],
41  matrix_t J[][n])
42  )
43 
44 {
45  matrix_t J[f_length][n];
46  matrix_t JT_J[n][n];
47  matrix_t JT_f[n];
48  vector_t f_vec[f_length];
49  matrix_t f_error;
50  matrix_t pinv_JTJ_mat[n][n];
51  vector_t correction_vec[n];
52  vector_t x_vec[n];
53  vector_t next_x_vec[n];
54  matrix_t max_error, min_error;
55  matrix_t step;
56  uint8_t iter_num;
57 
58  get_f_error(x0_vec, data_vec, f_vec);
59  f_error = vector_get_norm2(f_length, f_vec);
60  max_error = f_error;
61  min_error = max_error;
62  step = eps;
63 
64  vector_copy(n, x0_vec, x_vec);
65  vector_copy(n, x0_vec, est_x_vec);
66  iter_num = 0;
67 
68  while ((step >= eps) && (iter_num < max_iter_num) && (f_error > fmin)) {
69  /*
70  * Compute then correction terms & next x_vec values
71  */
72  //JT_J = J'*J
73  get_jacobian(x_vec, J);
74  matrix_trans_mul_itself(f_length, n, J, JT_J);
75 
76  //JT_f = J'*f
77  get_f_error(x_vec, data_vec, f_vec);
78  matrix_trans_mul_vec(f_length, n, J, f_length, f_vec, JT_f);
79 
80  //solve: J'J*s = -J'*f
81  moore_penrose_get_pinv(n, n, JT_J, pinv_JTJ_mat);
82  //s = (J'J)\J'*f
83  matrix_mul_vec(n, n, pinv_JTJ_mat, JT_f, correction_vec);
84 
85  // x = x - s
86  vector_sub(n, x_vec, correction_vec, next_x_vec);
87 
88  //next step
89  step = vector_get_euclidean_distance(n, x_vec, next_x_vec);
90 
91  // x_vec = next_x_vec
92  vector_copy(n, next_x_vec, x_vec);
93 
94  //error vector
95  get_f_error(x_vec, data_vec, f_vec);
96 
97  f_error = vector_get_norm2(f_length, f_vec);
98 
99  if (min_error > f_error) { //store the x_vec value with the minimum error in est_x_vec
100  vector_copy(n, x_vec, est_x_vec);
101  min_error = f_error; // update min_error
102  }
103 
104  max_error = utils_max(f_error, max_error); // update max_error
105  iter_num++;
106  if ((max_error - min_error) > 10) {
107  break;
108  }
109 
110  } //while
111 
112  return iter_num;
113 }
moore_penrose_get_pinv
int8_t moore_penrose_get_pinv(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t pinv_A[n][m])
Calculate the Moore–Penrose inverse of a rectangular matrix.
Definition: moore_penrose_pseudo_inverse.c:38
vector_get_norm2
vector_t vector_get_norm2(uint8_t length, vector_t arr[])
Compute the 2-norm norm of a vector.
Definition: vector.c:42
matrix_mul_vec
void matrix_mul_vec(uint8_t m, uint8_t n, matrix_t matrix[m][n], matrix_t vec[n], matrix_t dst_arr[m])
Compute the multiplication of a matrix with a column vector.
Definition: matrix.c:434
vector_get_euclidean_distance
vector_t vector_get_euclidean_distance(uint8_t length, vector_t vec1[], vector_t vec2[])
Compute the Euclidean distance between two vectors.
Definition: vector.c:163
vector_t
#define vector_t
Define the data type of the vector elements.
Definition: vector.h:33
matrix_trans_mul_itself
void matrix_trans_mul_itself(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t AT_mul_A[n][n])
Compute the multiplication of the transpose of a matrix with itself.
Definition: matrix.c:545
matrix_trans_mul_vec
void matrix_trans_mul_vec(uint8_t m, uint8_t n, matrix_t A[m][n], uint8_t b_size, matrix_t b_vec[m], matrix_t c_vec[n])
Compute the multiplication of transposed matrix with column vector.
Definition: matrix.c:511
matrix.h
Matrix computations.
moore_penrose_pseudo_inverse.h
Moore–Penrose algorithm to compute the pseudo-inverse of a matrix.
utils.h
Utilities for linear algebra.
utils_max
double utils_max(double a, double b)
Returns the greater of two real numbers.
Definition: utils.c:55
vector_copy
void vector_copy(uint8_t size, vector_t src_arr[], vector_t dest_arr[])
Copy the elements of the source vector to the destination vector.
Definition: vector.c:37
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
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector.h
Vector computations.
vector_sub
void vector_sub(uint8_t size, vector_t a_vec[], vector_t b_vec[], vector_t a_minus_b[])
Compute the subtraction of two vectors.
Definition: vector.c:94