diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 4717dc79..199440d5 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -888,14 +888,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // (H * P * H' + R) MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> - // double invResidualCov1[MDF] = {0}; double invResidualCov[MDF][MDF] = {{0}}; - // int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, - // *invResidualCov, - // invResidualCov1, MDF); int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); - double test[MDF][MDF]; - MatrixOperations::multiply(*residualCov, *invResidualCov, *test, MDF, MDF, MDF); if (inversionFailed) { { PoolReadGuard pg(mekfData); diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 8279ec40..6484a72d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -285,7 +285,7 @@ class MathOperations { static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) { float det = 0; - T1 matrix[size][size], submatrix[size][size]; + T1 matrix[size][size], submatrix[size - 1][size - 1]; for (uint8_t row = 0; row < size; row++) { for (uint8_t col = 0; col < size; col++) { matrix[row][col] = inputMatrix[row * size + col]; @@ -294,26 +294,25 @@ class MathOperations { if (size == 2) return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1])); else { - for (uint8_t x = 0; x < size; x++) { - int subi = 0; - for (uint8_t i = 1; i < size; i++) { - int subj = 0; - for (uint8_t j = 0; j < size; j++) { - if (j == x) continue; - submatrix[subi][subj] = matrix[i][j]; - subj++; + for (uint8_t col = 0; col < size; col++) { + int subRow = 0; + for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) { + int subCol = 0; + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + if (colIndex == col) continue; + submatrix[subRow][subCol] = matrix[rowIndex][colIndex]; + subCol++; } - subi++; + subRow++; } - det = det + (pow(-1, x) * matrix[0][x] * - MathOperations::matrixDeterminant(*submatrix, size - 1)); + det += (pow(-1, col) * matrix[0][col] * + MathOperations::matrixDeterminant(*submatrix, size - 1)); } } return det; } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - std::cout << MathOperations::matrixDeterminant(inputMatrix, size) << std::endl; if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { return 1; // Matrix is singular and not invertible } @@ -330,66 +329,64 @@ class MathOperations { identity[diag][diag] = 1; } // gauss-jordan algo - // start with gauss + // sort matrix such as no diag entry shall be 0 + // should not be needed as such a matrix has a det=0 for (uint8_t row = 0; row < size; row++) { - uint8_t rowIndex = row; - // check if diag entry is 0 - // in case it is, find next row whose diag entry is not 0 - while (matrix[rowIndex][row] == 0) { - if (rowIndex < size) { + if (matrix[row][row] == 0.0) { + bool swaped = false; + uint8_t rowIndex = 0; + while ((rowIndex < size) && !swaped) { + if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) { + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + swaped = true; + } rowIndex++; - } else { - return 1; // Matrix is not invertible + } + if (!swaped) { + return 1; // matrix not invertible } } - // swap rows if needed - if (rowIndex != row) { + } + + for (int row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + uint8_t rowIndex; + if (row == 0) { + rowIndex = size - 1; + } else { + rowIndex = row - 1; + } for (uint8_t colIndex = 0; colIndex < size; colIndex++) { std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); } - } - // normalize line - double normFactor = matrix[row][row]; - for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[row][colIndex] /= normFactor; - identity[row][colIndex] /= normFactor; - } - // make elements of the same col in following rows to 0 - std::cout << "C++ sucks" << std::endl; - for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) { - double elimFactor = matrix[rowIndex][row]; - for (uint8_t colIndex = 0; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; - identity[rowIndex][colIndex] -= identity[row][colIndex] * elimFactor; + row--; + if (row < 0) { + return 1; // Matrix is not invertible } } } - // finish with jordan - for (uint8_t row = size - 1; row > 0; row--) { - for (int16_t rowIndex = row - 1; rowIndex >= 0; rowIndex--) { - double elimFactor = matrix[rowIndex][row]; - for (uint8_t colIndex = 0; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; - identity[rowIndex][row] -= identity[row][colIndex] * elimFactor; + // remove non diag elements in matrix (jordan) + for (int row = 0; row < size; row++) { + for (int rowIndex = 0; rowIndex < size; rowIndex++) { + if (row != rowIndex) { + double ratio = matrix[rowIndex][row] / matrix[row][row]; + for (int colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex]; + identity[rowIndex][colIndex] -= ratio * identity[row][colIndex]; + } } } } - T1 test[size][size]; - MatrixOperations::multiply(inputMatrix, *identity, *test, size, size, size); - std::cout << "[\n" - << test[0][0] << " " << test[0][1] << " " << test[0][2] << " " << test[0][3] << " " - << test[0][4] << " " << test[0][5] << "\n" - << test[1][0] << " " << test[1][1] << " " << test[1][2] << " " << test[1][3] << " " - << test[1][4] << " " << test[1][5] << "\n" - << test[2][0] << " " << test[2][1] << " " << test[2][2] << " " << test[2][3] << " " - << test[2][4] << " " << test[2][5] << "\n" - << test[3][0] << " " << test[3][1] << " " << test[3][2] << " " << test[3][3] << " " - << test[3][4] << " " << test[3][5] << "\n" - << test[4][0] << " " << test[4][1] << " " << test[4][2] << " " << test[4][3] << " " - << test[4][4] << " " << test[4][5] << "\n" - << test[5][0] << " " << test[5][1] << " " << test[5][2] << " " << test[5][3] << " " - << test[5][4] << " " << test[5][5] << "\n]" << std::endl; + // normalize rows in matrix (gauss) + for (int row = 0; row < size; row++) { + for (int col = 0; col < size; col++) { + identity[row][col] = identity[row][col] / matrix[row][row]; + } + } std::memcpy(inverse, identity, sizeof(identity)); return 0; // successful inversion }