working versions of inverse calc and determinant calc
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
This commit is contained in:
parent
b2442041f0
commit
ce83b64ca2
@ -888,14 +888,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
// (H * P * H' + R)
|
||||
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
|
||||
// <<INVERSE residualCov HIER>>
|
||||
// double invResidualCov1[MDF] = {0};
|
||||
double invResidualCov[MDF][MDF] = {{0}};
|
||||
// int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov,
|
||||
// *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) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
|
||||
@ -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,18 +294,18 @@ 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] *
|
||||
det += (pow(-1, col) * matrix[0][col] *
|
||||
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
|
||||
}
|
||||
}
|
||||
@ -313,7 +313,6 @@ class MathOperations {
|
||||
}
|
||||
|
||||
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
||||
std::cout << MathOperations<T1>::matrixDeterminant(inputMatrix, size) << std::endl;
|
||||
if (MathOperations<T1>::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) {
|
||||
rowIndex++;
|
||||
} else {
|
||||
return 1; // Matrix is not invertible
|
||||
}
|
||||
}
|
||||
// swap rows if needed
|
||||
if (rowIndex != row) {
|
||||
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;
|
||||
}
|
||||
// normalize line
|
||||
double normFactor = matrix[row][row];
|
||||
for (uint8_t colIndex = row; colIndex < size; colIndex++) {
|
||||
matrix[row][colIndex] /= normFactor;
|
||||
identity[row][colIndex] /= normFactor;
|
||||
rowIndex++;
|
||||
}
|
||||
if (!swaped) {
|
||||
return 1; // matrix not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
// 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;
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
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<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;
|
||||
}
|
||||
// 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
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user