eive-obsw/mission/controller/acs/AttitudeEstimation.cpp

#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<double>::normalize(mgmB, normMgmB, 3);
    VectorOperations<double>::normalize(mgmI, normMgmI, 3);
    VectorOperations<double>::normalize(susB, normSusB, 3);
    VectorOperations<double>::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<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 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);
    }
  } else {
    // ToDo: fill dataset and set to invalid
  }
}