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

Implement the trilateration algorithm. More...

#include <math.h>
#include <stdio.h>
#include <string.h>
#include "moore_penrose_pseudo_inverse.h"
#include "matrix.h"
#include "svd.h"
#include "trilateration.h"

Go to the source code of this file.

Functions

void trilateration1 (uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t pseudo_inv_matrix[4][anchor_num], matrix_t homog_sol_arr[], matrix_t dist_arr[], matrix_t solution_x1[], matrix_t solution_x2[])
 Implement the trilateration algorithm using the pre-processed pseudo-inverse matrix. More...
 
void trilateration2 (uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t dist_arr[], matrix_t solution_x1[], matrix_t solution_x2[])
 Implement the trilateration algorithm. More...
 
void trilateration_preprocessed_get_particular_solution (matrix_t pseudo_inv_matrix[4][3], matrix_t b_arr[], matrix_t particular_solu_arr[])
 Compute the particular solution, which is the general solution of $ A \vec{x_0} = \vec{b_0}$. More...
 
uint8_t trilateration_get_rank_and_homogeneous_solution (uint8_t anchor_num, matrix_t anchor_pos_matrix[anchor_num][3], matrix_t Xh[])
 Compute the rank and the solution of the homogeneous system $ A \vec{x_0} = 0 $. More...
 
void trilateration_get_particular_solution (uint8_t m, uint8_t n, matrix_t anchor_pos_mat[m][n], matrix_t dist_arr[], matrix_t Xp[])
 Compute the particular solution, which is the general solution of $ A \vec{x_0} = \vec{b_0} $. More...
 
void trilateration_get_quadratic_equation_solution (matrix_t Xp[], matrix_t Xh[], matrix_t solution_x1[], matrix_t solution_x2[])
 Solve a quadratic equation. More...
 
void trilateration_get_A_matrix (uint8_t m, matrix_t anchor_pos_matrix[m][3], matrix_t A_matrix[m][4])
 Computes the matrix $ A $ of the equation system: $ A \vec{x} = \vec{b}$. More...
 
void trilateration_get_b_vector (uint8_t anchor_num, matrix_t dist_arr[], matrix_t anchor_pos_matrix[anchor_num][3], matrix_t b_vec[])
 Computes the vector $ \vec{b} $ of the equation system: $ A \vec{x} = \vec{b}$. More...
 
void trilateration_solve_linear_equation (uint8_t line_num, uint8_t col_num, matrix_t pseudo_inv_matrix[line_num][col_num], matrix_t b_vec[], matrix_t sol_vec[])
 Solve a linear equation. More...
 

Detailed Description

Implement the trilateration 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 trilateration.c.

Function Documentation

◆ trilateration1()

void trilateration1 ( uint8_t  anchor_num,
matrix_t  anchor_pos_matrix[anchor_num][3],
matrix_t  pseudo_inv_matrix[4][anchor_num],
matrix_t  homog_sol_arr[],
matrix_t  dist_arr[],
matrix_t  solution_x1[],
matrix_t  solution_x2[] 
)

Implement the trilateration algorithm using the pre-processed pseudo-inverse matrix.

Note
This function should be initialized with the pre-processed pseudo-inverse matrix of the equation system: $ A \vec{x} = 0 $. Caution!: The solution_x1 and the solution_x2 vectors have a length of 3 or 4 in the case of two-dimensional or three-dimensional space. The first element $(x_0)$ of the solution_x1[] and the solution_x2[] vectors is a measure of the solvability of the multilateration problem. For example, in the three-dimensional space: $ d = x_0 - \left( x_1^{2} + x_2^{2} + x_3^{2} \right)$.
Parameters
[in]anchor_numnumber of the reference stations.
[in]anchor_pos_matrix[][]three-dimensional coordinates of the reference stations.
[in]pseudo_inv_matrix[][]pointer to the pre-processed pseudo-inverse matrix.
[in]homog_sol_arr[]the homogeneous solution.
[in]dist_arr[]distances to the reference stations.
[in,out]solution_x1[]includes the first solution. It has the length of 3 or 4 in the case of two-dimensional or three-dimensional space.
[in,out]solution_x2[]includes the second solution. It has length of 3 or 4 in the case of two-dimensional or three-dimensional space.

