Merge pull request 'Quest Fix III' (#837) from i-should-quit into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good

Reviewed-on: #837
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2023-12-14 09:59:28 +01:00
commit 26b9343ca4
2 changed files with 8 additions and 6 deletions

View File

@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
## Fixed
- Fixed faulty scaling within the QUEST algorithm.
# [v7.5.1] 2023-12-13
- `eive-tmtc` v5.12.1

View File

@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI);
}
// Low Pass