From a45e96b772398e079fd8367260ba968c878792a1 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Nov 2023 13:36:04 +0100 Subject: [PATCH] quest attitude estimation --- mission/controller/acs/AttitudeEstimation.cpp | 91 +++++++++++++++++++ mission/controller/acs/AttitudeEstimation.h | 28 ++++++ mission/controller/acs/CMakeLists.txt | 1 + 3 files changed, 120 insertions(+) create mode 100644 mission/controller/acs/AttitudeEstimation.cpp create mode 100644 mission/controller/acs/AttitudeEstimation.h diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp new file mode 100644 index 00000000..0e8be1b1 --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -0,0 +1,91 @@ +#include "AttitudeEstimation.h" + +AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) { + acsParameters = acsParameters_; +} + +AttitudeEstimation::~AttitudeEstimation() {} + +void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const double susI[3], + const bool susIvalid, const double mgmB[3], const bool mgmBvalid, + const double mgmI[3], const bool mgmIvalid, double qBI[4]) { + if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) { + // Normalize Data + double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, + normSusI[3] = {0, 0, 0}; + VectorOperations::normalize(mgmB, normMgmB, 3); + VectorOperations::normalize(mgmI, normMgmI, 3); + VectorOperations::normalize(susB, normSusB, 3); + VectorOperations::normalize(susI, normSusI, 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::cross(normSusB, normMgmB, normHelperB); + VectorOperations::cross(normSusI, normMgmI, normHelperI); + VectorOperations::normalize(normHelperB, normHelperB, 3); + VectorOperations::normalize(normHelperI, normHelperI, 3); + VectorOperations::cross(normHelperB, normHelperI, helperCross); + VectorOperations::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::mulScalar(normSusB, kSus, weightedSusB, 3); + VectorOperations::mulScalar(normMgmB, kMgm, weightedMgmB, 3); + VectorOperations::cross(weightedSusB, normSusI, kSusVec); + VectorOperations::cross(weightedMgmB, normMgmI, kMgmVec); + VectorOperations::add(kSusVec, kMgmVec, kSumVec, 3); + + // Some weird Angles + double alpha = (1 + VectorOperations::dot(normHelperB, normHelperI)) * + (VectorOperations::dot(weightedSusB, normSusI) + + VectorOperations::dot(weightedMgmB, normMgmI)) + + VectorOperations::dot(helperCross, kSumVec); + double beta = VectorOperations::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::dot(normHelperB, normHelperI)))); + double constMinus = + 1. / (2 * std::sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + + // Calculate Quaternion + double 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::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constPlus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } else { + // Scalar Part + qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constMinus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } + } else { + // ToDo: fill dataset and set to invalid + } +} diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h new file mode 100644 index 00000000..eee58f81 --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.h @@ -0,0 +1,28 @@ +#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ + +#include +#include +#include + +#include +#include + +class AttitudeEstimation { + public: + AttitudeEstimation(AcsParameters *acsParameters_); + virtual ~AttitudeEstimation(); + ; + + void Quest(const double susB[3], const bool susBvalid, const double susI[3], const bool susIvalid, + const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid, + double qBI[4]); + + protected: + private: + AcsParameters *acsParameters; + + void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]); +}; + +#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */ diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index a8b0e9a6..4af05d8b 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + AttitudeEstimation.cpp FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp