eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 119 Wiki Activity

corrected calculations of Q and Phi

Browse Source
This commit is contained in:
Marius Eggert 2022-12-12 11:52:46 +01:00
parent e9dd0f8f6d
commit f35c42aead
1 changed files with 139 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

300
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -14,6 +14,8 @@
#include <stdio.h>
#include <string.h>
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
@ -932,6 +934,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
sif::debug << "errQ=[" << errStateQuat[0] << "," << errStateQuat[1] << "," << errStateQuat[2]
<< "]" << std::endl;
sif::debug << "errW_bias=[" << errStateGyroBias[0] << "," << errStateGyroBias[1] << ","
<< errStateGyroBias[2] << "]" << std::endl;
// State Vector Elements
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
@ -968,179 +974,150 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
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};
sif::debug << "MEKF rateGYR= " << rateGYRs_[0] << " " << rateGYRs_[1] << " " << rateGYRs_[2]
<< std::endl;
sif::debug << "MEKF bias= " << updatedGyroBias[0] << " " << updatedGyroBias[1] << " "
<< updatedGyroBias[2] << std::endl;
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}};
// ToDo
// 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<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3);
MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3);
double fact3 = sampleTime * pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
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 * sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 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];
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
// double fact1 = sampleTime*pow(sigmaV,2);
double covQ11[3][3], covQ12[3][3], covQ13[3][3];
// MatrixOperations<double>::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<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
double fact2 = pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3);
MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3);
double fact3 = sampleTime * pow(sigmaV, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3);
MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
double fact22 = pow(sigmaU, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 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) /
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3);
double fact5 = 0.5 * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3);
MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3);
double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3);
MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3);
double fact7 = pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3);
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);
MatrixOperations<double>::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];
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(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];
// ToDo
// 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}},
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[3][3], phi12[3][3];
double fact1 = sin(normRotEst * sampleTime);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3);
double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3);
MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3);
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 = (1 - cos(normRotEst * 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 phi21[3][3], phi22[3][3];
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3);
double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 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, sampleTime, *phi12_1, 3, 3);
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * 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] = 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];
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}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotSin[3] = {0, 0, 0}, rotCosMat[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<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
@ -1148,25 +1125,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3);
MatrixOperations<double>::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]},
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 cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6];
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>::transpose(*phi, *transPhi, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov2, *cov2, 6, 6, 6);
MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6);
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true;
// Discrete Time Step