diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8fcbcbca..351aa5fd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun sdcMan(sdcMan), attitudeEstimation(&acsParameters), fusedRotationEstimation(&acsParameters), + navigation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), ptgCtrl(&acsParameters), @@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t } case UPDATE_TLE: { if (size != 69 * 2) { - return INVALID_PARAMETERS; + return HasActionsIF::INVALID_PARAMETERS; } ReturnValue_t result = updateTle(data, data + 69, false); if (result != returnvalue::OK) { @@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t } std::memcpy(tle + 69, line2, 69); actionHelper.reportData(commandedBy, actionId, tle, 69 * 2); - return EXECUTION_FINISHED; + return HasActionsIF::EXECUTION_FINISHED; } + case (UPDATE_STANDARD_DEVIATIONS): + navigation.updateMekfStandardDeviations(&acsParameters); + return HasActionsIF::EXECUTION_FINISHED; default: { return HasActionsIF::INVALID_ACTION_ID; } @@ -176,7 +180,7 @@ void AcsController::performAttitudeControl() { mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &attitudeEstimationData, &acsParameters); + &susDataProcessed, timeDelta, &attitudeEstimationData); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e8102f35..087173b8 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -111,6 +111,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2; static const DeviceCommandId_t UPDATE_TLE = 0x3; static const DeviceCommandId_t READ_TLE = 0x4; + static const DeviceCommandId_t UPDATE_STANDARD_DEVIATIONS = 0x5; ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 647e2141..06595ec3 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1,1128 +1,636 @@ #include "MultiplicativeKalmanFilter.h" -#include -#include -#include -#include -#include -#include - -#include - -MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters) { + updateStandardDeviations(acsParameters); +} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} -ReturnValue_t MultiplicativeKalmanFilter::init( - const double *magneticField_, const bool validMagField_, const double *sunDir_, - const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters) { // valids for "model measurements"? - // check for valid mag/sun - if (validMagField_ && validSS && validSSModel && validMagModel) { - // QUEST ALGO ----------------------------------------------------------------------- - double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; - sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr; - - double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, - normSunJ[3] = {0, 0, 0}; - VectorOperations::normalize(magneticField_, normMagB, 3); // b2 - VectorOperations::normalize(sunDir_, normSunB, 3); // b1 - VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 - VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 - - double thirdAxis_B[3] = {0, 0, 0}, thirdAxis_J[3] = {0, 0, 0}; - VectorOperations::cross(normSunJ, normMagJ, thirdAxis_J); - VectorOperations::cross(normSunB, normMagB, thirdAxis_B); - VectorOperations::normalize(thirdAxis_J, thirdAxis_J, 3); // rx - VectorOperations::normalize(thirdAxis_B, thirdAxis_B, 3); // bx - - double weigthSun = 1 / (sigmaSun * sigmaSun); // a1 - double weigthMag = 1 / (sigmaMag * sigmaMag); // a2 - - double a[3] = {0, 0, 0}, b[3] = {0, 0, 0}; - VectorOperations::mulScalar(normSunB, weigthSun, a, 3); // a - VectorOperations::mulScalar(normMagB, weigthMag, b, 3); // b - - double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0}, - weigthCrossSum[3] = {0, 0, 0}; - VectorOperations::cross(thirdAxis_B, thirdAxis_J, - thirdAxisCross); // cross(bx,rx) - VectorOperations::cross(a, normSunJ, weightACross); - VectorOperations::cross(b, normMagJ, weightBCross); - VectorOperations::add(weightACross, weightBCross, weigthCrossSum); - - double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}; - VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); - VectorOperations::add(weightACross, weightBCross, sum2); - - double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; - alpha = (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)) * - (VectorOperations::dot(a, normSunJ) + - VectorOperations::dot(b, normMagJ)) + - VectorOperations::dot(thirdAxisCross, weigthCrossSum); - beta = VectorOperations::dot(thirdAxisSum, sum2); - gamma = sqrt(alpha * alpha + beta * beta); - constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) * - (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); - constMin = 1 / (2 * sqrt(gamma * (gamma - alpha) * - (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); - - /*Quaternion Computation*/ - double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0}; - if (alpha >= 0) { - VectorOperations::mulScalar(thirdAxisCross, gamma + alpha, var1, 3); - VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); - VectorOperations::mulScalar(var2, beta, var2, 3); - VectorOperations::add(var1, var2, quat); - - for (int i = 0; i < 3; i++) { - initialQuaternion[i] = quat[i]; - } - initialQuaternion[3] = - (gamma + alpha) * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); - VectorOperations::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4); - } else { - double diffGammaAlpha = gamma - alpha; - VectorOperations::mulScalar(thirdAxisCross, beta, var1, 3); - VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); - VectorOperations::mulScalar(var2, diffGammaAlpha, var2, 3); - VectorOperations::add(var1, var2, quat); - for (int i = 0; i < 3; i++) { - initialQuaternion[i] = quat[i]; - } - initialQuaternion[3] = beta * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); - VectorOperations::mulScalar(initialQuaternion, constMin, initialQuaternion, 4); - } - propagatedQuaternion[0] = initialQuaternion[0]; - propagatedQuaternion[1] = initialQuaternion[1]; - propagatedQuaternion[2] = initialQuaternion[2]; - propagatedQuaternion[3] = initialQuaternion[3]; - - /*Initial Covariance Matrix------------------------------------ */ - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, - magEstB[3] = {0, 0, 0}; - QuaternionOperations::toDcm(initialQuaternion, dcmBJ); - MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); - - double matrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); - MatrixOperations::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); - MatrixOperations::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); - MatrixOperations::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); - - double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0; - MatrixOperations::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3); - MatrixOperations::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3); - MatrixOperations::add(*matrixSunMag, *matrixMagSun, *partA, 3, 3); - double factor = weigthMag * weigthSun * VectorOperations::dot(sunEstB, magEstB); - MatrixOperations::multiplyScalar(*partA, factor, *partA, 3, 3); - MatrixOperations::add(*partA, *matrixSun, *partA, 3, 3); - MatrixOperations::add(*partA, *matrixMag, *partA, 3, 3); - - double crossSunMag[3] = {0, 0, 0}; - double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - VectorOperations::cross(sunEstB, magEstB, crossSunMag); - partB = 1 / (weigthMag * weigthSun * pow(VectorOperations::norm(crossSunMag, 3), 2)); - MatrixOperations::multiplyScalar(*partA, partB, *partA, 3, 3); - MatrixOperations::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3); - factor = 1 / (weigthMag + weigthMag); - - double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3); - double initGyroCov[3][3] = { - {pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}}; - initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; - initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; - initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; - initialCovarianceMatrix[0][3] = 0; - initialCovarianceMatrix[0][4] = 0; - initialCovarianceMatrix[0][5] = 0; - initialCovarianceMatrix[1][0] = questCovMatrix[1][0]; - initialCovarianceMatrix[1][1] = questCovMatrix[1][1]; - initialCovarianceMatrix[1][2] = questCovMatrix[1][2]; - initialCovarianceMatrix[1][3] = 0; - initialCovarianceMatrix[1][4] = 0; - initialCovarianceMatrix[1][5] = 0; - initialCovarianceMatrix[2][0] = questCovMatrix[2][0]; - initialCovarianceMatrix[2][1] = questCovMatrix[2][1]; - initialCovarianceMatrix[2][2] = questCovMatrix[2][2]; - initialCovarianceMatrix[2][3] = 0; - initialCovarianceMatrix[2][4] = 0; - initialCovarianceMatrix[2][5] = 0; - initialCovarianceMatrix[3][0] = 0; - initialCovarianceMatrix[3][1] = 0; - initialCovarianceMatrix[3][2] = 0; - initialCovarianceMatrix[3][3] = initGyroCov[0][0]; - initialCovarianceMatrix[3][4] = initGyroCov[0][1]; - initialCovarianceMatrix[3][5] = initGyroCov[0][2]; - initialCovarianceMatrix[4][0] = 0; - initialCovarianceMatrix[4][1] = 0; - initialCovarianceMatrix[4][2] = 0; - initialCovarianceMatrix[4][3] = initGyroCov[1][0]; - initialCovarianceMatrix[4][4] = initGyroCov[1][1]; - initialCovarianceMatrix[4][5] = initGyroCov[1][2]; - initialCovarianceMatrix[5][0] = 0; - initialCovarianceMatrix[5][1] = 0; - initialCovarianceMatrix[5][2] = 0; - initialCovarianceMatrix[5][3] = initGyroCov[2][0]; - initialCovarianceMatrix[5][4] = initGyroCov[2][1]; - initialCovarianceMatrix[5][5] = initGyroCov[2][2]; - updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); - return MEKF_INITIALIZED; - } else { - // no initialisation possible, no valid measurements - updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); - return MEKF_UNINITIALIZED; - } -} - -// --------------- MEKF DISCRETE TIME STEP ------------------------------- -ReturnValue_t MultiplicativeKalmanFilter::mekfEst( - const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, - const bool validGYRs_, const double *magneticField_, const bool validMagField_, - const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters) { - // Check for GYR Measurements - int MDF = 0; // Matrix Dimension Factor - if (!validGYRs_) { - updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); - return MEKF_NO_GYR_DATA; - } - // Check for Model Calculations - else if (!validSSModel || !validMagModel) { - updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); - return MEKF_NO_MODEL_VECTORS; - } - // Check Measurements available from SS, MAG, STR - if (validSS && validMagField_ && validSTR_) { - sensorsAvail = 1; - MDF = 9; - } else if (validSS && validMagField_ && !validSTR_) { - sensorsAvail = 2; - MDF = 6; - } else if (validSS && !validMagField_ && validSTR_) { - sensorsAvail = 3; - MDF = 6; - } else if (!validSS && validMagField_ && validSTR_) { - sensorsAvail = 4; - MDF = 6; - } else if (validSS && !validMagField_ && !(validSTR_)) { - sensorsAvail = 5; - MDF = 3; - } else if (!validSS && validMagField_ && !validSTR_) { - sensorsAvail = 6; - MDF = 3; - } else if (!validSS && !validMagField_ && validSTR_) { - sensorsAvail = 7; - MDF = 3; - } else { - sensorsAvail = 8; // no measurements - updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA); - return returnvalue::FAILED; - } - - // If we are here, MEKF will perform - double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; +void MultiplicativeKalmanFilter::updateStandardDeviations(const AcsParameters *acsParameters) { + sigmaSus = acsParameters->kalmanFilterParameters.sensorNoiseSus; + sigmaMgm = acsParameters->kalmanFilterParameters.sensorNoiseMgm; sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr; + sigmaGyr = acsParameters->kalmanFilterParameters.sensorNoiseGyr; + sigmaGyrArw = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw; + sigmaGyrBs = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs; - double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, - normSunJ[3] = {0, 0, 0}; - VectorOperations::normalize(magneticField_, normMagB, 3); // b2 - VectorOperations::normalize(sunDir_, normSunB, 3); // b1 - VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 - VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 - - /*-------GAIN - UPDATE - STEP------*/ - double covMatrix11[3][3] = { - {pow(sigmaSun, 2), 0, 0}, {0, pow(sigmaSun, 2), 0}, {0, 0, pow(sigmaSun, 2)}}; - double covMatrix22[3][3] = { - {pow(sigmaMag, 2), 0, 0}, {0, pow(sigmaMag, 2), 0}, {0, 0, pow(sigmaMag, 2)}}; - double covMatrix33[3][3] = { - {pow(sigmaStr, 2), 0, 0}, {0, pow(sigmaStr, 2), 0}, {0, 0, pow(sigmaStr, 2)}}; - - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, - magEstB[3] = {0, 0, 0}; - QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); - MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); - double quatEstErr[3] = {0, 0, 0}; - - // Measurement Sensitivity Matrix - double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss - double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag - double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str - MatrixOperations::skewMatrix(sunEstB, *measSensMatrix11); - MatrixOperations::skewMatrix(magEstB, *measSensMatrix22); - - double measVecQuat[3] = {0, 0, 0}; - if (validSTR_) { - double quatError[4] = {0, 0, 0, 0}; - double invPropagatedQuat[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); - QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); - for (int i = 0; i < 3; i++) { - measVecQuat[i] = 2 * quatError[i] / quatError[3]; - } - } - // Adjust matrices based on valid measurements - double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], measEstVec[MDF]; - if (sensorsAvail == 1) { // All measurements valid - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix22[0][0]; - measSensMatrix[3][1] = measSensMatrix22[0][1]; - measSensMatrix[3][2] = measSensMatrix22[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix22[1][0]; - measSensMatrix[4][1] = measSensMatrix22[1][1]; - measSensMatrix[4][2] = measSensMatrix22[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix22[2][0]; - measSensMatrix[5][1] = measSensMatrix22[2][1]; - measSensMatrix[5][2] = measSensMatrix22[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - measSensMatrix[6][0] = measSensMatrix33[0][0]; - measSensMatrix[6][1] = measSensMatrix33[0][1]; - measSensMatrix[6][2] = measSensMatrix33[0][2]; - measSensMatrix[6][3] = 0; - measSensMatrix[6][4] = 0; - measSensMatrix[6][5] = 0; - measSensMatrix[7][0] = measSensMatrix33[1][0]; - measSensMatrix[7][1] = measSensMatrix33[1][1]; - measSensMatrix[7][2] = measSensMatrix33[1][2]; - measSensMatrix[7][3] = 0; - measSensMatrix[7][4] = 0; - measSensMatrix[7][5] = 0; - measSensMatrix[8][0] = measSensMatrix33[2][0]; - measSensMatrix[8][1] = measSensMatrix33[2][1]; - measSensMatrix[8][2] = measSensMatrix33[2][2]; - measSensMatrix[8][3] = 0; - measSensMatrix[8][4] = 0; - measSensMatrix[8][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[0][6] = 0; - measCovMatrix[0][7] = 0; - measCovMatrix[0][8] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[1][6] = 0; - measCovMatrix[1][7] = 0; - measCovMatrix[1][8] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[2][6] = 0; - measCovMatrix[2][7] = 0; - measCovMatrix[2][8] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix22[0][0]; - measCovMatrix[3][4] = covMatrix22[0][1]; - measCovMatrix[3][5] = covMatrix22[0][2]; - measCovMatrix[3][6] = 0; - measCovMatrix[3][7] = 0; - measCovMatrix[3][8] = 0; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix22[1][0]; - measCovMatrix[4][4] = covMatrix22[1][1]; - measCovMatrix[4][5] = covMatrix22[1][2]; - measCovMatrix[4][6] = 0; - measCovMatrix[4][7] = 0; - measCovMatrix[4][8] = 0; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix22[2][0]; - measCovMatrix[5][4] = covMatrix22[2][1]; - measCovMatrix[5][5] = covMatrix22[2][2]; - measCovMatrix[5][6] = 0; - measCovMatrix[5][7] = 0; - measCovMatrix[5][8] = 0; - measCovMatrix[6][0] = 0; - measCovMatrix[6][1] = 0; - measCovMatrix[6][2] = 0; - measCovMatrix[6][3] = 0; - measCovMatrix[6][4] = 0; - measCovMatrix[6][5] = 0; - measCovMatrix[6][6] = covMatrix33[0][0]; - measCovMatrix[6][7] = covMatrix33[0][1]; - measCovMatrix[6][8] = covMatrix33[0][2]; - measCovMatrix[7][0] = 0; - measCovMatrix[7][1] = 0; - measCovMatrix[7][2] = 0; - measCovMatrix[7][3] = 0; - measCovMatrix[7][4] = 0; - measCovMatrix[7][5] = 0; - measCovMatrix[7][6] = covMatrix33[1][0]; - measCovMatrix[7][7] = covMatrix33[1][1]; - measCovMatrix[7][8] = covMatrix33[1][2]; - measCovMatrix[8][0] = 0; - measCovMatrix[8][1] = 0; - measCovMatrix[8][2] = 0; - measCovMatrix[8][3] = 0; - measCovMatrix[8][4] = 0; - measCovMatrix[8][5] = 0; - measCovMatrix[8][6] = covMatrix33[2][0]; - measCovMatrix[8][7] = covMatrix33[2][1]; - measCovMatrix[8][8] = covMatrix33[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = normMagB[0]; - measVec[4] = normMagB[1]; - measVec[5] = normMagB[2]; - measVec[6] = measVecQuat[0]; - measVec[7] = measVecQuat[1]; - measVec[8] = measVecQuat[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = magEstB[0]; - measEstVec[4] = magEstB[1]; - measEstVec[5] = magEstB[2]; - measEstVec[6] = quatEstErr[0]; - measEstVec[7] = quatEstErr[1]; - measEstVec[8] = quatEstErr[2]; - - } else if (sensorsAvail == 2) { // ss / mag valid - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix22[0][0]; - measSensMatrix[3][1] = measSensMatrix22[0][1]; - measSensMatrix[3][2] = measSensMatrix22[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix22[1][0]; - measSensMatrix[4][1] = measSensMatrix22[1][1]; - measSensMatrix[4][2] = measSensMatrix22[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix22[2][0]; - measSensMatrix[5][1] = measSensMatrix22[2][1]; - measSensMatrix[5][2] = measSensMatrix22[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix22[0][0]; - measCovMatrix[3][4] = covMatrix22[0][1]; - measCovMatrix[3][5] = covMatrix22[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix22[1][0]; - measCovMatrix[4][4] = covMatrix22[1][1]; - measCovMatrix[4][5] = covMatrix22[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix22[2][0]; - measCovMatrix[5][4] = covMatrix22[2][1]; - measCovMatrix[5][5] = covMatrix22[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = normMagB[0]; - measVec[4] = normMagB[1]; - measVec[5] = normMagB[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = magEstB[0]; - measEstVec[4] = magEstB[1]; - measEstVec[5] = magEstB[2]; - - } else if (sensorsAvail == 3) { // ss / str valid - - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix33[0][0]; - measSensMatrix[3][1] = measSensMatrix33[0][1]; - measSensMatrix[3][2] = measSensMatrix33[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix33[1][0]; - measSensMatrix[4][1] = measSensMatrix33[1][1]; - measSensMatrix[4][2] = measSensMatrix33[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix33[2][0]; - measSensMatrix[5][1] = measSensMatrix33[2][1]; - measSensMatrix[5][2] = measSensMatrix33[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix33[0][0]; - measCovMatrix[3][4] = covMatrix33[0][1]; - measCovMatrix[3][5] = covMatrix33[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix33[1][0]; - measCovMatrix[4][4] = covMatrix33[1][1]; - measCovMatrix[4][5] = covMatrix33[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix33[2][0]; - measCovMatrix[5][4] = covMatrix33[2][1]; - measCovMatrix[5][5] = covMatrix33[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = measVecQuat[0]; - measVec[4] = measVecQuat[1]; - measVec[5] = measVecQuat[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = quatEstErr[0]; - measEstVec[4] = quatEstErr[1]; - measEstVec[5] = quatEstErr[2]; - - } else if (sensorsAvail == 4) { // mag / str avail - - measSensMatrix[0][0] = measSensMatrix22[0][0]; - measSensMatrix[0][1] = measSensMatrix22[0][1]; - measSensMatrix[0][2] = measSensMatrix22[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix22[1][0]; - measSensMatrix[1][1] = measSensMatrix22[1][1]; - measSensMatrix[1][2] = measSensMatrix22[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix22[2][0]; - measSensMatrix[2][1] = measSensMatrix22[2][1]; - measSensMatrix[2][2] = measSensMatrix22[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix33[0][0]; - measSensMatrix[3][1] = measSensMatrix33[0][1]; - measSensMatrix[3][2] = measSensMatrix33[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix33[1][0]; - measSensMatrix[4][1] = measSensMatrix33[1][1]; - measSensMatrix[4][2] = measSensMatrix33[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix33[2][0]; - measSensMatrix[5][1] = measSensMatrix33[2][1]; - measSensMatrix[5][2] = measSensMatrix33[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix22[0][0]; - measCovMatrix[0][1] = covMatrix22[0][1]; - measCovMatrix[0][2] = covMatrix22[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix22[1][0]; - measCovMatrix[1][1] = covMatrix22[1][1]; - measCovMatrix[1][2] = covMatrix22[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix22[2][0]; - measCovMatrix[2][1] = covMatrix22[2][1]; - measCovMatrix[2][2] = covMatrix22[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix33[0][0]; - measCovMatrix[3][4] = covMatrix33[0][1]; - measCovMatrix[3][5] = covMatrix33[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix33[1][0]; - measCovMatrix[4][4] = covMatrix33[1][1]; - measCovMatrix[4][5] = covMatrix33[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix33[2][0]; - measCovMatrix[5][4] = covMatrix33[2][1]; - measCovMatrix[5][5] = covMatrix33[2][2]; - - measVec[0] = normMagB[0]; - measVec[1] = normMagB[1]; - measVec[2] = normMagB[2]; - measVec[3] = measVecQuat[0]; - measVec[4] = measVecQuat[1]; - measVec[5] = measVecQuat[2]; - - measEstVec[0] = magEstB[0]; - measEstVec[1] = magEstB[1]; - measEstVec[2] = magEstB[2]; - measEstVec[3] = quatEstErr[0]; - measEstVec[4] = quatEstErr[1]; - measEstVec[5] = quatEstErr[2]; - - } else if (sensorsAvail == 5) { // only ss - - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - - } else if (sensorsAvail == 6) { // only mag - - measSensMatrix[0][0] = measSensMatrix22[0][0]; - measSensMatrix[0][1] = measSensMatrix22[0][1]; - measSensMatrix[0][2] = measSensMatrix22[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix22[1][0]; - measSensMatrix[1][1] = measSensMatrix22[1][1]; - measSensMatrix[1][2] = measSensMatrix22[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix22[2][0]; - measSensMatrix[2][1] = measSensMatrix22[2][1]; - measSensMatrix[2][2] = measSensMatrix22[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix22[0][0]; - measCovMatrix[0][1] = covMatrix22[0][1]; - measCovMatrix[0][2] = covMatrix22[0][2]; - measCovMatrix[1][0] = covMatrix22[1][0]; - measCovMatrix[1][1] = covMatrix22[1][1]; - measCovMatrix[1][2] = covMatrix22[1][2]; - measCovMatrix[2][0] = covMatrix22[2][0]; - measCovMatrix[2][1] = covMatrix22[2][1]; - measCovMatrix[2][2] = covMatrix22[2][2]; - - measVec[0] = normMagB[0]; - measVec[1] = normMagB[1]; - measVec[2] = normMagB[2]; - - measEstVec[0] = magEstB[0]; - measEstVec[1] = magEstB[1]; - measEstVec[2] = magEstB[2]; - - } else if (sensorsAvail == 7) { // only str - - measSensMatrix[0][0] = measSensMatrix33[0][0]; - measSensMatrix[0][1] = measSensMatrix33[0][1]; - measSensMatrix[0][2] = measSensMatrix33[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix33[1][0]; - measSensMatrix[1][1] = measSensMatrix33[1][1]; - measSensMatrix[1][2] = measSensMatrix33[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix33[2][0]; - measSensMatrix[2][1] = measSensMatrix33[2][1]; - measSensMatrix[2][2] = measSensMatrix33[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix33[0][0]; - measCovMatrix[0][1] = covMatrix33[0][1]; - measCovMatrix[0][2] = covMatrix33[0][2]; - measCovMatrix[1][0] = covMatrix33[1][0]; - measCovMatrix[1][1] = covMatrix33[1][1]; - measCovMatrix[1][2] = covMatrix33[1][2]; - measCovMatrix[2][0] = covMatrix33[2][0]; - measCovMatrix[2][1] = covMatrix33[2][1]; - measCovMatrix[2][2] = covMatrix33[2][2]; - - measVec[0] = measVecQuat[0]; - measVec[1] = measVecQuat[1]; - measVec[2] = measVecQuat[2]; - - measEstVec[0] = quatEstErr[0]; - measEstVec[1] = quatEstErr[1]; - measEstVec[2] = quatEstErr[2]; - } - // Kalman Gain: [K = P * H' / (H * P * H' + R)] - double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; - - double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], residualCov1[6][MDF]; - // H * P * H' - MatrixOperations::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF); - MatrixOperations::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1, - 6, 6, MDF); - MatrixOperations::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF); - // negative values, restrictions ? - // (H * P * H' + R) - MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); - // <> - double invResidualCov[MDF][MDF] = {{0}}; - ReturnValue_t result = - MatrixOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); - if (result != returnvalue::OK) { - updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); - return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed - } - - // [K = P * H' / (H * P * H' + R)] - MatrixOperations::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF, - MDF); - MatrixOperations::multiply(*initialCovarianceMatrix, *kalmanGain1, *kalmanGain, 6, 6, - MDF); - - /* ------- UPDATE -STEP ---------*/ - - // Update Covariance Matrix: P_plus = (I-K*H)*P_min - double covMatPlus[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - double identityMatrix[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, - {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; - double updateCov1[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - MatrixOperations::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF); - MatrixOperations::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6); - MatrixOperations::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6); - - // Error State Vector - double errStateVec[6] = {0, 0, 0, 0, 0, 0}; - double errStateVec1[MDF] = {0}; - VectorOperations::subtract(measVec, measEstVec, errStateVec1, MDF); - MatrixOperations::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1); - - double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; - double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; - - // State Vector Elements - double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::skewMatrix(propagatedQuaternion, *xi2); - double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); - MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); - double xi[4][3] = { - {xi1[0][0], xi1[0][1], xi1[0][2]}, - {xi1[1][0], xi1[1][1], xi1[1][2]}, - {xi1[2][0], xi1[2][1], xi1[2][2]}, - {-propagatedQuaternion[0], -propagatedQuaternion[1], -propagatedQuaternion[2]}}; - - double errQuatTerm[4] = {0, 0, 0, 0}; - MatrixOperations::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); - VectorOperations::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); - VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); - VectorOperations::normalize(quatBJ, quatBJ, 4); - - double updatedGyroBias[3] = {0, 0, 0}; - VectorOperations::add(biasGYR, errStateGyroBias, updatedGyroBias, 3); - // Bias GYR State - biasGYR[0] = updatedGyroBias[0]; - biasGYR[1] = updatedGyroBias[1]; - biasGYR[2] = updatedGyroBias[2]; - - /* ----------- PROPAGATION ----------*/ - double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs; - double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw; - - double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, - {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; - double rotRateEst[3] = {0, 0, 0}; - VectorOperations::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3); - double normRotEst = VectorOperations::norm(rotRateEst, 3); - double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]}, - {rotRateEst[2], 0, -rotRateEst[0]}, - {-rotRateEst[1], rotRateEst[0], 0}}; - - // Discrete Process Noise Covariance Q - double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) { - double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime + - 1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - - double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2)); - MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); - std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); - - double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; - MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); - } else { - double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; - MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); - - double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3]; - double fact12_0 = - (normRotEst * acsParameters->onBoardParams.sampleTime - - sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); - MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); - double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3); - double fact12_2 = - (1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) + - cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) / - pow(normRotEst, 4); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); - MatrixOperations::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3); - MatrixOperations::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3); - MatrixOperations::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3); - MatrixOperations::transpose(*covQ12, *covQ12trans, 3); - - double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; - double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime; - MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); - double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3); - double fact11_2 = - (2 * normRotEst * acsParameters->onBoardParams.sampleTime - - 2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) - - 1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) / - pow(normRotEst, 5); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); - MatrixOperations::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3); - MatrixOperations::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3); - MatrixOperations::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3); - } - discProcessNoiseCov[0][0] = covQ11[0][0]; - discProcessNoiseCov[0][1] = covQ11[0][1]; - discProcessNoiseCov[0][2] = covQ11[0][2]; - discProcessNoiseCov[0][3] = covQ12[0][0]; - discProcessNoiseCov[0][4] = covQ12[0][1]; - discProcessNoiseCov[0][5] = covQ12[0][2]; - discProcessNoiseCov[1][0] = covQ11[1][0]; - discProcessNoiseCov[1][1] = covQ11[1][1]; - discProcessNoiseCov[1][2] = covQ11[1][2]; - discProcessNoiseCov[1][3] = covQ12[1][0]; - discProcessNoiseCov[1][4] = covQ12[1][1]; - discProcessNoiseCov[1][5] = covQ12[1][2]; - discProcessNoiseCov[2][0] = covQ11[2][0]; - discProcessNoiseCov[2][1] = covQ11[2][1]; - discProcessNoiseCov[2][2] = covQ11[2][2]; - discProcessNoiseCov[2][3] = covQ12[2][0]; - discProcessNoiseCov[2][4] = covQ12[2][1]; - discProcessNoiseCov[2][5] = covQ12[2][2]; - discProcessNoiseCov[3][0] = covQ12trans[0][0]; - discProcessNoiseCov[3][1] = covQ12trans[0][1]; - discProcessNoiseCov[3][2] = covQ12trans[0][2]; - discProcessNoiseCov[3][3] = covQ22[0][0]; - discProcessNoiseCov[3][4] = covQ22[0][1]; - discProcessNoiseCov[3][5] = covQ22[0][2]; - discProcessNoiseCov[4][0] = covQ12trans[1][0]; - discProcessNoiseCov[4][1] = covQ12trans[1][1]; - discProcessNoiseCov[4][2] = covQ12trans[1][2]; - discProcessNoiseCov[4][3] = covQ22[1][0]; - discProcessNoiseCov[4][4] = covQ22[1][1]; - discProcessNoiseCov[4][5] = covQ22[1][2]; - discProcessNoiseCov[5][0] = covQ12trans[2][0]; - discProcessNoiseCov[5][1] = covQ12trans[2][1]; - discProcessNoiseCov[5][2] = covQ12trans[2][2]; - discProcessNoiseCov[5][3] = covQ22[2][0]; - discProcessNoiseCov[5][4] = covQ22[2][1]; - discProcessNoiseCov[5][5] = covQ22[2][2]; - - // State Transition Matrix phi - double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; - double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; - double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; - MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); - double fact11_2 = - (1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); - MatrixOperations::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3); - MatrixOperations::add(*phi11_01, *phi11_2, *phi11, 3, 3); - - double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3]; - double fact12_0 = fact11_2; - MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, - acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3); - double fact12_2 = - (normRotEst * acsParameters->onBoardParams.sampleTime - - sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); - MatrixOperations::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3); - MatrixOperations::subtract(*phi12_01, *phi12_2, *phi12, 3, 3); - - phi[0][0] = phi11[0][0]; - phi[0][1] = phi11[0][1]; - phi[0][2] = phi11[0][2]; - phi[0][3] = phi12[0][0]; - phi[0][4] = phi12[0][1]; - phi[0][5] = phi12[0][2]; - phi[1][0] = phi11[1][0]; - phi[1][1] = phi11[1][1]; - phi[1][2] = phi11[1][2]; - phi[1][3] = phi12[1][0]; - phi[1][4] = phi12[1][1]; - phi[1][5] = phi12[1][2]; - phi[2][0] = phi11[2][0]; - phi[2][1] = phi11[2][1]; - phi[2][2] = phi11[2][2]; - phi[2][3] = phi12[2][0]; - phi[2][4] = phi12[2][1]; - phi[2][5] = phi12[2][2]; - - // Propagated Quaternion - double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime); - double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; - VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); - - double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::skewMatrix(rotSin, *skewSin); - - MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); - double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3); - double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]}, - {subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]}, - {subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]}, - {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}}; - MatrixOperations::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); - - // Update Covariance Matrix - double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6]; - MatrixOperations::transpose(*phi, *transPhi, 6); - MatrixOperations::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6); - MatrixOperations::multiply(*phi, *cov0, *cov0, 6, 6, 6); - - MatrixOperations::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); - MatrixOperations::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); - MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); - - MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - - if (not(VectorOperations::isFinite(propagatedQuaternion, 4)) || - not(MatrixOperations::isFinite(*initialCovarianceMatrix, 6, 6))) { - updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); - return MEKF_NOT_FINITE; - } - - updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); - return MEKF_RUNNING; + MatrixOperations::multiplyScalar(*EYE3, sigmaSus * sigmaSus, *covSus, 3, 3); + MatrixOperations::multiplyScalar(*EYE3, sigmaMgm * sigmaMgm, *covMgm, 3, 3); + MatrixOperations::multiplyScalar(*EYE3, sigmaStr * sigmaStr, *covStr, 3, 3); } -ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) { - double resetQuaternion[4] = {0, 0, 0, 1}; - double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); - std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); - updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); +ReturnValue_t MultiplicativeKalmanFilter::reset( + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + std::memcpy(estimatedQuaternionBI, UNIT_QUAT, sizeof(estimatedQuaternionBI)); + std::memcpy(estimatedBiasGyr, ZERO_VEC3, sizeof(estimatedBiasGyr)); + std::memcpy(estimatedCovarianceMatrix, ZERO_MAT66, sizeof(estimatedCovarianceMatrix)); + mekfStatus = MekfStatus::UNINITIALIZED; + updateDataSetWithoutData(attitudeEstimationData); return MEKF_UNINITIALIZED; } -void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, - MekfStatus mekfStatus) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mekfData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double)); - mekfData->quatMekf.setValid(false); - std::memcpy(mekfData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double)); - mekfData->satRotRateMekf.setValid(false); - mekfData->mekfStatus = mekfStatus; - mekfData->setValidity(true, false); +ReturnValue_t MultiplicativeKalmanFilter::init( + const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + // check for valid QUEST quaternion + if (attitudeEstimationData->quatQuest.isValid()) { + // Initial Quaternion + std::memcpy(estimatedQuaternionBI, attitudeEstimationData->quatQuest.value, + sizeof(estimatedQuaternionBI)); + + // Initial Covariance + if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and + gyrData->gyrVecTot.isValid()) { + // QUEST Covariance + double normSunI[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0}, + normMagEstB[3] = {0, 0, 0}; + VectorOperations::normalize(susData->sunIjkModel.value, normSunI, 3); + VectorOperations::normalize(mgmData->magIgrfModel.value, normMagI, 3); + QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB); + QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB); + + double matrixSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + matrixMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + matrixSusMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + matrixMgmSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(normSunEstB, normSunEstB, *matrixSus, 3, 1, 3); + MatrixOperations::multiply(normMagEstB, normMagEstB, *matrixMgm, 3, 1, 3); + MatrixOperations::multiply(normSunEstB, normMagEstB, *matrixSusMgm, 3, 1, 3); + MatrixOperations::multiply(normMagEstB, normSunEstB, *matrixMgmSus, 3, 1, 3); + + double factorSus = 1. / (sigmaSus * sigmaSus); + double factorMgm = 1. / (sigmaMgm * sigmaMgm); + + double covQuest[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiplyScalar(*matrixSus, factorSus * factorSus, *matrixSus, 3, 3); + MatrixOperations::multiplyScalar(*matrixMgm, factorMgm * factorMgm, *matrixMgm, 3, 3); + MatrixOperations::add(*matrixSusMgm, *matrixMgmSus, *covQuest, 3, 3); + MatrixOperations::multiplyScalar( + *covQuest, + factorSus * factorMgm * VectorOperations::dot(normSunEstB, normMagEstB), + *covQuest, 3, 3); + MatrixOperations::add(*covQuest, *matrixSus, *covQuest, 3, 3); + MatrixOperations::add(*covQuest, *matrixMgm, *covQuest, 3, 3); + + double crossSunMag[3] = {0, 0, 0}; + VectorOperations::cross(normSunEstB, normMagEstB, crossSunMag); + double normCrossSunMag = VectorOperations::norm(crossSunMag, 3); + double factor = factorSus * factorMgm * normCrossSunMag * normCrossSunMag; + + MatrixOperations::multiplyScalar(*covQuest, 1. / factor, *covQuest, 3, 3); + MatrixOperations::add(*EYE3, *covQuest, *covQuest, 3, 3); + MatrixOperations::multiplyScalar(*covQuest, 1. / (factorSus + factorMgm), *covQuest, + 3, 3); + + // GYR Covariance + double covGyr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiplyScalar(*EYE3, sigmaGyr, *covGyr, 3, 3); + + // Combine Covariances + MatrixOperations::writeSubmatrix(*estimatedCovarianceMatrix, *covQuest, 3, 3, 6, 6, 0, + 0); + MatrixOperations::writeSubmatrix(*estimatedCovarianceMatrix, *covGyr, 3, 3, 6, 6, 3, + 3); + + mekfStatus = MekfStatus::INITIALIZED; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_INITIALIZED; } } + // no initialisation possible, no valid measurements + mekfStatus = MekfStatus::UNINITIALIZED; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_UNINITIALIZED; +} + +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, const double timeDelta, + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + ReturnValue_t result = checkAvailableSensors(susData, mgmData, gyrData, attitudeEstimationData); + if (result != returnvalue::OK) { + reset(attitudeEstimationData); + return result; + } + + double measSensMatrix[matrixDimensionFactor][6] = {{0}}, + measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, + measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0}; + kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); + + double kalmanGain[6][matrixDimensionFactor] = {{0}}; + result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData); + if (result != returnvalue::OK) { + reset(attitudeEstimationData); + return result; + } + + kfCovAposteriori(*kalmanGain, *measSensMatrix); + kfStateAposteriori(*kalmanGain, measVec, estVec); + kfPropagate(gyrData, timeDelta); + + result = kfFiniteCheck(attitudeEstimationData); + if (result != returnvalue::OK) { + return result; + } + mekfStatus = MekfStatus::RUNNING; + updateDataSet(attitudeEstimationData); + return MEKF_RUNNING; +} + +ReturnValue_t MultiplicativeKalmanFilter::checkAvailableSensors( + const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + // Check for GYR Measurements + if (not gyrData->gyrVecTot.isValid()) { + mekfStatus = MekfStatus::NO_GYR_DATA; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_NO_GYR_DATA; + } + // Check for Model Calculations + if (not susData->sunIjkModel.isValid() or not mgmData->magIgrfModel.isValid()) { + mekfStatus = MekfStatus::NO_MODEL_VECTORS; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_NO_MODEL_VECTORS; + } + // Check Measurements available from SUS, MGM, STR + if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and + not strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::NONE; + mekfStatus = MekfStatus::NO_SUS_MGM_STR_DATA; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_NO_SUS_MGM_STR_DATA; + } + if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::SUS_MGM_STR; + matrixDimensionFactor = 9; + } else if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and + not strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::SUS_MGM; + matrixDimensionFactor = 6; + } else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and + strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::SUS_STR; + matrixDimensionFactor = 6; + } else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and + strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::MGM_STR; + matrixDimensionFactor = 6; + } else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and + not strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::SUS; + matrixDimensionFactor = 3; + } else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and + not strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::MGM; + matrixDimensionFactor = 3; + } else if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and + strData.strQuat.valid) { + sensorsAvailable = SensorAvailability::STR; + matrixDimensionFactor = 3; + } + return returnvalue::OK; +} + +void MultiplicativeKalmanFilter::kfUpdate(const acsctrl::SusDataProcessed *susData, + const acsctrl::MgmDataProcessed *mgmData, + double *measSensMatrix, double *measCovMatrix, + double *measVec, double *estVec) { + double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0}, + normSunI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0}, normMagEstB[3] = {0, 0, 0}; + VectorOperations::normalize(mgmData->mgmVecTot.value, normMagB, 3); // b2 + VectorOperations::normalize(susData->susVecTot.value, normSunB, 3); // b1 + VectorOperations::normalize(mgmData->magIgrfModel.value, normMagI, 3); // r2 + VectorOperations::normalize(susData->sunIjkModel.value, normSunI, 3); // r1 + QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB); + QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB); + + // Measurement Sensitivity Matrix + double measSensMatrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // H11 + double measSensMatrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // H22 + double measSensMatrixStr[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // H33 + MatrixOperations::skewMatrix(normSunEstB, *measSensMatrixSun); + MatrixOperations::skewMatrix(normMagEstB, *measSensMatrixMag); + + double measVecQuat[3] = {0, 0, 0}, quatEstErr[3] = {0, 0, 0}; + if (strData.strQuat.valid) { + double quatError[4] = {0, 0, 0, 0}; + double invPropagatedQuat[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(estimatedQuaternionBI, invPropagatedQuat); + QuaternionOperations::multiply(strData.strQuat.value, invPropagatedQuat, quatError); + for (uint8_t i = 0; i < 3; i++) { + measVecQuat[i] = 2. * quatError[i] / quatError[3]; + } + } + + switch (sensorsAvailable) { + case SensorAvailability::SUS_MGM_STR: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3, + matrixDimensionFactor, 6, 0, 0); + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3, + matrixDimensionFactor, 6, 3, 0); + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3, + matrixDimensionFactor, 6, 6, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + MatrixOperations::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 3, 3); + MatrixOperations::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 6, 6); + + std::memcpy(measVec, normSunB, sizeof(normSunB)); + std::memcpy(measVec + 3, normMagB, sizeof(normMagB)); + std::memcpy(measVec + 6, measVecQuat, sizeof(measVecQuat)); + + std::memcpy(estVec, normSunEstB, sizeof(normSunEstB)); + std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB)); + std::memcpy(estVec + 6, quatEstErr, sizeof(quatEstErr)); + return; + case SensorAvailability::SUS_MGM: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3, + matrixDimensionFactor, 6, 0, 0); + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3, + matrixDimensionFactor, 6, 3, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + MatrixOperations::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 3, 3); + + std::memcpy(measVec, normSunB, sizeof(normSunB)); + std::memcpy(measVec + 3, normMagB, sizeof(normMagB)); + + std::memcpy(estVec, normSunEstB, sizeof(normSunEstB)); + std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB)); + return; + case SensorAvailability::SUS_STR: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3, + matrixDimensionFactor, 6, 0, 0); + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3, + matrixDimensionFactor, 6, 3, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + MatrixOperations::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 3, 3); + + std::memcpy(measVec, normSunB, sizeof(normSunB)); + std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat)); + + std::memcpy(estVec, normSunEstB, sizeof(normSunEstB)); + std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr)); + return; + case SensorAvailability::MGM_STR: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3, + matrixDimensionFactor, 6, 0, 0); + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3, + matrixDimensionFactor, 6, 3, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + MatrixOperations::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 3, 3); + + std::memcpy(measVec, normMagB, sizeof(normMagB)); + std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat)); + + std::memcpy(estVec, normMagEstB, sizeof(normMagEstB)); + std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr)); + return; + case SensorAvailability::SUS: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3, + matrixDimensionFactor, 6, 0, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + + std::memcpy(measVec, normSunB, sizeof(normSunB)); + + std::memcpy(estVec, normSunEstB, sizeof(normSunEstB)); + return; + case SensorAvailability::MGM: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3, + matrixDimensionFactor, 6, 0, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + + std::memcpy(measVec, normMagB, sizeof(normMagB)); + + std::memcpy(estVec, normMagEstB, sizeof(normMagEstB)); + return; + case SensorAvailability::STR: + MatrixOperations::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3, + matrixDimensionFactor, 6, 0, 0); + + MatrixOperations::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor, + matrixDimensionFactor, 0, 0); + + std::memcpy(measVec, measVecQuat, sizeof(measVecQuat)); + + std::memcpy(estVec, quatEstErr, sizeof(quatEstErr)); + return; + default: + sif::error << "MEKF::Invalid SensorAvailability Flag" << std::endl; + } } -void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, - MekfStatus mekfStatus, double quat[4], - double satRotRate[3]) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double)); - mekfData->mekfStatus = mekfStatus; - mekfData->setValidity(true, true); - } +ReturnValue_t MultiplicativeKalmanFilter::kfGain( + double *measSensMatrix, double *measCovMatrix, double *kalmanGain, + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + // Kalman Gain: K = P * H' / (H * P * H' + R) + double kalmanGainDen[6][matrixDimensionFactor] = {{0}}, + invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, + residualCov[6][matrixDimensionFactor] = {{0}}, + measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}}; + + MatrixOperations::transpose(measSensMatrix, *measSensMatrixTransposed, + matrixDimensionFactor, 6); + MatrixOperations::multiply(*estimatedCovarianceMatrix, *measSensMatrixTransposed, + *residualCov, 6, 6, matrixDimensionFactor); + + MatrixOperations::multiply(measSensMatrix, *residualCov, *kalmanGainDen, + matrixDimensionFactor, 6, matrixDimensionFactor); + MatrixOperations::add(*kalmanGainDen, measCovMatrix, *kalmanGainDen, + matrixDimensionFactor, matrixDimensionFactor); + ReturnValue_t result = MatrixOperations::inverseMatrix(*kalmanGainDen, *invKalmanGainDen, + matrixDimensionFactor); + if (result != returnvalue::OK) { + mekfStatus = MekfStatus::COVARIANCE_INVERSION_FAILED; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_COVARIANCE_INVERSION_FAILED; + } + + MatrixOperations::multiply(*residualCov, *invKalmanGainDen, kalmanGain, 6, + matrixDimensionFactor, matrixDimensionFactor); + return returnvalue::OK; +} + +void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *measSensMatrix) { + // Covariance Matrix: P_plus [k] = (I-K*H)*P_min + double helperMat[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + MatrixOperations::multiply(kalmanGain, measSensMatrix, *helperMat, 6, + matrixDimensionFactor, 6); + MatrixOperations::subtract(*EYE6, *helperMat, *helperMat, 6, 6); + MatrixOperations::multiply(*helperMat, *estimatedCovarianceMatrix, *covAposteriori, 6, 6, + 6); +} + +void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec, + double *estVec) { + double stateVecErr[6] = {0, 0, 0, 0, 0, 0}; + double plantOutputDiff[matrixDimensionFactor] = {0}; + VectorOperations::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor); + MatrixOperations::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6, + matrixDimensionFactor, 1); + + double quatAposteriori[3] = {stateVecErr[0], stateVecErr[1], stateVecErr[2]}; + double biasAposteriori[3] = {stateVecErr[3], stateVecErr[4], stateVecErr[5]}; + + double quaternionIdentity[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double quaternionIdentityUpperMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double quaternionIdentityUpperMatrixHelper[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double negQuatVec[3] = {0, 0, 0}; + MatrixOperations::multiplyScalar(*EYE3, estimatedQuaternionBI[3], + *quaternionIdentityUpperMatrix, 3, 3); + MatrixOperations::skewMatrix(estimatedQuaternionBI, *quaternionIdentityUpperMatrixHelper); + MatrixOperations::add(*quaternionIdentityUpperMatrix, + *quaternionIdentityUpperMatrixHelper, + *quaternionIdentityUpperMatrix, 3, 3); + + VectorOperations::mulScalar(estimatedQuaternionBI, -1, negQuatVec, 3); + + MatrixOperations::writeSubmatrix(*quaternionIdentity, *quaternionIdentityUpperMatrix, 3, + 3, 4, 3, 0, 0); + MatrixOperations::writeSubmatrix(*quaternionIdentity, negQuatVec, 1, 3, 4, 3, 3, 0); + + double quatCorrection[4] = {0, 0, 0, 0}; + MatrixOperations::multiply(*quaternionIdentity, quatAposteriori, quatCorrection, 4, 3, 1); + VectorOperations::mulScalar(quatCorrection, 0.5, quatCorrection, 4); + VectorOperations::add(estimatedQuaternionBI, quatCorrection, estimatedQuaternionBI, 4); + QuaternionOperations::normalize(estimatedQuaternionBI); + + VectorOperations::add(estimatedBiasGyr, biasAposteriori, estimatedBiasGyr, 3); +} + +void MultiplicativeKalmanFilter::kfPropagate(const acsctrl::GyrDataProcessed *gyrData, + const double timeDelta) { + // Estimated Rotation Rate + VectorOperations::subtract(gyrData->gyrVecTot.value, estimatedBiasGyr, estimatedRotRate); + double estimatedRotRateNorm = VectorOperations::norm(estimatedRotRate, 3); + double estimatedRotRateSkewed[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::skewMatrix(estimatedRotRate, *estimatedRotRateSkewed); + + // Estimated Quaternion + double diffQuatMatrix[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}, + diffQuatMatrixHelper1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + diffQuatMatrixHelper2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, diffVector[3] = {0, 0, 0}, + helperQuat[4] = {0, 0, 0, 0}; + double diffScalar = std::cos(.5 * estimatedRotRateNorm * timeDelta); + VectorOperations::mulScalar( + estimatedRotRate, std::sin(.5 * estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm, + diffVector, 3); + MatrixOperations::multiplyScalar(*EYE3, diffScalar, *diffQuatMatrixHelper1, 3, 3); + MatrixOperations::skewMatrix(diffVector, *diffQuatMatrixHelper2); + MatrixOperations::subtract(*diffQuatMatrixHelper1, *diffQuatMatrixHelper2, + *diffQuatMatrixHelper1, 3, 3); + + MatrixOperations::writeSubmatrix(*diffQuatMatrix, *diffQuatMatrixHelper1, 3, 3, 4, 4, 0, + 0); + MatrixOperations::writeSubmatrix(*diffQuatMatrix, diffVector, 3, 1, 4, 4, 0, 3); + VectorOperations::mulScalar(diffVector, -1, diffVector, 3); + MatrixOperations::writeSubmatrix(*diffQuatMatrix, diffVector, 1, 3, 4, 4, 3, 0); + diffQuatMatrix[3][3] = diffScalar; + + std::memcpy(helperQuat, estimatedQuaternionBI, sizeof(helperQuat)); + MatrixOperations::multiply(*diffQuatMatrix, helperQuat, estimatedQuaternionBI, 4, 4, 1); + QuaternionOperations::normalize(estimatedQuaternionBI); + + // Covariance Matrix: P_minus [k+1] = Phi*P_plus [k]*Phi'+G*Q*G' + // Phi [Discrete Error State Transition Matrix] + double errorStateTransition[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; + double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi11L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi11R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi12L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi12R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + + MatrixOperations::multiplyScalar( + *estimatedRotRateSkewed, std::sin(estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm, + *phi11L, 3, 3); + MatrixOperations::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi11R, 3, + 3, 3); + MatrixOperations::multiplyScalar(*phi11R, + (1 - std::cos(estimatedRotRateNorm * timeDelta)) / + (estimatedRotRateNorm * estimatedRotRateNorm), + *phi11R, 3, 3); + MatrixOperations::subtract(*EYE3, *phi11L, *phi11, 3, 3); + MatrixOperations::add(*phi11, *phi11R, *phi11, 3, 3); + + MatrixOperations::multiplyScalar(*EYE3, -timeDelta, *phi12, 3, 3); + MatrixOperations::multiplyScalar(*estimatedRotRateSkewed, + (1 - std::cos(estimatedRotRateNorm * timeDelta)) / + (estimatedRotRateNorm * estimatedRotRateNorm), + *phi12L, 3, 3); + MatrixOperations::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi12R, 3, + 3, 3); + MatrixOperations::multiplyScalar( + *phi12R, + (estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) / + (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm), + *phi12R, 3, 3); + MatrixOperations::add(*phi12, *phi12L, *phi12, 3, 3); + MatrixOperations::subtract(*phi12, *phi12R, *phi12, 3, 3); + + MatrixOperations::writeSubmatrix(*errorStateTransition, *phi11, 3, 3, 6, 6, 0, 0); + MatrixOperations::writeSubmatrix(*errorStateTransition, *phi12, 3, 3, 6, 6, 0, 3); + + // G [Discrete Transfer Matrix] + double transferMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, + {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; + + // Q [Discrete Time Covariance] + double timeCovariance[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + double q11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q21[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + + if (estimatedRotRateNorm * timeDelta < M_PI / 10.) { + MatrixOperations::multiplyScalar( + *EYE3, + (sigmaGyrArw * sigmaGyrArw) * timeDelta + + ((sigmaGyrBs * sigmaGyrBs) * (timeDelta * timeDelta * timeDelta)) / 3., + *q11, 3, 3); + MatrixOperations::multiplyScalar( + *EYE3, -(sigmaGyrBs * sigmaGyrBs * timeDelta * timeDelta) / 2., *q12, 3, 3); + } else { + double q11h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q11h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q12h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + q12h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + + MatrixOperations::multiplyScalar(*EYE3, (sigmaGyrArw * sigmaGyrArw) * timeDelta, *q11, + 3, 3); + MatrixOperations::multiplyScalar(*EYE3, (timeDelta * timeDelta * timeDelta) / 3., + *q11h1, 3, 3); + MatrixOperations::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q11h2, 3, + 3, 3); + MatrixOperations::multiplyScalar( + *q11h2, + (2 * estimatedRotRateNorm * timeDelta - 2 * std::sin(estimatedRotRateNorm * timeDelta) - + ((estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm) * + (timeDelta * timeDelta * timeDelta) / .3)) / + (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm * + estimatedRotRateNorm * estimatedRotRateNorm), + *q11h2, 3, 3); + MatrixOperations::add(*q11h1, *q11h2, *q11h1, 3, 3); + MatrixOperations::multiplyScalar(*q11h1, sigmaGyrBs * sigmaGyrBs, *q11h1, 3, 3); + MatrixOperations::add(*q11, *q11h1, *q11, 3, 3); + + MatrixOperations::multiplyScalar(*EYE3, -(timeDelta * timeDelta) / 2., *q12, 3, 3); + MatrixOperations::multiplyScalar( + *estimatedRotRateSkewed, + (estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) / + (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm), + *q12h1, 3, 3); + MatrixOperations::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q12h2, 3, + 3, 3); + MatrixOperations::multiplyScalar( + *q12h2, + (((estimatedRotRateNorm * estimatedRotRateNorm) * (timeDelta * timeDelta)) / 2. + + std::cos(estimatedRotRateNorm * timeDelta) - 1) / + (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm * + estimatedRotRateNorm), + *q12h2, 3, 3); + MatrixOperations::add(*q12, *q12h1, *q12, 3, 3); + MatrixOperations::subtract(*q12, *q12h2, *q12, 3, 3); + MatrixOperations::multiplyScalar(*q12, sigmaGyrBs * sigmaGyrBs, *q12, 3, 3); + } + + MatrixOperations::transpose(*q12, *q21, 3); + MatrixOperations::multiplyScalar(*EYE3, sigmaGyrBs * sigmaGyrBs * timeDelta, *q22, 3, 3); + + MatrixOperations::writeSubmatrix(*timeCovariance, *q11, 3, 3, 6, 6, 0, 0); + MatrixOperations::writeSubmatrix(*timeCovariance, *q12, 3, 3, 6, 6, 0, 3); + MatrixOperations::writeSubmatrix(*timeCovariance, *q21, 3, 3, 6, 6, 3, 0); + MatrixOperations::writeSubmatrix(*timeCovariance, *q22, 3, 3, 6, 6, 3, 3); + + // Estimated Covariance + double errorStateTransitionTransposed[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}, + estCovarianceMatrixL[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}, + estCovarianceMatrixR[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}, + estCovarianceMatrixHelp[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + + MatrixOperations::transpose(*errorStateTransition, *errorStateTransitionTransposed, 6); + MatrixOperations::multiply(*errorStateTransition, *covAposteriori, + *estCovarianceMatrixHelp, 6, 6, 6); + MatrixOperations::multiply(*estCovarianceMatrixHelp, *errorStateTransitionTransposed, + *estCovarianceMatrixL, 6, 6, 6); + MatrixOperations::multiply(*transferMatrix, *timeCovariance, *estCovarianceMatrixHelp, 6, + 6, 6); + MatrixOperations::multiply(*estCovarianceMatrixHelp, *transferMatrix, + *estCovarianceMatrixR, 6, 6, 6); + MatrixOperations::add(*estCovarianceMatrixL, *estCovarianceMatrixR, + *estimatedCovarianceMatrix, 6, 6); +} + +ReturnValue_t MultiplicativeKalmanFilter::kfFiniteCheck( + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + if (not(VectorOperations::isFinite(estimatedQuaternionBI, 4)) or + not(VectorOperations::isFinite(estimatedRotRate, 3)) or + not(MatrixOperations::isFinite(*estimatedCovarianceMatrix, 6, 6))) { + mekfStatus = MekfStatus::NOT_FINITE; + updateDataSetWithoutData(attitudeEstimationData); + return MEKF_NOT_FINITE; + } + return returnvalue::OK; +} + +void MultiplicativeKalmanFilter::updateDataSetWithoutData( + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + PoolReadGuard pg(attitudeEstimationData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimationData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double)); + attitudeEstimationData->quatMekf.setValid(false); + std::memcpy(attitudeEstimationData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double)); + attitudeEstimationData->satRotRateMekf.setValid(false); + attitudeEstimationData->mekfStatus.value = mekfStatus; + attitudeEstimationData->mekfStatus.setValid(true); } } + +void MultiplicativeKalmanFilter::updateDataSet( + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + PoolReadGuard pg(attitudeEstimationData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimationData->quatMekf.value, estimatedQuaternionBI, 4 * sizeof(double)); + attitudeEstimationData->quatMekf.setValid(true); + std::memcpy(attitudeEstimationData->satRotRateMekf.value, estimatedRotRate, 3 * sizeof(double)); + attitudeEstimationData->satRotRateMekf.setValid(true); + attitudeEstimationData->mekfStatus.value = mekfStatus; + attitudeEstimationData->mekfStatus.setValid(true); + } +} + +void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, + bool valid) { + strData.strQuat.value[0] = qX; + strData.strQuat.value[1] = qX; + strData.strQuat.value[2] = qX; + strData.strQuat.value[3] = qX; + strData.strQuat.valid = valid; +} diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 09fad744..6d475912 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -1,14 +1,16 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ -#include - -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "eive/resultClassIds.h" +#include +#include +#include +#include +#include +#include +#include class MultiplicativeKalmanFilter { - /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by + /* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by * means of the spacecraft attitude sensors * * @note: A description of the used algorithms can be found in the bachelor thesis of Robin @@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter { public: /* @brief: Constructor */ - MultiplicativeKalmanFilter(); + MultiplicativeKalmanFilter(AcsParameters *acsParameters); virtual ~MultiplicativeKalmanFilter(); - ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData); + ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData); - /* @brief: init() - This function initializes the Kalman Filter and will provide the first - * quaternion through the QUEST algorithm - * @param: magneticField_ magnetic field vector in the body frame - * sunDir_ sun direction vector in the body frame - * sunDirJ sun direction vector in the ECI frame - * magFieldJ magnetic field vector in the ECI frame - */ - ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, - const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, - acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); + ReturnValue_t init(const acsctrl::SusDataProcessed *susData, + const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, + acsctrl::AttitudeEstimationData *attitudeEstimationData); - /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter - * for the current step after the initalization - * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame - * rateGYRs_ Estimated satellite rotation rate from the - * Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_ - * sun direction vector in the body frame sunDirJ sun direction vector in the ECI - * frame magFieldJ magnetic field vector in the ECI frame - * outputQuat Stores the calculated quaternion - * outputSatRate Stores the adjusted satellite rate - * @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from - * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model - * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, - * RETURN_OK else - */ - ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, - const bool validGYRs_, const double *magneticField_, - const bool validMagField_, const double *sunDir_, const bool validSS, - const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters); + ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData, + const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, const double timeDelta, + acsctrl::AttitudeEstimationData *attitudeEstimationData); - enum MekfStatus : uint8_t { - UNINITIALIZED = 0, - NO_GYR_DATA = 1, - NO_MODEL_VECTORS = 2, - NO_SUS_MGM_STR_DATA = 3, - COVARIANCE_INVERSION_FAILED = 4, - NOT_FINITE = 5, - INITIALIZED = 10, - RUNNING = 11, - }; + void updateStandardDeviations(const AcsParameters *acsParameters); + + void setStrData(const double qX, const double qY, const double qZ, const double qW, + const bool valid); - // resetting Mekf static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF; static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2); static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3); @@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter { private: static constexpr double ZERO_VEC3[3] = {0, 0, 0}; static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; + static constexpr double ZERO_MAT66[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; + static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; - /*Parameters*/ - double quaternion_STR_SB[4]; + enum MekfStatus : uint8_t { + UNINITIALIZED = 0, + NO_GYR_DATA = 1, + NO_MODEL_VECTORS = 2, + NO_SUS_MGM_STR_DATA = 3, + COVARIANCE_INVERSION_FAILED = 4, + NOT_FINITE = 5, + INITIALIZED = 10, + RUNNING = 11, + }; - /*States*/ - double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/ - double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - uint8_t sensorsAvail = 0; + enum SensorAvailability : uint8_t { + NONE = 0, + SUS_MGM_STR = 1, + SUS_MGM = 2, + SUS_STR = 3, + MGM_STR = 4, + SUS = 5, + MGM = 6, + STR = 7, + }; - /*Outputs*/ - double quatBJ[4]; /* Output Quaternion */ - double biasGYR[3]; /*Between measured and estimated sat Rate*/ - /*Parameter INIT*/ - /*Functions*/ - void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus); - void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, - double quat[4], double satRotRate[3]); + MekfStatus mekfStatus = MekfStatus::UNINITIALIZED; + + struct StrData { + struct StrQuat { + double value[4] = {0, 0, 0, 0}; + bool valid = false; + } strQuat; + } strData; + + // Standard Deviations + double sigmaSus = 0; + double sigmaMgm = 0; + double sigmaStr = 0; + double sigmaGyr = 0; + // sigmaV + double sigmaGyrArw = 0; + // sigmaU + double sigmaGyrBs = 0; + + // Covariance Matrices + double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + + double covAposteriori[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + + // Sensor Availability + SensorAvailability sensorsAvailable = SensorAvailability::NONE; + uint8_t matrixDimensionFactor = 0; + + // Estimated States + double estimatedQuaternionBI[4] = {0, 0, 0, 1}; + double estimatedBiasGyr[3] = {0, 0, 0}; + double estimatedRotRate[3] = {0, 0, 0}; + double estimatedCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + + // Functions + ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData, + const acsctrl::MgmDataProcessed *mgmData, + const acsctrl::GyrDataProcessed *gyrData, + acsctrl::AttitudeEstimationData *attitudeEstimationData); + + void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData, + double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec); + ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain, + acsctrl::AttitudeEstimationData *attitudeEstimationData); + void kfCovAposteriori(double *kalmanGain, double *measSensMatrix); + void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec); + void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff); + + ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData); + + void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData); + void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData); }; #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 5990eca1..038abfdd 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,40 +1,27 @@ #include "Navigation.h" -#include -#include -#include -#include - -Navigation::Navigation() {} +Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {} Navigation::~Navigation() {} -ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, - acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::AttitudeEstimationData *mekfData, - AcsParameters *acsParameters) { - double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, - sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; +ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues, + const acsctrl::GyrDataProcessed *gyrDataProcessed, + const acsctrl::MgmDataProcessed *mgmDataProcessed, + const acsctrl::SusDataProcessed *susDataProcessed, + const double timeDelta, + acsctrl::AttitudeEstimationData *attitudeEstimationData) { + multiplicativeKalmanFilter.setStrData( + sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value, + sensorValues->strSet.caliQx.isValid()); if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { - mekfStatus = multiplicativeKalmanFilter.init( - mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), - susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), - susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData, - acsParameters); + mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed, + gyrDataProcessed, attitudeEstimationData); return mekfStatus; } else { mekfStatus = multiplicativeKalmanFilter.mekfEst( - quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value, - gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, - mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, - susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, - susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, - mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters); + susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData); return mekfStatus; } } @@ -79,3 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) { return sgp4Propagator.initialize(line1, line2); } + +void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) { + multiplicativeKalmanFilter.updateStandardDeviations(acsParameters); +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 9f29c4f2..b944073d 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -5,24 +5,24 @@ #include #include #include -#include #include #include class Navigation { public: - Navigation(); + Navigation(AcsParameters *acsParameters); virtual ~Navigation(); - ReturnValue_t useMekf(ACS::SensorValues *sensorValues, - acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); + ReturnValue_t useMekf(const ACS::SensorValues *sensorValues, + const acsctrl::GyrDataProcessed *gyrDataProcessed, + const acsctrl::MgmDataProcessed *mgmDataProcessed, + const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta, + acsctrl::AttitudeEstimationData *attitudeEstimationData); void resetMekf(acsctrl::AttitudeEstimationData *mekfData); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); + void updateMekfStandardDeviations(const AcsParameters *acsParameters); protected: private: