112 lines
5.1 KiB
C++
112 lines
5.1 KiB
C++
#include "AttitudeEstimation.h"
|
|
|
|
AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
|
|
acsParameters = acsParameters_;
|
|
}
|
|
|
|
AttitudeEstimation::~AttitudeEstimation() {}
|
|
|
|
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
|
|
acsctrl::MgmDataProcessed *mgmData,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimation) {
|
|
if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
|
|
mgmData->mgmVecTot.isValid() and mgmData->magIgrfModel.isValid())) {
|
|
{
|
|
PoolReadGuard pg{attitudeEstimation};
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
|
|
attitudeEstimation->quatQuest.setValid(false);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Normalize Data
|
|
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
|
|
normSusI[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(susData->susVecTot.value, normSusB, 3);
|
|
VectorOperations<double>::normalize(susData->sunIjkModel.value, normSusI, 3);
|
|
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
|
|
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
|
|
|
|
// Create Helper Vectors
|
|
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
|
|
helperSum[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
|
|
VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
|
|
VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
|
|
VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
|
|
VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
|
|
VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
|
|
|
|
// Sensor Weights
|
|
double kSus = 0, kMgm = 0;
|
|
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
|
|
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
|
|
|
|
// Weighted Vectors
|
|
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
|
|
kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
|
|
VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
|
|
VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
|
|
VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
|
|
VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
|
|
|
|
// Some weird Angles
|
|
double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
|
|
(VectorOperations<double>::dot(weightedSusB, normSusI) +
|
|
VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
|
|
VectorOperations<double>::dot(helperCross, kSumVec);
|
|
double beta = VectorOperations<double>::dot(helperSum, kSumVec);
|
|
double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
|
|
|
|
// I don't even know what this is supposed to be
|
|
double constPlus =
|
|
1. / (2 * std::sqrt(gamma * (gamma + alpha) *
|
|
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
|
|
double constMinus =
|
|
1. / (2 * std::sqrt(gamma * (gamma - alpha) *
|
|
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
|
|
|
|
// Calculate Quaternion
|
|
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
|
|
qRotVecPt1[3] = {0, 0, 0};
|
|
if (alpha >= 0) {
|
|
// Scalar Part
|
|
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>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
|
|
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
|
|
|
|
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
|
|
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>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
|
|
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
|
|
|
|
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
|
|
QuaternionOperations::normalize(qBI, qBI);
|
|
}
|
|
// Low Pass
|
|
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
|
|
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
|
|
}
|
|
{
|
|
PoolReadGuard pg{attitudeEstimation};
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
|
|
attitudeEstimation->quatQuest.setValid(true);
|
|
}
|
|
}
|
|
}
|