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

added finite check for MEKF
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-03-13 15:58:48 +01:00
parent 63f45ec27e
commit 79bd349ac7
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1080,6 +1080,12 @@ 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;
} }

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

@ -62,6 +62,7 @@ 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,
}; };
@ -74,8 +75,9 @@ 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_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
private: private:
/*Parameters*/ /*Parameters*/