eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity
236ca64de3
eive-obsw/mission/controller/acs/MultiplicativeKalmanFilter.cpp
meggert 236ca64de3
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details
whole lot of cleanup
2024-02-12 14:43:34 +01:00

1129 lines
48 KiB
C++
Raw Blame History

#include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <stdio.h>
#include <string.h>
#include <cmath>
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); // r1
double thirdAxis_B[3] = {0, 0, 0}, thirdAxis_J[3] = {0, 0, 0};
VectorOperations<double>::cross(normSunJ, normMagJ, thirdAxis_J);
VectorOperations<double>::cross(normSunB, normMagB, thirdAxis_B);
VectorOperations<double>::normalize(thirdAxis_J, thirdAxis_J, 3); // rx
VectorOperations<double>::normalize(thirdAxis_B, thirdAxis_B, 3); // bx
double weigthSun = 1 / (sigmaSun * sigmaSun); // a1
double weigthMag = 1 / (sigmaMag * sigmaMag); // a2
double a[3] = {0, 0, 0}, b[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSunB, weigthSun, a, 3); // a
VectorOperations<double>::mulScalar(normMagB, weigthMag, b, 3); // b
double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0},
weigthCrossSum[3] = {0, 0, 0};
VectorOperations<double>::cross(thirdAxis_B, thirdAxis_J,
thirdAxisCross); // cross(bx,rx)
VectorOperations<double>::cross(a, normSunJ, weightACross);
VectorOperations<double>::cross(b, normMagJ, weightBCross);
VectorOperations<double>::add(weightACross, weightBCross, weigthCrossSum);
double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0};
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, thirdAxisSum);
VectorOperations<double>::add(weightACross, weightBCross, sum2);
double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0;
alpha = (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J)) *
(VectorOperations<double>::dot(a, normSunJ) +
VectorOperations<double>::dot(b, normMagJ)) +
VectorOperations<double>::dot(thirdAxisCross, weigthCrossSum);
beta = VectorOperations<double>::dot(thirdAxisSum, sum2);
gamma = sqrt(alpha * alpha + beta * beta);
constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) *
(1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J))));
constMin = 1 / (2 * sqrt(gamma * (gamma - alpha) *
(1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J))));
/*Quaternion Computation*/
double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0};
if (alpha >= 0) {
VectorOperations<double>::mulScalar(thirdAxisCross, gamma + alpha, var1, 3);
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, var2, 3);
VectorOperations<double>::mulScalar(var2, beta, var2, 3);
VectorOperations<double>::add(var1, var2, quat);
for (int i = 0; i < 3; i++) {
initialQuaternion[i] = quat[i];
}
initialQuaternion[3] =
(gamma + alpha) * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4);
} else {
double diffGammaAlpha = gamma - alpha;
VectorOperations<double>::mulScalar(thirdAxisCross, beta, var1, 3);
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, var2, 3);
VectorOperations<double>::mulScalar(var2, diffGammaAlpha, var2, 3);
VectorOperations<double>::add(var1, var2, quat);
for (int i = 0; i < 3; i++) {
initialQuaternion[i] = quat[i];
}
initialQuaternion[3] = beta * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion, constMin, initialQuaternion, 4);
}
propagatedQuaternion[0] = initialQuaternion[0];
propagatedQuaternion[1] = initialQuaternion[1];
propagatedQuaternion[2] = initialQuaternion[2];
propagatedQuaternion[3] = initialQuaternion[3];
/*Initial Covariance Matrix------------------------------------ */
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0},
magEstB[3] = {0, 0, 0};
QuaternionOperations::toDcm(initialQuaternion, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
double matrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3);
double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0;
MatrixOperations<double>::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3);
MatrixOperations<double>::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3);
MatrixOperations<double>::add(*matrixSunMag, *matrixMagSun, *partA, 3, 3);
double factor = weigthMag * weigthSun * VectorOperations<double>::dot(sunEstB, magEstB);
MatrixOperations<double>::multiplyScalar(*partA, factor, *partA, 3, 3);
MatrixOperations<double>::add(*partA, *matrixSun, *partA, 3, 3);
MatrixOperations<double>::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<double>::cross(sunEstB, magEstB, crossSunMag);
partB = 1 / (weigthMag * weigthSun * pow(VectorOperations<double>::norm(crossSunMag, 3), 2));
MatrixOperations<double>::multiplyScalar(*partA, partB, *partA, 3, 3);
MatrixOperations<double>::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3);
factor = 1 / (weigthMag + weigthMag);
double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3);
double initGyroCov[3][3] = {
{pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}};
initialCovarianceMatrix[0][0] = questCovMatrix[0][0];
initialCovarianceMatrix[0][1] = questCovMatrix[0][1];
initialCovarianceMatrix[0][2] = questCovMatrix[0][2];
initialCovarianceMatrix[0][3] = 0;
initialCovarianceMatrix[0][4] = 0;
initialCovarianceMatrix[0][5] = 0;
initialCovarianceMatrix[1][0] = questCovMatrix[1][0];
initialCovarianceMatrix[1][1] = questCovMatrix[1][1];
initialCovarianceMatrix[1][2] = questCovMatrix[1][2];
initialCovarianceMatrix[1][3] = 0;
initialCovarianceMatrix[1][4] = 0;
initialCovarianceMatrix[1][5] = 0;
initialCovarianceMatrix[2][0] = questCovMatrix[2][0];
initialCovarianceMatrix[2][1] = questCovMatrix[2][1];
initialCovarianceMatrix[2][2] = questCovMatrix[2][2];
initialCovarianceMatrix[2][3] = 0;
initialCovarianceMatrix[2][4] = 0;
initialCovarianceMatrix[2][5] = 0;
initialCovarianceMatrix[3][0] = 0;
initialCovarianceMatrix[3][1] = 0;
initialCovarianceMatrix[3][2] = 0;
initialCovarianceMatrix[3][3] = initGyroCov[0][0];
initialCovarianceMatrix[3][4] = initGyroCov[0][1];
initialCovarianceMatrix[3][5] = initGyroCov[0][2];
initialCovarianceMatrix[4][0] = 0;
initialCovarianceMatrix[4][1] = 0;
initialCovarianceMatrix[4][2] = 0;
initialCovarianceMatrix[4][3] = initGyroCov[1][0];
initialCovarianceMatrix[4][4] = initGyroCov[1][1];
initialCovarianceMatrix[4][5] = initGyroCov[1][2];
initialCovarianceMatrix[5][0] = 0;
initialCovarianceMatrix[5][1] = 0;
initialCovarianceMatrix[5][2] = 0;
initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
return MEKF_INITIALIZED;
} else {
// no initialisation possible, no valid measurements
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
}
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
return MEKF_NO_GYR_DATA;
}
// Check for Model Calculations
else if (!validSSModel || !validMagModel) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
return MEKF_NO_MODEL_VECTORS;
}
// Check Measurements available from SS, MAG, STR
if (validSS && validMagField_ && validSTR_) {
sensorsAvail = 1;
MDF = 9;
} else if (validSS && validMagField_ && !validSTR_) {
sensorsAvail = 2;
MDF = 6;
} else if (validSS && !validMagField_ && validSTR_) {
sensorsAvail = 3;
MDF = 6;
} else if (!validSS && validMagField_ && validSTR_) {
sensorsAvail = 4;
MDF = 6;
} else if (validSS && !validMagField_ && !(validSTR_)) {
sensorsAvail = 5;
MDF = 3;
} else if (!validSS && validMagField_ && !validSTR_) {
sensorsAvail = 6;
MDF = 3;
} else if (!validSS && !validMagField_ && validSTR_) {
sensorsAvail = 7;
MDF = 3;
} else {
sensorsAvail = 8; // no measurements
updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
return returnvalue::FAILED;
}
// If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
VectorOperations<double>::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<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
double quatEstErr[3] = {0, 0, 0};
// Measurement Sensitivity Matrix
double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss
double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag
double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
MatrixOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
MatrixOperations<double>::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<double>::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1,
6, 6, MDF);
MatrixOperations<double>::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF);
// negative values, restrictions ?
// (H * P * H' + R)
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
// <<INVERSE residualCov HIER>>
double invResidualCov[MDF][MDF] = {{0}};
ReturnValue_t result =
MatrixOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (result != returnvalue::OK) {
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}
// [K = P * H' / (H * P * H' + R)]
MatrixOperations<double>::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF,
MDF);
MatrixOperations<double>::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<double>::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF);
MatrixOperations<double>::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6);
MatrixOperations<double>::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6);
// Error State Vector
double errStateVec[6] = {0, 0, 0, 0, 0, 0};
double errStateVec1[MDF] = {0};
VectorOperations<double>::subtract(measVec, measEstVec, errStateVec1, MDF);
MatrixOperations<double>::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1);
double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
// State Vector Elements
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3);
MatrixOperations<double>::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<double>::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1);
VectorOperations<double>::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4);
VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4);
VectorOperations<double>::normalize(quatBJ, quatBJ, 4);
double updatedGyroBias[3] = {0, 0, 0};
VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
// Bias GYR State
biasGYR[0] = updatedGyroBias[0];
biasGYR[1] = updatedGyroBias[1];
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double rotRateEst[3] = {0, 0, 0};
VectorOperations<double>::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3);
double normRotEst = VectorOperations<double>::norm(rotRateEst, 3);
double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]},
{rotRateEst[2], 0, -rotRateEst[0]},
{-rotRateEst[1], rotRateEst[0], 0}};
// Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime +
1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) +
cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
MatrixOperations<double>::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3);
MatrixOperations<double>::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3);
MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 =
(2 * normRotEst * acsParameters->onBoardParams.sampleTime -
2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3);
MatrixOperations<double>::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3);
}
discProcessNoiseCov[0][0] = covQ11[0][0];
discProcessNoiseCov[0][1] = covQ11[0][1];
discProcessNoiseCov[0][2] = covQ11[0][2];
discProcessNoiseCov[0][3] = covQ12[0][0];
discProcessNoiseCov[0][4] = covQ12[0][1];
discProcessNoiseCov[0][5] = covQ12[0][2];
discProcessNoiseCov[1][0] = covQ11[1][0];
discProcessNoiseCov[1][1] = covQ11[1][1];
discProcessNoiseCov[1][2] = covQ11[1][2];
discProcessNoiseCov[1][3] = covQ12[1][0];
discProcessNoiseCov[1][4] = covQ12[1][1];
discProcessNoiseCov[1][5] = covQ12[1][2];
discProcessNoiseCov[2][0] = covQ11[2][0];
discProcessNoiseCov[2][1] = covQ11[2][1];
discProcessNoiseCov[2][2] = covQ11[2][2];
discProcessNoiseCov[2][3] = covQ12[2][0];
discProcessNoiseCov[2][4] = covQ12[2][1];
discProcessNoiseCov[2][5] = covQ12[2][2];
discProcessNoiseCov[3][0] = covQ12trans[0][0];
discProcessNoiseCov[3][1] = covQ12trans[0][1];
discProcessNoiseCov[3][2] = covQ12trans[0][2];
discProcessNoiseCov[3][3] = covQ22[0][0];
discProcessNoiseCov[3][4] = covQ22[0][1];
discProcessNoiseCov[3][5] = covQ22[0][2];
discProcessNoiseCov[4][0] = covQ12trans[1][0];
discProcessNoiseCov[4][1] = covQ12trans[1][1];
discProcessNoiseCov[4][2] = covQ12trans[1][2];
discProcessNoiseCov[4][3] = covQ22[1][0];
discProcessNoiseCov[4][4] = covQ22[1][1];
discProcessNoiseCov[4][5] = covQ22[1][2];
discProcessNoiseCov[5][0] = covQ12trans[2][0];
discProcessNoiseCov[5][1] = covQ12trans[2][1];
discProcessNoiseCov[5][2] = covQ12trans[2][2];
discProcessNoiseCov[5][3] = covQ22[2][0];
discProcessNoiseCov[5][4] = covQ22[2][1];
discProcessNoiseCov[5][5] = covQ22[2][2];
// State Transition Matrix phi
double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 =
(1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
MatrixOperations<double>::add(*phi11_01, *phi11_2, *phi11, 3, 3);
double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3,
acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3);
double fact12_2 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
MatrixOperations<double>::subtract(*phi12_01, *phi12_2, *phi12, 3, 3);
phi[0][0] = phi11[0][0];
phi[0][1] = phi11[0][1];
phi[0][2] = phi11[0][2];
phi[0][3] = phi12[0][0];
phi[0][4] = phi12[0][1];
phi[0][5] = phi12[0][2];
phi[1][0] = phi11[1][0];
phi[1][1] = phi11[1][1];
phi[1][2] = phi11[1][2];
phi[1][3] = phi12[1][0];
phi[1][4] = phi12[1][1];
phi[1][5] = phi12[1][2];
phi[2][0] = phi11[2][0];
phi[2][1] = phi11[2][1];
phi[2][2] = phi11[2][2];
phi[2][3] = phi12[2][0];
phi[2][4] = phi12[2][1];
phi[2][5] = phi12[2][2];
// Propagated Quaternion
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3);
double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]},
{subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]},
{subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]},
{-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix
double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6];
MatrixOperations<double>::transpose(*phi, *transPhi, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov0, *cov0, 6, 6, 6);
MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
if (not(VectorOperations<double>::isFinite(propagatedQuaternion, 4)) ||
not(MatrixOperations<double>::isFinite(initialQuaternion, 6, 6))) {
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
return MEKF_NOT_FINITE;
}
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return MEKF_RUNNING;
}
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double));
mekfData->quatMekf.setValid(false);
std::memcpy(mekfData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double));
mekfData->satRotRateMekf.setValid(false);
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, false);
}
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus, double quat[4],
double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, true);
}
}
}
Reference in New Issue View Git Blame Copy Permalink