eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

gauss-jordan v2 that hopefully noone ever sees

Browse Source
This commit is contained in:
Marius Eggert 2022-11-30 09:07:09 +01:00
parent b2e0ef24f3
commit b2442041f0
2 changed files with 49 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -888,10 +888,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// (H * P * H' + R) // (H * P * H' + R)
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
// <<INVERSE residualCov HIER>> // <<INVERSE residualCov HIER>>
double invResidualCov1[MDF] = {0}; // double invResidualCov1[MDF] = {0};
double invResidualCov[MDF][MDF] = {{0}}; double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov, *invResidualCov, // int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov,
invResidualCov1, MDF); // *invResidualCov,
// invResidualCov1, MDF);
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
double test[MDF][MDF];
MatrixOperations<double>::multiply(*residualCov, *invResidualCov, *test, MDF, MDF, MDF);
if (inversionFailed) { if (inversionFailed) {
{ {
PoolReadGuard pg(mekfData); PoolReadGuard pg(mekfData);

53
mission/controller/acs/util/MathOperations.h
View File

@ -313,10 +313,11 @@ class MathOperations {
} }
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
if (MathOperations<T1>::matrixDeterminant(*inputMatrix, size) == 0) { std::cout << MathOperations<T1>::matrixDeterminant(inputMatrix, size) << std::endl;
return 0; // Matrix is singular and not invertible if (MathOperations<T1>::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 // reformat array to matrix
for (uint8_t row = 0; row < size; row++) { for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) { for (uint8_t col = 0; col < size; col++) {
@ -324,10 +325,12 @@ class MathOperations {
} }
} }
// init identity matrix // init identity matrix
std::memset(identity, 0.0, sizeof(identity));
for (uint8_t diag = 0; diag < size; diag++) { for (uint8_t diag = 0; diag < size; diag++) {
identity[diag][diag] = 1; identity[diag][diag] = 1;
} }
// gauss-jordan algo // gauss-jordan algo
// start with gauss
for (uint8_t row = 0; row < size; row++) { for (uint8_t row = 0; row < size; row++) {
uint8_t rowIndex = row; uint8_t rowIndex = row;
// check if diag entry is 0 // check if diag entry is 0
@ -336,7 +339,7 @@ class MathOperations {
if (rowIndex < size) { if (rowIndex < size) {
rowIndex++; rowIndex++;
} else { } else {
return 0; // Matrix is not invertible return 1; // Matrix is not invertible
} }
} }
// swap rows if needed // swap rows if needed
@ -347,20 +350,48 @@ class MathOperations {
} }
} }
// normalize line // normalize line
double normFactor = matrix[row][row];
for (uint8_t colIndex = row; colIndex < size; colIndex++) { for (uint8_t colIndex = row; colIndex < size; colIndex++) {
matrix[row][colIndex] = matrix[row][colIndex] / matrix[row][row]; matrix[row][colIndex] /= normFactor;
identity[row][colIndex] = identity[row][colIndex] / matrix[row][row]; identity[row][colIndex] /= normFactor;
} }
// make elements of the same col in following rows to 0 // 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 rowIndex = row + 1; rowIndex < size; rowIndex++) {
for (uint8_t colIndex = row; colIndex < size; colIndex++) { double elimFactor = matrix[rowIndex][row];
matrix[rowIndex][colIndex] -= matrix[row][colIndex] * matrix[rowIndex][row]; for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
identity[rowIndex][colIndex] -= identity[row][colIndex] * matrix[rowIndex][row]; matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor;
identity[rowIndex][colIndex] -= identity[row][colIndex] * elimFactor;
} }
} }
} }
std::memcpy(inverse, identity, size * size * sizeof(T1)); // finish with jordan
return 1; // successful inversion 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<T1>::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
} }
}; };