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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +10
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 13fe25ff03 - Show all commits

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
}