Definition at line 32 of file trilateration.c.

References matrix_t, trilateration_get_b_vector(), trilateration_get_quadratic_equation_solution(), trilateration_preprocessed_get_particular_solution(), and trilateration_solve_linear_equation().

Referenced by magnetic_based_preprocessing_get_position().

◆ trilateration2()

void trilateration2 ( uint8_t  anchor_num,
matrix_t  anchor_pos_matrix[anchor_num][3],
matrix_t  dist_arr[],
matrix_t  solution_x1[],
matrix_t  solution_x2[] 
)

Implement the trilateration algorithm.

Note
The pre-processed pseudo-inverse matrix of the equation system: $ A \vec{x} = 0 $ is computed on the mobile station. Caution!: The solution_x1 and the solution_x2 vectors have a length of 3 or 4 in the case of two-dimensional or three-dimensional space. The first element $(x_0)$ of the solution_x1[] and the solution_x2[] vectors is a measure of the solvability of the multilateration problem. For example, in the three-dimensional space: $ d = x_0 - \left( x_1^{2} + x_2^{2} + x_3^{2} \right)$.
Parameters
[in]anchor_numnumber of the reference stations.
[in]anchor_pos_matrix[][]three-dimensional coordinates of the reference stations.
[in]dist_arr[]distances to the reference stations.
[in,out]solution_x1[]includes the first solution. It has the length of 3 or 4 in the case of two-dimensional or three-dimensional space.
[in,out]solution_x2[]includes the second solution. It has the length of 3 or 4 in the case of two-dimensional or three-dimensional space.

Definition at line 69 of file trilateration.c.

References matrix_t, moore_penrose_get_pinv(), trilateration_get_A_matrix(), trilateration_get_b_vector(), trilateration_get_particular_solution(), trilateration_get_quadratic_equation_solution(), trilateration_get_rank_and_homogeneous_solution(), and trilateration_solve_linear_equation().

Referenced by distance_based_test(), magnetic_based_test(), and recog_mitigate_multipath().

◆ trilateration_get_A_matrix()

void trilateration_get_A_matrix ( uint8_t  m,
matrix_t  anchor_pos_matrix[m][3],
matrix_t  A_matrix[m][4] 
)

Computes the matrix $ A $ of the equation system: $ A \vec{x} = \vec{b}$.

Parameters
[in]mnumber of the reference stations.
[in]anchor_pos_matrix[][]three-dimensional coordinates of the reference stations.
[in,out]A_matrix[][]pointer to the matrix $ A $.

Definition at line 256 of file trilateration.c.

Referenced by pos_algos_common_test(), trilateration2(), trilateration_get_particular_solution(), and trilateration_get_rank_and_homogeneous_solution().

◆ trilateration_get_b_vector()

void trilateration_get_b_vector ( uint8_t  anchor_num,
matrix_t  dist_arr[],
matrix_t  anchor_pos_matrix[anchor_num][3],
matrix_t  b_vec[] 
)

Computes the vector $ \vec{b} $ of the equation system: $ A \vec{x} = \vec{b}$.

Parameters
[in]anchor_numnumber of the reference stations.
[in]dist_arr[]distances to the reference stations.
[in]anchor_pos_matrix[][]three-dimensional coordinates of the reference stations.
[in,out]b_vec[]pointer to the vector $ vec{b} $.

Definition at line 275 of file trilateration.c.

Referenced by trilateration1(), trilateration2(), and trilateration_get_particular_solution().

◆ trilateration_get_particular_solution()

void trilateration_get_particular_solution ( uint8_t  m,
uint8_t  n,
matrix_t  anchor_pos_mat[m][n],
matrix_t  dist_arr[],
matrix_t  Xp[] 
)

