diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 2ada8ab1..e752da39 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); + if (not(MathOperations::checkVectorIsFinite(propagatedQuaternion, 4)) || + not(MathOperations::checkMatrixIsFinite(initialQuaternion, 6, 6))) { + updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); + return MEKF_NOT_FINITE; + } + updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); return MEKF_RUNNING; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index ceb98339..fe749890 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter { NO_MODEL_VECTORS = 2, NO_SUS_MGM_STR_DATA = 3, COVARIANCE_INVERSION_FAILED = 4, + NOT_FINITE = 5, INITIALIZED = 10, 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_COVARIANCE_INVERSION_FAILED = returnvalue::makeCode(IF_MEKF_ID, 6); - static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); - static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); + static constexpr ReturnValue_t MEKF_NOT_FINITE = 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, 9); private: /*Parameters*/