|
RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
|
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 . 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 . 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 . 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 of the equation system: . 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 of the equation system: . 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... | |
Implement the trilateration algorithm.
Definition in file trilateration.c.
| 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.
. 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
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:
.| [in] | anchor_num | number 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().
| 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.
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
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:
.| [in] | anchor_num | number 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().
| void trilateration_get_A_matrix | ( | uint8_t | m, |
| matrix_t | anchor_pos_matrix[m][3], | ||
| matrix_t | A_matrix[m][4] | ||
| ) |
Computes the matrix
of the equation system:
.
| [in] | m | number of the reference stations. |
| [in] | anchor_pos_matrix[][] | three-dimensional coordinates of the reference stations. |
| [in,out] | A_matrix[][] | pointer to the matrix . |
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().
| 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
of the equation system:
.
| [in] | anchor_num | number 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 . |
Definition at line 275 of file trilateration.c.
Referenced by trilateration1(), trilateration2(), and 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
.
| [in] | m | number of the reference stations. |
| [in] | n | column 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().
| 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.
| [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().
| 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
.
| [in] | anchor_num | number of the reference stations. |
| [in] | anchor_pos_matrix[][] | three-dimensional coordinates of the reference stations. |
| [in,out] | Xh[] | includes the homogeneous solution. |
. 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().
| 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
.
| [in] | pseudo_inv_matrix[][] | pointer to the pre-processed pseudo-inverse matrix. |
| [in] | b_arr[] | pointer the vector . |
| [in,out] | particular_solu_arr[] | includes the particular solution. |
Definition at line 121 of file trilateration.c.
References matrix_t.
Referenced by trilateration1().
| 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 
| [in] | line_num | row number of the pseudo-inverse matrix. |
| [in] | col_num | column 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 . |
| [out] | sol_vec[] | solution vector. |
Definition at line 291 of file trilateration.c.
References matrix_t.
Referenced by trilateration1(), and trilateration2().