RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
DOP.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 <math.h>
24 
25 #include "DOP.h"
26 
28 #include "matrix.h"
29 
30 matrix_t get_PDOP(uint8_t m, matrix_t ref_Matrix[m][3],
31  matrix_t true_pos[m])
32 {
33 
34  matrix_t PDOP = 0.0;
35  matrix_t H[m][4];
36  matrix_t H_transpose[4][m];
37  matrix_t H_transpose_H[4][4];
38  matrix_t H_transpose_H_pinv[4][4];
39 
40  for (int i = 0; i < m; i++) {
41  matrix_t R_i = sqrt(pow(ref_Matrix[i][0] - true_pos[0], 2)
42  + pow(ref_Matrix[i][1] - true_pos[1], 2)
43  + pow(ref_Matrix[i][2] - true_pos[2], 2));
44 
45  for (int l = 0; l < 3; l++) {
46  H[i][l] = (ref_Matrix[i][l] - true_pos[l]) / R_i;
47  }
48  H[i][3] = -1;
49  }
50 
51  matrix_transpose(m, 4, H, H_transpose);
52  matrix_mul(4, m, H_transpose, m, 4, H, H_transpose_H);
53  moore_penrose_get_pinv(4, 4, H_transpose_H, H_transpose_H_pinv);
54 
55  for (int i = 0; i < 3; i++) {
56  PDOP = PDOP + H_transpose_H_pinv[i][i];
57  }
58  PDOP = sqrt(PDOP);
59  return PDOP;
60 }
moore_penrose_get_pinv
int8_t moore_penrose_get_pinv(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t pinv_A[n][m])
Calculate the Moore–Penrose inverse of a rectangular matrix.
Definition: moore_penrose_pseudo_inverse.c:38
matrix_transpose
void matrix_transpose(uint8_t m, uint8_t n, matrix_t src_matrix[m][n], matrix_t dest_matrix[n][m])
Computes the transpose of a matrix.
Definition: matrix.c:56
get_PDOP
matrix_t get_PDOP(uint8_t m, matrix_t ref_Matrix[m][3], matrix_t true_pos[m])
Compute the Position Dilution of Precision (PDOP).
Definition: DOP.c:30
matrix.h
Matrix computations.
moore_penrose_pseudo_inverse.h
Moore–Penrose algorithm to compute the pseudo-inverse of a matrix.
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
DOP.h
Compute the Position Dilution of Precision (PDOP).
matrix_mul
void matrix_mul(uint8_t a_line_num, uint8_t a_col_num, matrix_t a_matrix[a_line_num][a_col_num], uint8_t b_line_num, uint8_t b_col_num, matrix_t b_matrix[b_line_num][b_col_num], matrix_t dest_matrix[a_line_num][b_col_num])
Compute the multiplication of two matrices.
Definition: matrix.c:363