RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
levenberg_marquardt.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 #include <float.h>
26 #include <inttypes.h>
27 
28 #include "levenberg_marquardt.h"
29 #include "matrix.h"
30 #include "vector.h"
31 #include "solve.h"
32 #include "utils.h"
33 
53 matrix_t opt_levenberg_marquardt_correction(uint8_t f_length, uint8_t n,
54  matrix_t x_vec[n],
55  matrix_t data_vec[f_length],
56  matrix_t mu,
57  matrix_t s[n],
58  void (*get_f_error)(
59  vector_t x_vec[],
60  vector_t data_vec[],
61  vector_t f_vec[]),
62  void (*get_jacobian)(
63  vector_t x_vec[],
64  matrix_t J[][n])
65  )
66 {
67 
68  matrix_t Fx[f_length];
69  matrix_t J[f_length][n];
70  vector_t f_vec[f_length];
71  matrix_t JTJ_mu2_I[n][n];
72  vector_t JTF[n];
73  matrix_t ro_mu = 0;
74  matrix_t Fx_square = 0;
75  matrix_t Fx_plus_s_square = 0;
76  matrix_t Fx_plus_J_mul_s_square = 0;
77  vector_t Fx_plus_J_mul_s[f_length];
78  vector_t F_x_plus_s[f_length];
79  vector_t x_plus_s[n];
80  matrix_t denom;
81 
82  /*
83  * compute: -(J'J + mu^2*I)
84  */
85  //JT_J = J'*J
86  get_jacobian(x_vec, J);
87 
88  //store J'J in JTJ_mu2_I
89  matrix_trans_mul_itself(f_length, n, J, JTJ_mu2_I);
90 
91  //compute: J'J + mu^2*I
92  matrix_add_to_diag(n, JTJ_mu2_I, n, pow(mu, 2));
93 
94  // -(J'*J+mu^2*eye(n)), is an (nxn) matrix
95  matrix_mul_scalar(n, n, JTJ_mu2_I, -1, JTJ_mu2_I);
96 
97  //JT_f = J'*f
98  get_f_error(x_vec, data_vec, f_vec);
99  matrix_trans_mul_vec(f_length, n, J, f_length, f_vec, JTF);
100 
101  //solve the equation: s = -(J'*J+mu^2*I)\(J'*Fx);
102  solve_householder(n, n, JTJ_mu2_I, JTF, s);
103 
104  //f(x)
105  get_f_error(x_vec, data_vec, Fx);
106 
107  //f(x)^2
108  Fx_square = vector_get_scalar_product(f_length, Fx, Fx);
109 
110  //(x+s)
111  vector_add(n, x_vec, s, x_plus_s);
112 
113  //f(x+s)
114  get_f_error(x_plus_s, data_vec, F_x_plus_s);
115 
116  //f(x+s)^2
117  Fx_plus_s_square = vector_get_scalar_product(f_length, F_x_plus_s,
118  F_x_plus_s);
119 
120  //compute: J(x)*s
121  matrix_mul_vec(f_length, n, J, s, Fx_plus_J_mul_s);
122 
123  //compute: f(x) + J(x)*s
124  vector_add(f_length, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);
125 
126  //compute: ||f(x) + J(x)*s||^2
127  Fx_plus_J_mul_s_square = vector_get_scalar_product(f_length,
128  Fx_plus_J_mul_s,
129  Fx_plus_J_mul_s);
130 
131  denom = Fx_square - Fx_plus_J_mul_s_square;
132  if (denom != 0) {
133  ro_mu = (Fx_square - Fx_plus_s_square) / denom;
134  }
135  else {
136  puts("ro_mu is infinite !!!");
137  ro_mu = FLT_MAX;
138  }
139 
140  return ro_mu;
141 }
142 
143 uint8_t opt_levenberg_marquardt(uint8_t f_length, uint8_t n,
144  vector_t x0_vec[n],
145  vector_t data_vec[f_length],
146  matrix_t eps, matrix_t tau, matrix_t beta0,
147  matrix_t beta1,
148  uint8_t max_iter_num,
149  vector_t est_x_vec[n],
150  void (*get_f_error)(vector_t x0_vec[],
151  vector_t data_vec[],
152  vector_t f_vec[]),
153  void (*get_jacobian)(vector_t x0_vec[],
154  matrix_t J[][n])
155  )
156 {
157  matrix_t J[f_length][n];
158  vector_t JT_f[n];
159  matrix_t JTJ_mu2_I[n][n];
160  vector_t s[n];
161  vector_t f_vec[f_length];
162  matrix_t mu;
163  matrix_t ro_mu;
164  uint8_t it;
165 
166  /*
167  * compute mu0 and -(J'J + mu0^2*I)
168  */
169  //JT_J = J'*J
170  get_jacobian(x0_vec, J);
171 
172  //store J'J in JTJ_mu2_I
173  matrix_trans_mul_itself(f_length, n, J, JTJ_mu2_I);
174 
175  // compute mu_0: mu = mu_0
176  mu = opt_levenberg_marquardt_get_mu0(n, tau, JTJ_mu2_I);
177 
178  //compute: J'J + mu0^2*I
179  matrix_add_to_diag(n, JTJ_mu2_I, n, pow(mu, 2));
180 
181  //-(J'*J+mu0^2*eye(n)) is a (n,n) matrix
182  matrix_mul_scalar(n, n, JTJ_mu2_I, -1, JTJ_mu2_I);
183 
184  //compute JTF: JT_f = J'*f
185  get_f_error(x0_vec, data_vec, f_vec);
186  matrix_trans_mul_vec(f_length, n, J, f_length, f_vec, JT_f);
187 
188  //solve the equation: s=-(J'*J+mu^2*eye(n))\(J'*F);
189  solve_householder(n, n, JTJ_mu2_I, JT_f, s);
190 
191  vector_copy(n, x0_vec, est_x_vec);
192 
193  it = 0;
194  while ((vector_get_norm2(n, s)
195  > eps * (1 + vector_get_norm2(n, est_x_vec)))
196  && (it < max_iter_num)) { //norm(s,2)>eps*(1+norm(x,2))
197 
198  ro_mu = opt_levenberg_marquardt_correction(f_length, n,
199  est_x_vec, data_vec,
200  mu, s, get_f_error,
201  get_jacobian);
202 
203  while (1) {
204  if (ro_mu <= beta0) {
205  mu = 2.0 * mu;
207  f_length, n,
208  est_x_vec,
209  data_vec, mu, s, get_f_error,
210  get_jacobian);
211  }
212  else if (ro_mu >= beta1) {
213  mu = mu / 2.0;
214  break;
215  }
216  else {
217  break;
218  }
219  }
220  vector_add(n, est_x_vec, s, est_x_vec);
221  it = it + 1;
222  } //while
223 
224  return it;
225 }
226 
228  matrix_t JTJ[][n])
229 {
230  matrix_t max_diag_JTJ = JTJ[0][0];
231 
232  for (uint8_t i = 1; i < n; i++) {
233  if (JTJ[i][i] > max_diag_JTJ) {
234  max_diag_JTJ = JTJ[i][i];
235  }
236  }
237 
238  return tau * max_diag_JTJ;
239 }
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
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
vector_get_scalar_product
vector_t vector_get_scalar_product(uint8_t n, vector_t vec1[n], vector_t vec2[n])
Compute the dot product of two vectors.
Definition: vector.c:190
solve_householder
int8_t solve_householder(uint8_t m, uint8_t n, matrix_t A[][n], matrix_t b[m], matrix_t x_sol[n])
Solve an (m n) linear system Ax = b, using the Householder algorithm.
Definition: solve.c:77
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_add_to_diag
void matrix_add_to_diag(uint8_t n, matrix_t A[][n], uint8_t diag_el_num, matrix_t value)
Add a number to diagonal elements of a matrix.
Definition: matrix.c:353
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
solve.h
Enables to solve systems of linear equations Ax = b for x.
matrix.h
Matrix computations.
utils.h
Utilities for linear algebra.
opt_levenberg_marquardt_correction
matrix_t opt_levenberg_marquardt_correction(uint8_t f_length, uint8_t n, matrix_t x_vec[n], matrix_t data_vec[f_length], matrix_t mu, matrix_t s[n], void(*get_f_error)(vector_t x_vec[], vector_t data_vec[], vector_t f_vec[]), void(*get_jacobian)(vector_t x_vec[], matrix_t J[][n]))
Implements the correction-function of the Levenberg–Marquardt (LVM) algorithm.
Definition: levenberg_marquardt.c:53
matrix_mul_scalar
void matrix_mul_scalar(uint8_t m, uint8_t n, matrix_t mat_src[m][n], matrix_t value, matrix_t mat_dest[m][n])
Multiply all elements of a matrix with a specified value.
Definition: matrix.c:602
levenberg_marquardt.h
Implement the Levenberg–Marquardt (LVM) algorithm.
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
opt_levenberg_marquardt_get_mu0
matrix_t opt_levenberg_marquardt_get_mu0(uint8_t n, matrix_t tau, matrix_t JTJ[][n])
Compute the initial value of the Levenberg–Marquardt (LVM) algorithm.
Definition: levenberg_marquardt.c:227
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector_add
void vector_add(uint8_t size, vector_t a_vec[size], vector_t b_vec[size], vector_t a_plus_b_vec[size])
Compute the addition of two vectors.
Definition: vector.c:104
vector.h
Vector computations.