RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
loc_levenberg_marquardt.c File Reference

Implement the Levenberg–Marquardt (LVM) algorithm. More...

#include <stdio.h>
#include <math.h>
#include <float.h>
#include <inttypes.h>
#include "matrix.h"
#include "vector.h"
#include "solve.h"
#include "utils.h"
#include "loc_levenberg_marquardt.h"

Go to the source code of this file.

Functions

matrix_t loc_levenberg_marquardt_correction (uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t point[3], matrix_t measured_data_vec[ref_points_num], matrix_t mu, matrix_t s[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]), void(*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]))
 Implements the correction-function of the Levenberg–Marquardt (LVM) algorithm. More...
 
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. More...
 
matrix_t loc_levenberg_marquardt_get_mu0 (matrix_t tau, matrix_t JTJ[3][3])
 Compute the initial value $ \mu_0 $ of the Levenberg–Marquardt (LVM) algorithm. More...
 
void loc_levenberg_marquardt_get_JTJ_mu2_I (uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t point[3], matrix_t measured_data_vec[ref_points_num], matrix_t mu, matrix_t JTJ_mu2_I[3][3], 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]))
 Compute the matrix $ J_f^{T} J_f + \mu^{(i)})^{2} I $. More...
 

Detailed Description

Implement the Levenberg–Marquardt (LVM) algorithm.

Author
Zakaria Kasmi zkasm.nosp@m.i@in.nosp@m.f.fu-.nosp@m.berl.nosp@m.in.de
Naouar Guerchali

Definition in file loc_levenberg_marquardt.c.

Function Documentation

◆ 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(*)(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[])  f_i,
void(*)(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])  jacobian_get_JTJ,
void(*)(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])  jacobian_get_JTf,
void(*)(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])  jacobian_get_J_mul_s 
)

Implements the Levenberg–Marquardt (LVM) algorithm.

The user should provide pointers to the error and Jacobian functions.

Note
This function is optimized for localization algorithms.
Parameters
[in]ref_points_numnumber of the reference stations.
[in]ref_points_matrix[][]three-dimensional coordinates of the reference stations.
[in]start_pos[]start (approximate) position.
[in]measured_data_vec[]pointer to the measured data.
[in]epsaccuracy bound.
[in]tau$ \tau $ factor.
[in]beta0$ \beta_0 $ factor.
[in]beta1$ \beta_1 $ factor.
[in]max_iter_nummaximal iteration number of the LVM algorithm.
[out]est_pos[]estimated (optimized) position.
[in](*f_i)pointer to the error function.
[in](*jacobian_get_JTJ)pointer to the function that calculates the matrix $ J_f^{T} J_{f} $.
[in](*jacobian_get_JTf)pointer to the function that calculates the vector $ J_f^{T} \vec{f} $.
[in](*jacobian_get_J_mul_s)pointer to the function that calculates the vector $ J_f^{T} \vec{s} $.
Returns
required iteration number.

Definition at line 126 of file loc_levenberg_marquardt.c.

References loc_levenberg_marquardt_correction(), loc_levenberg_marquardt_get_JTJ_mu2_I(), loc_levenberg_marquardt_get_mu0(), matrix_mul_scalar(), matrix_t, solve_householder(), vector_add(), vector_copy(), and vector_get_norm2().

Referenced by multipath_algo_own_norm_distr_test(), and position_optimization_test().

◆ loc_levenberg_marquardt_correction()

matrix_t loc_levenberg_marquardt_correction ( uint8_t  ref_points_num,
matrix_t  ref_points_matrix[ref_points_num][3],
matrix_t  point[3],
matrix_t  measured_data_vec[ref_points_num],
matrix_t  mu,
matrix_t  s[3],
void(*)(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[])  f_i,
void(*)(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])  jacobian_get_JTJ,
void(*)(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])  jacobian_get_JTf,
void(*)(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])  jacobian_get_J_mul_s 
)

Implements the correction-function of the Levenberg–Marquardt (LVM) algorithm.

The user should provide pointers to the error and Jacobian functions.

Note
This function is optimized for localization algorithms.
Parameters
[in]ref_points_numnumber of the reference stations.
[in]ref_points_matrix[][]three-dimensional coordinates of the reference stations.
[in]point[]a position to adjust.
[in]measured_data_vec[]pointer to the measured data.
[in]muregularization parameter $ \mu $.
[out]s[]correction vector.
[in](*f_i)pointer to the error function.
[in](*jacobian_get_JTJ)pointer to the function that calculates the matrix $ J_f^{T} J_{f} $.
[in](*jacobian_get_JTf)pointer to the function that calculates the vector $ J_f^{T} \vec{f} $.
[in](*jacobian_get_J_mul_s)pointer to the function that calculates the vector $ J_f^{T} \vec{s} $
Returns
the parameter $ \rho_{\mu} $

Definition at line 34 of file loc_levenberg_marquardt.c.

References loc_levenberg_marquardt_get_JTJ_mu2_I(), matrix_mul_scalar(), matrix_t, solve_householder(), vector_add(), and vector_get_scalar_product().

Referenced by loc_levenberg_marquardt().

◆ loc_levenberg_marquardt_get_JTJ_mu2_I()

void loc_levenberg_marquardt_get_JTJ_mu2_I ( uint8_t  ref_points_num,
matrix_t  ref_points_matrix[ref_points_num][3],
matrix_t  point[3],
matrix_t  measured_data_vec[ref_points_num],
matrix_t  mu,
matrix_t  JTJ_mu2_I[3][3],
void(*)(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])  jacobian_get_JTJ 
)

Compute the matrix $ J_f^{T} J_f + \mu^{(i)})^{2} I $.

Note
This function is optimized for localization algorithms.
Parameters
[in]ref_points_numnumber of the reference stations.
[in]ref_points_matrix[][]three-dimensional coordinates of the reference stations.
[in]point[]a position to adjust.
[in]measured_data_vec[]pointer to the measured data.
[in]mu$ \rho_{\mu} $-parameter
[out]JTJ_mu2_I[][]includes the matrix $ J_f^{T} J_f + \mu^{(i)})^{2} I $.
[in](*jacobian_get_JTJ)pointer to the function that calculates the matrix $ J_f^{T} J_{f} $.

Definition at line 236 of file loc_levenberg_marquardt.c.

Referenced by loc_levenberg_marquardt(), and loc_levenberg_marquardt_correction().

◆ loc_levenberg_marquardt_get_mu0()

matrix_t loc_levenberg_marquardt_get_mu0 ( matrix_t  tau,
matrix_t  JTJ[3][3] 
)

Compute the initial value $ \mu_0 $ of the Levenberg–Marquardt (LVM) algorithm.

The user should provide a pointer to the matrix $ J_f^{T} J_{f} $.

Parameters
[in]tau$ \tau $ factor.
[in]JTJ[][]pointer to the matrix $ J_f^T J_f $.
Returns
$ \rho_{\mu} $-parameter

Definition at line 223 of file loc_levenberg_marquardt.c.

References matrix_t.

Referenced by loc_levenberg_marquardt().