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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +56 -65
2 changed files with 56 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit ce83b64ca2 - Show all commits

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

@ -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);

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

@ -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<T1>::matrixDeterminant(*submatrix, size - 1));
det += (pow(-1, col) * matrix[0][col] *
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
}
}
return det;
}
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) {
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<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
}