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

cleanup
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-03-15 10:17:24 +01:00
parent 102228100e
commit 95a4d88ed4
3 changed files with 2 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1080,12 +1080,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
return MEKF_NOT_FINITE;
}
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return MEKF_RUNNING; return MEKF_RUNNING;
} }
@ -1095,7 +1089,6 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(propagatedQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED; return MEKF_UNINITIALIZED;

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

@ -62,7 +62,6 @@ class MultiplicativeKalmanFilter {
NO_MODEL_VECTORS = 2, NO_MODEL_VECTORS = 2,
NO_SUS_MGM_STR_DATA = 3, NO_SUS_MGM_STR_DATA = 3,
COVARIANCE_INVERSION_FAILED = 4, COVARIANCE_INVERSION_FAILED = 4,
NOT_FINITE = 5,
INITIALIZED = 10, INITIALIZED = 10,
RUNNING = 11, RUNNING = 11,
}; };
@ -75,9 +74,8 @@ class MultiplicativeKalmanFilter {
static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5); static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED = static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
returnvalue::makeCode(IF_MEKF_ID, 6); returnvalue::makeCode(IF_MEKF_ID, 6);
static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7); static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8); static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
private: private:
/*Parameters*/ /*Parameters*/

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

@ -404,26 +404,6 @@ class MathOperations {
std::memcpy(inverse, identity, sizeof(identity)); std::memcpy(inverse, identity, sizeof(identity));
return 0; // successful inversion return 0; // successful inversion
} }
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
for (uint8_t i = 0; i < size; i++) {
if (not isfinite(inputVector[i])) {
return false;
}
}
return true;
}
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
for (uint8_t col = 0; col < cols; col++) {
for (uint8_t row = 0; row < rows; row++) {
if (not isfinite(inputMatrix[row * cols + cols])) {
return false;
}
}
}
return true;
}
}; };
#endif /* ACS_MATH_MATHOPERATIONS_H_ */ #endif /* ACS_MATH_MATHOPERATIONS_H_ */