From 8b23fd3dd261d64090fc5c22c7963f880bc73d1a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Oct 2022 10:29:57 +0200 Subject: [PATCH] fixed int32_t to double warnings. reformats --- .../acs/MultiplicativeKalmanFilter.cpp | 2274 ++++++++--------- .../acs/MultiplicativeKalmanFilter.h | 148 +- mission/controller/acs/Navigation.cpp | 8 +- mission/controller/acs/control/PtgCtrl.cpp | 4 +- 4 files changed, 1186 insertions(+), 1248 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5f7facd2..8cfa0ad3 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -6,1199 +6,1143 @@ */ #include "MultiplicativeKalmanFilter.h" -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" -#include -#include + #include #include #include +#include +#include + +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" /*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(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() { +MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} +void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { + inertiaEIVE = &(acsParameters_->inertiaEIVE); + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ } -void MultiplicativeKalmanFilter::loadAcsParameters( - AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ -} +void MultiplicativeKalmanFilter::reset() {} -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 -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; + 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 - sigmaGyro = 0.1*3.141/180; // acs parameters + 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 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 weigthSun = 1 / (sigmaSun * sigmaSun); // a1 + double weigthMag = 1 / (sigmaMag * sigmaMag); // a2 - 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 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 weigthSun= 1 / (sigmaSun * sigmaSun); //a1 - double weigthMag= 1 / (sigmaMag * sigmaMag); //a2 + 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 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 thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}; + VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); + VectorOperations::add(weightACross, weightBCross, sum2); - 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 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)))); - double thirdAxisSum[3] = {0,0,0}, sum2[3] = {0,0,0}; - VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); - VectorOperations::add(weightACross, weightBCross, sum2); + /*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); - 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)))); + 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]; - /*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); + /*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); - 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); + 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); - } - propagatedQuaternion[0] = initialQuaternion[0]; - propagatedQuaternion[1] = initialQuaternion[1]; - propagatedQuaternion[2] = initialQuaternion[2]; - propagatedQuaternion[3] = initialQuaternion[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); - /*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 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 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; - } + 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 returnvalue::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 returnvalue::OK; + 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, double *outputQuat, double *outputSatRate) { + // Check for GYR Measurements + // AcsParameters mekfEstParams; + // loadAcsParameters(&mekfEstParams); + int MDF = 0; // Matrix Dimension Factor + if (!(*validGYRs_)) { + validMekf = false; + return KALMAN_NO_GYR_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 returnvalue::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(biasGYR, errStateGyroBias, updatedGyroBias, 3); + // Bias GYR State + biasGYR[0] = updatedGyroBias[0]; + biasGYR[1] = updatedGyroBias[1]; + biasGYR[2] = updatedGyroBias[2]; + + /* ----------- PROPAGATION ----------*/ + // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; + // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; + 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(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}}; + + // 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 returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 542db996..f1d2d7a0 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -7,101 +7,95 @@ * @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 + * @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 "config/classIds.h" -#include //uint8_t -#include /*purpose, timeval ?*/ //#include <_timeval.h> #include "AcsParameters.h" -class MultiplicativeKalmanFilter{ -public: - /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - MultiplicativeKalmanFilter(AcsParameters *acsParameters_); - virtual ~MultiplicativeKalmanFilter(); +class MultiplicativeKalmanFilter { + 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 + 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: 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); + /* @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, 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_GYR_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); - // 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; -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; - -/*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_); + /*Outputs*/ + double quatBJ[4]; /* Output Quaternion */ + double biasGYR[3]; /*Between measured and estimated sat Rate*/ + /*Parameter INIT*/ + // double alpha, gamma, beta; + /*Functions*/ + void loadAcsParameters(AcsParameters *acsParameters_); }; - - #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index deb50aa1..2c1596e5 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -36,8 +36,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? - } - else { + } else { multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, @@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out kalmanInit = true; *mekfValid = 0; - // Maybe we need feedback from kalmanfilter to identify if there was a problem with the init - //of kalman filter where does this class know from that kalman filter was not initialized ? + // Maybe we need feedback from kalmanfilter to identify if there was a problem with the + // init of kalman filter where does this class know from that kalman filter was not + // initialized ? } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6201a32a..7160fb47 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -123,7 +123,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl } // calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, @@ -145,7 +145,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; // Conversion to [rad/s] for further calculations