Compute the particular solution, which is the general solution of $ A \vec{x_0} = \vec{b_0} $.

Note
The particular solution is calculated in the case of three reference stations. The particular solution is computed on the mobile station.
Parameters
[in]mnumber of the reference stations.
[in]ncolumn number of the reference stations matrix.
[in]anchor_pos_mat[][]three-dimensional coordinates of the reference stations.
[in]dist_arr[]distances to the reference stations.
[in,out]Xp[]includes the particular solution.

Definition at line 179 of file trilateration.c.

References matrix_mul_vec(), matrix_t, moore_penrose_get_pinv(), trilateration_get_A_matrix(), and trilateration_get_b_vector().

Referenced by trilateration2().

◆ trilateration_get_quadratic_equation_solution()

void trilateration_get_quadratic_equation_solution ( matrix_t  particular_solu_arr[],
matrix_t  homogeneous_solution_arr[],
matrix_t  solution_x1[],
matrix_t  solution_x2[] 
)

Solve a quadratic equation.

The quotients of the quadratic equation are derived from the particular and homogeneous solution.

Parameters
[in]particular_solu_arr[]pointer to the particular solution.
[in]homogeneous_solution_arr[]pointer to the homogeneous solution.
[in,out]solution_x1[]presents the first solution.
[in,out]solution_x2[]presents the second solution.

Definition at line 193 of file trilateration.c.

References matrix_t.

Referenced by trilateration1(), and trilateration2().

◆ trilateration_get_rank_and_homogeneous_solution()

uint8_t trilateration_get_rank_and_homogeneous_solution ( uint8_t  anchor_num,
matrix_t  anchor_pos_matrix[anchor_num][3],
matrix_t  Xh[] 
)

Compute the rank and the solution of the homogeneous system $ A \vec{x_0} = 0 $.

Parameters
[in]anchor_numnumber of the reference stations.
[in]anchor_pos_matrix[][]three-dimensional coordinates of the reference stations.
[in,out]Xh[]includes the homogeneous solution.
Returns
the rank of the matrix $ A $.

Definition at line 149 of file trilateration.c.

References matrix_dim_t::col_num, matrix_get_rank(), matrix_t, matrix_dim_t::row_num, svd(), svd_get_single_values_num(), svd_get_U_dim(), and trilateration_get_A_matrix().

Referenced by trilateration2().

◆ trilateration_preprocessed_get_particular_solution()

void trilateration_preprocessed_get_particular_solution ( matrix_t  pseudo_inv_matrix[4][3],
matrix_t  b_arr[],
matrix_t  particular_solu_arr[] 
)

Compute the particular solution, which is the general solution of $ A \vec{x_0} = \vec{b_0}$.

Note
The particular solution is calculated in the case of three reference stations. The particular solution is computed using the pre-processed pseudo-inverse matrix.
Parameters
[in]pseudo_inv_matrix[][]pointer to the pre-processed pseudo-inverse matrix.
[in]b_arr[]pointer the vector $ vec{b} $.
[in,out]particular_solu_arr[]includes the particular solution.

Definition at line 121 of file trilateration.c.

References matrix_t.

Referenced by trilateration1().

◆ trilateration_solve_linear_equation()

void trilateration_solve_linear_equation ( uint8_t  line_num,
uint8_t  col_num,
matrix_t  pseudo_inv_matrix[line_num][col_num],
matrix_t  b_vec[],
matrix_t  sol_vec[] 
)

Solve a linear equation.

The linear equation is solved by using the pre-processed pseudo-inverse matrix and the vector $ \vec{b} $

Parameters
[in]line_numrow number of the pseudo-inverse matrix.
[in]col_numcolumn number of the pseudo-inverse matrix.
[in]pseudo_inv_matrix[][]pointer to the pre-processed pseudo-inverse matrix.
[in]b_vec[]pointer to the vector $ \vec{b} $.
[out]sol_vec[]solution vector.

Definition at line 291 of file trilateration.c.

References matrix_t.

Referenced by trilateration1(), and trilateration2().