From b2442041f0c74581c5003e589d7cea68c0eee778 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 30 Nov 2022 09:07:09 +0100 Subject: [PATCH] gauss-jordan v2 that hopefully noone ever sees --- .../acs/MultiplicativeKalmanFilter.cpp | 10 ++-- mission/controller/acs/util/MathOperations.h | 53 +++++++++++++++---- 2 files changed, 49 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5d66bd8d..4717dc79 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -888,10 +888,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // (H * P * H' + R) MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> - double invResidualCov1[MDF] = {0}; + // double invResidualCov1[MDF] = {0}; double invResidualCov[MDF][MDF] = {{0}}; - int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, *invResidualCov, - invResidualCov1, MDF); + // 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 c37dbde8..8279ec40 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -313,10 +313,11 @@ class MathOperations { } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - if (MathOperations::matrixDeterminant(*inputMatrix, size) == 0) { - return 0; // Matrix is singular and not invertible + std::cout << MathOperations::matrixDeterminant(inputMatrix, size) << std::endl; + if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { + return 1; // Matrix is singular and not invertible } - T1 matrix[size][size], identity[size][size] = {0}; + T1 matrix[size][size], identity[size][size]; // reformat array to matrix for (uint8_t row = 0; row < size; row++) { for (uint8_t col = 0; col < size; col++) { @@ -324,10 +325,12 @@ class MathOperations { } } // init identity matrix + std::memset(identity, 0.0, sizeof(identity)); for (uint8_t diag = 0; diag < size; diag++) { identity[diag][diag] = 1; } // gauss-jordan algo + // start with gauss for (uint8_t row = 0; row < size; row++) { uint8_t rowIndex = row; // check if diag entry is 0 @@ -336,7 +339,7 @@ class MathOperations { if (rowIndex < size) { rowIndex++; } else { - return 0; // Matrix is not invertible + return 1; // Matrix is not invertible } } // swap rows if needed @@ -347,20 +350,48 @@ class MathOperations { } } // normalize line + double normFactor = matrix[row][row]; for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[row][colIndex] = matrix[row][colIndex] / matrix[row][row]; - identity[row][colIndex] = identity[row][colIndex] / matrix[row][row]; + 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++) { - for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * matrix[rowIndex][row]; - identity[rowIndex][colIndex] -= identity[row][colIndex] * matrix[rowIndex][row]; + 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; } } } - std::memcpy(inverse, identity, size * size * sizeof(T1)); - return 1; // successful inversion + // 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; + } + } + } + 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; + std::memcpy(inverse, identity, sizeof(identity)); + return 0; // successful inversion } };