added finite check for MEKF
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
63f45ec27e
commit
79bd349ac7
@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
|
||||
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);
|
||||
return MEKF_RUNNING;
|
||||
}
|
||||
|
@ -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*/
|
||||
|
Loading…
Reference in New Issue
Block a user