diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp new file mode 100644 index 00000000..5d895566 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -0,0 +1,1205 @@ +/* + * MultiplicativeKalmanFilter.cpp + * + * Created on: 4 Feb 2022 + * Author: rooob + */ + +#include +#include +#include +#include +#include +#include +#include "MultiplicativeKalmanFilter.h" +#include +#include + +/*Initialisation of values for parameters in constructor*/ +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : + initialQuaternion { 0.5, 0.5, 0.5, 0.5 },initialCovarianceMatrix + {{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}}{ + loadAcsParameters(acsParameters_); + +} + +MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() { + +} + +void MultiplicativeKalmanFilter::loadAcsParameters( + AcsParameters *acsParameters_) { + inertiaEIVE = &(acsParameters_->inertiaEIVE); + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ +} + +void MultiplicativeKalmanFilter::reset() { + +} + +void 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) { // valids for "model measurements"? + // check for valid mag/sun + if (*validMagField_ && *validSS && *validSSModel && *validMagModel) { + validInit = true; + //AcsParameters mekfEstParams; + //loadAcsParameters(&mekfEstParams); + // QUEST ALGO ----------------------------------------------------------------------- + double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + + sigmaGyro = 0.1*3.141/180; // acs parameters + + 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}}; + /* vector*transpose(vector)*/ + 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]; + //initialCovarianceMatrix[][] = {{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}}; + } else { + // no initialisation possible, no valid measurements + validInit = false; + } +} + +// --------------- MEKF DISCRETE TIME STEP ------------------------------- +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool *validSTR_, + const double *rateRMUs_, const bool *validRMUs_, + const double *magneticField_, const bool *validMagField_, + const double *sunDir_, const bool *validSS, + const double *sunDirJ, const bool *validSSModel, + const double *magFieldJ,const bool *validMagModel, + double *outputQuat, double *outputSatRate) { + + // Check for RMU Measurements + //AcsParameters mekfEstParams; + //loadAcsParameters(&mekfEstParams); + int MDF = 0; // Matrix Dimension Factor + if (!(*validRMUs_)){ + validMekf = false; + return KALMAN_NO_RMU_MEAS; + } + // Check for Model Calculations + else if (!(*validSSModel) || !(*validMagModel)){ + validMekf = false; + return KALMAN_NO_MODEL; + } + // 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 + validMekf = false; + return RETURN_FAILED; + } + + // If we are here, MEKF will perform + double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + + 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 + MathOperations::skewMatrix(sunEstB, *measSensMatrix11); + MathOperations::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 invResidualCov1[MDF] = {0}; + double invResidualCov[MDF][MDF] = {{0}}; + int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov,*invResidualCov,invResidualCov1,MDF); + if(inversionFailed) + { + validMekf = false; + return KALMAN_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}}; + MathOperations::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); + + outputQuat[0] = quatBJ[0]; + outputQuat[1] = quatBJ[1]; + outputQuat[2] = quatBJ[2]; + outputQuat[3] = quatBJ[3]; + + double updatedGyroBias[3] = {0,0,0}; + VectorOperations::add(biasRMU, errStateGyroBias, updatedGyroBias, 3); + // Bias RMU State + biasRMU[0] = updatedGyroBias[0]; + biasRMU[1] = updatedGyroBias[1]; + biasRMU[2] = updatedGyroBias[2]; + + + /* ----------- PROPAGATION ----------*/ + //double sigmaU = kalmanFilterParameters->sensorNoiseBsRMU; + //double sigmaV = kalmanFilterParameters->sensorNoiseArwRmu; + double sigmaU = 3*3.141/180/3600; + double sigmaV = 3*0.0043*3.141/sqrt(10)/180; + + 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(rateRMUs_, 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}}; + + // Corrected Sat Rate via Bias + outputSatRate[0] = rotRateEst[0]; + outputSatRate[1] = rotRateEst[1]; + outputSatRate[2] = rotRateEst[2]; + + // 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 covQ1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + covQ2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + covQ3[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + transCovQ2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + if(normRotEst*sampleTime< 3.141/10){ + double fact1 = sampleTime*pow(sigmaV,2)+pow(sampleTime,3)*pow(sigmaU,2/3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); + double fact2 = -(0.5*pow(sampleTime,2)*pow(sigmaU,2)); + MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3); + MatrixOperations::transpose(*covQ2, *transCovQ2, 3); + double fact3 = sampleTime*pow(sigmaU,2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3); + + discProcessNoiseCov[0][0] = covQ1[0][0]; + discProcessNoiseCov[0][1] = covQ1[0][1]; + discProcessNoiseCov[0][2] = covQ1[0][2]; + discProcessNoiseCov[0][3] = covQ2[0][0]; + discProcessNoiseCov[0][4] = covQ2[0][1]; + discProcessNoiseCov[0][5] = covQ2[0][2]; + discProcessNoiseCov[1][0] = covQ1[1][0]; + discProcessNoiseCov[1][1] = covQ1[1][1]; + discProcessNoiseCov[1][2] = covQ1[1][2]; + discProcessNoiseCov[1][3] = covQ2[1][0]; + discProcessNoiseCov[1][4] = covQ2[1][1]; + discProcessNoiseCov[1][5] = covQ2[1][2]; + discProcessNoiseCov[2][0] = covQ1[2][0]; + discProcessNoiseCov[2][1] = covQ1[2][1]; + discProcessNoiseCov[2][2] = covQ1[2][2]; + discProcessNoiseCov[2][3] = covQ2[2][0]; + discProcessNoiseCov[2][4] = covQ2[2][1]; + discProcessNoiseCov[2][5] = covQ2[2][2]; + discProcessNoiseCov[3][0] = transCovQ2[0][0]; + discProcessNoiseCov[3][1] = transCovQ2[0][1]; + discProcessNoiseCov[3][2] = transCovQ2[0][2]; + discProcessNoiseCov[3][3] = covQ3[0][0]; + discProcessNoiseCov[3][4] = covQ3[0][1]; + discProcessNoiseCov[3][5] = covQ3[0][2]; + discProcessNoiseCov[4][0] = transCovQ2[1][0]; + discProcessNoiseCov[4][1] = transCovQ2[1][1]; + discProcessNoiseCov[4][2] = transCovQ2[1][2]; + discProcessNoiseCov[4][3] = covQ3[1][0]; + discProcessNoiseCov[4][4] = covQ3[1][1]; + discProcessNoiseCov[4][5] = covQ3[1][2]; + discProcessNoiseCov[5][0] = transCovQ2[2][0]; + discProcessNoiseCov[5][1] = transCovQ2[2][1]; + discProcessNoiseCov[5][2] = transCovQ2[2][2]; + discProcessNoiseCov[5][3] = covQ3[2][0]; + discProcessNoiseCov[5][4] = covQ3[2][1]; + discProcessNoiseCov[5][5] = covQ3[2][2]; + } + else{ + //double fact1 = sampleTime*pow(sigmaV,2); + double covQ11[3][3],covQ12[3][3],covQ13[3][3]; + //MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); + double fact1 = (2*normRotEst+sampleTime-2*sin(normRotEst*sampleTime) + -pow(normRotEst,3)/3*pow(sampleTime,3))/pow(normRotEst,5); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3); + double fact2 = pow(sampleTime,3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3); + MatrixOperations::subtract(*covQ12, *covQ11, *covQ11, 3, 3); + double fact3 = sampleTime*pow(sigmaV,2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3); + MatrixOperations::add(*covQ13, *covQ11, *covQ1, 3, 3); + + double covQ21[3][3], covQ22[3][3], covQ23[3][3]; + double fact4 = (0.5*pow(normRotEst,2)*pow(sampleTime,2) + +cos(normRotEst*sampleTime)-1)/pow(normRotEst,4); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); + double fact5 = 0.5*pow(sampleTime,2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); + MatrixOperations::add(*covQ22, *covQ21, *covQ21, 3, 3); + double fact6 = normRotEst*sampleTime-sin(normRotEst*sampleTime)/pow(normRotEst,3); + MatrixOperations::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3); + MatrixOperations::subtract(*covQ23, *covQ21, *covQ21, 3, 3); + double fact7 = pow(sigmaU,2); + MatrixOperations::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3); + + MatrixOperations::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); + + discProcessNoiseCov[0][0] = covQ1[0][0]; + discProcessNoiseCov[0][1] = covQ1[0][1]; + discProcessNoiseCov[0][2] = covQ1[0][2]; + discProcessNoiseCov[0][3] = covQ2[0][0]; + discProcessNoiseCov[0][4] = covQ2[0][1]; + discProcessNoiseCov[0][5] = covQ2[0][2]; + discProcessNoiseCov[1][0] = covQ1[1][0]; + discProcessNoiseCov[1][1] = covQ1[1][1]; + discProcessNoiseCov[1][2] = covQ1[1][2]; + discProcessNoiseCov[1][3] = covQ2[1][0]; + discProcessNoiseCov[1][4] = covQ2[1][1]; + discProcessNoiseCov[1][5] = covQ2[1][2]; + discProcessNoiseCov[2][0] = covQ1[2][0]; + discProcessNoiseCov[2][1] = covQ1[2][1]; + discProcessNoiseCov[2][2] = covQ1[2][2]; + discProcessNoiseCov[2][3] = covQ2[2][0]; + discProcessNoiseCov[2][4] = covQ2[2][1]; + discProcessNoiseCov[2][5] = covQ2[2][2]; + discProcessNoiseCov[3][0] = covQ2[0][0]; + discProcessNoiseCov[3][1] = covQ2[0][1]; + discProcessNoiseCov[3][2] = covQ2[0][2]; + discProcessNoiseCov[3][3] = covQ3[0][0]; + discProcessNoiseCov[3][4] = covQ3[0][1]; + discProcessNoiseCov[3][5] = covQ3[0][2]; + discProcessNoiseCov[4][0] = covQ2[1][0]; + discProcessNoiseCov[4][1] = covQ2[1][1]; + discProcessNoiseCov[4][2] = covQ2[1][2]; + discProcessNoiseCov[4][3] = covQ3[1][0]; + discProcessNoiseCov[4][4] = covQ3[1][1]; + discProcessNoiseCov[4][5] = covQ3[1][2]; + discProcessNoiseCov[5][0] = covQ2[2][0]; + discProcessNoiseCov[5][1] = covQ2[2][1]; + discProcessNoiseCov[5][2] = covQ2[2][2]; + discProcessNoiseCov[5][3] = covQ3[2][0]; + discProcessNoiseCov[5][4] = covQ3[2][1]; + discProcessNoiseCov[5][5] = covQ3[2][2]; + } + + // State Transition Matrix phi + double phi1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + phi2[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[3][3],phi12[3][3]; + double fact1 = sin(normRotEst*sampleTime); + MatrixOperations::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); + double fact2 = (1-cos(normRotEst*sampleTime))/pow(normRotEst,2); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi12, fact2, *phi12, 3, 3); + MatrixOperations::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); + MatrixOperations::add(*phi11, *phi12, *phi1, 3, 3); + + double phi21[3][3],phi22[3][3]; + MatrixOperations::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); + MatrixOperations::subtract(*phi21, *phi22, *phi21, 3, 3); + double fact3 = (normRotEst*sampleTime-sin(normRotEst*sampleTime)/pow(normRotEst,3)); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi22, fact3, *phi22, 3, 3); + MatrixOperations::subtract(*phi21, *phi22, *phi2, 3, 3); + + phi[0][0] = phi1[0][0]; + phi[0][1] = phi1[0][1]; + phi[0][2] = phi1[0][2]; + phi[0][3] = phi2[0][0]; + phi[0][4] = phi2[0][1]; + phi[0][5] = phi2[0][2]; + phi[1][0] = phi1[1][0]; + phi[1][1] = phi1[1][1]; + phi[1][2] = phi1[1][2]; + phi[1][3] = phi2[1][0]; + phi[1][4] = phi2[1][1]; + phi[1][5] = phi2[1][2]; + phi[2][0] = phi1[2][0]; + phi[2][1] = phi1[2][1]; + phi[2][2] = phi1[2][2]; + phi[2][3] = phi2[2][0]; + phi[2][4] = phi2[2][1]; + phi[2][5] = phi2[2][2]; + + // Propagated Quaternion + double rotSin[3] = {0,0,0}, + omega1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + double rotCos = cos(0.5*normRotEst*sampleTime); + double sinFac = sin(0.5*normRotEst*sampleTime)/normRotEst; + VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); + + double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::skewMatrix(rotSin, *skewSin); + + MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); + MatrixOperations::subtract(*omega1, *skewSin, *omega1, 3, 3); + double omega[4][4] = {{omega1[0][0],omega1[0][1],omega1[0][2],rotSin[0]}, + {omega1[1][0],omega1[1][1],omega1[1][2],rotSin[1]}, + {omega1[2][0],omega1[2][1],omega1[2][2],rotSin[2]}, + {-rotSin[0],-rotSin[1],-rotSin[2],rotCos}}; + MatrixOperations::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); + + // Update Covariance Matrix + double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], + transPhi[6][6]; + MatrixOperations::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); + MatrixOperations::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); + MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); + + MatrixOperations::transpose(*phi, *transPhi, 6); + MatrixOperations::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6); + MatrixOperations::multiply(*phi, *cov2, *cov2, 6, 6, 6); + + MatrixOperations::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6); + validMekf = true; + + + // Discrete Time Step + + // Check for new data in measurement -> SensorProcessing ? + + return RETURN_OK; +} diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h new file mode 100644 index 00000000..d4370ad3 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -0,0 +1,107 @@ +/* + * MultiplicativeKalmanFilter.h + * + * Created on: 4 Feb 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by + * means of the spacecraft attitude sensors + * + * @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 + */ + +#ifndef MULTIPLICATIVEKALMANFILTER_H_ +#define MULTIPLICATIVEKALMANFILTER_H_ + +#include //uint8_t +#include /*purpose, timeval ?*/ +#include "acs/config/classIds.h" +//#include <_timeval.h> + +#include "AcsParameters.h" + +class MultiplicativeKalmanFilter : public HasReturnvaluesIF { +public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + virtual ~MultiplicativeKalmanFilter(); + + void reset(); // NOT YET DEFINED - should only reset all mekf variables + + /* @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 + */ + void 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); + + /* @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 + * rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [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_RMU_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 *rateRMUs_, const bool *validRMUs_, + const double *magneticField_, const bool *validMagField_, + const double *sunDir_, const bool *validSS, + const double *sunDirJ, const bool *validSSModel, + const double *magFieldJ,const bool *validMagModel, + double *outputQuat, double *outputSatRate); + + + // Declaration of Events (like init) and memberships + //static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h) + //static const Event RESET = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be + // resetting Mekf + static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; + static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); + +private: +/*Parameters*/ + AcsParameters::InertiaEIVE* inertiaEIVE; + AcsParameters::KalmanFilterParameters* kalmanFilterParameters; + double quaternion_STR_SB[4]; + bool validInit; + double sampleTime = 0.1; + + +/*States*/ + double initialQuaternion[4]; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6];/*after reset?QUEST*/ + double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ + bool validMekf; + uint8_t sensorsAvail; + +/*Outputs*/ + double quatBJ[4]; /* Output Quaternion */ + double biasRMU[3]; /*Between measured and estimated sat Rate*/ +/*Parameter INIT*/ + //double alpha, gamma, beta; +/*Functions*/ + void loadAcsParameters(AcsParameters *acsParameters_); +}; + + + +#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */