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

Implement the Gauss–Newton algorithm. More...

#include <stdio.h>
#include "utils.h"
#include "matrix.h"
#include "vector.h"
#include "moore_penrose_pseudo_inverse.h"

Go to the source code of this file.

Functions

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. More...
 

Detailed Description

Implement the Gauss–Newton algorithm.

Author
Zakaria Kasmi zkasm.nosp@m.i@in.nosp@m.f.fu-.nosp@m.berl.nosp@m.in.de
Abdelmoumen Norrdine a.nor.nosp@m.rdin.nosp@m.e@goo.nosp@m.glem.nosp@m.ail.c.nosp@m.om

Definition in file loc_gauss_newton.c.

Function Documentation

◆ 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(*)(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 
)

Implements the modified Gauss–Newton 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]fmintermination tolerance on the error function.
[in]max_iter_nummaximal iteration number of the Gauss-Newton algorithm.
[in,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 $ J_f^{T} J_{f} $ matrix.
[in](*jacobian_get_JTf)pointer to the function that calculates the $ J_f^{T} \vec{f} $ vector.
Returns
required iteration number.

Definition at line 30 of file loc_gauss_newton.c.

References matrix_mul_vec(), matrix_t, moore_penrose_get_pinv(), utils_max(), vector_copy(), vector_get_euclidean_distance(), vector_get_norm2(), and vector_sub().

Referenced by position_optimization_test().