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

added dataPool Output, in case MEKF inverion fails
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-11-21 10:30:48 +01:00
parent f07b905df5
commit 13fe25ff03
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -893,6 +893,16 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov, *invResidualCov,
invResidualCov1, MDF);
if (inversionFailed) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false;
return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}