637 lines
34 KiB
C++
637 lines
34 KiB
C++
#include "MultiplicativeKalmanFilter.h"
|
|
|
|
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters) {
|
|
updateStandardDeviations(acsParameters);
|
|
}
|
|
|
|
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
|
|
|
|
void MultiplicativeKalmanFilter::updateStandardDeviations(const AcsParameters *acsParameters) {
|
|
sigmaSus = acsParameters->kalmanFilterParameters.sensorNoiseSus;
|
|
sigmaMgm = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
|
|
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
|
|
sigmaGyr = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
|
|
sigmaGyrArw = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
|
|
sigmaGyrBs = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;
|
|
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, sigmaSus * sigmaSus, *covSus, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, sigmaMgm * sigmaMgm, *covMgm, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, sigmaStr * sigmaStr, *covStr, 3, 3);
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::reset(
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
std::memcpy(estimatedQuaternionBI, UNIT_QUAT, sizeof(estimatedQuaternionBI));
|
|
std::memcpy(estimatedBiasGyr, ZERO_VEC3, sizeof(estimatedBiasGyr));
|
|
std::memcpy(estimatedCovarianceMatrix, ZERO_MAT66, sizeof(estimatedCovarianceMatrix));
|
|
mekfStatus = MekfStatus::UNINITIALIZED;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_UNINITIALIZED;
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::init(
|
|
const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
// check for valid QUEST quaternion
|
|
if (attitudeEstimationData->quatQuest.isValid()) {
|
|
// Initial Quaternion
|
|
std::memcpy(estimatedQuaternionBI, attitudeEstimationData->quatQuest.value,
|
|
sizeof(estimatedQuaternionBI));
|
|
|
|
// Initial Covariance
|
|
if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
|
|
gyrData->gyrVecTot.isValid()) {
|
|
// QUEST Covariance
|
|
double normSunI[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0},
|
|
normMagEstB[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(susData->sunIjkModel.value, normSunI, 3);
|
|
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMagI, 3);
|
|
QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB);
|
|
QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB);
|
|
|
|
double matrixSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
matrixMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
matrixSusMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
matrixMgmSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::multiply(normSunEstB, normSunEstB, *matrixSus, 3, 1, 3);
|
|
MatrixOperations<double>::multiply(normMagEstB, normMagEstB, *matrixMgm, 3, 1, 3);
|
|
MatrixOperations<double>::multiply(normSunEstB, normMagEstB, *matrixSusMgm, 3, 1, 3);
|
|
MatrixOperations<double>::multiply(normMagEstB, normSunEstB, *matrixMgmSus, 3, 1, 3);
|
|
|
|
double factorSus = 1. / (sigmaSus * sigmaSus);
|
|
double factorMgm = 1. / (sigmaMgm * sigmaMgm);
|
|
|
|
double covQuest[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::multiplyScalar(*matrixSus, factorSus * factorSus, *matrixSus, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*matrixMgm, factorMgm * factorMgm, *matrixMgm, 3, 3);
|
|
MatrixOperations<double>::add(*matrixSusMgm, *matrixMgmSus, *covQuest, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*covQuest,
|
|
factorSus * factorMgm * VectorOperations<double>::dot(normSunEstB, normMagEstB),
|
|
*covQuest, 3, 3);
|
|
MatrixOperations<double>::add(*covQuest, *matrixSus, *covQuest, 3, 3);
|
|
MatrixOperations<double>::add(*covQuest, *matrixMgm, *covQuest, 3, 3);
|
|
|
|
double crossSunMag[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(normSunEstB, normMagEstB, crossSunMag);
|
|
double normCrossSunMag = VectorOperations<double>::norm(crossSunMag, 3);
|
|
double factor = factorSus * factorMgm * normCrossSunMag * normCrossSunMag;
|
|
|
|
MatrixOperations<double>::multiplyScalar(*covQuest, 1. / factor, *covQuest, 3, 3);
|
|
MatrixOperations<double>::add(*EYE3, *covQuest, *covQuest, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*covQuest, 1. / (factorSus + factorMgm), *covQuest,
|
|
3, 3);
|
|
|
|
// GYR Covariance
|
|
double covGyr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, sigmaGyr, *covGyr, 3, 3);
|
|
|
|
// Combine Covariances
|
|
MatrixOperations<double>::writeSubmatrix(*estimatedCovarianceMatrix, *covQuest, 3, 3, 6, 6, 0,
|
|
0);
|
|
MatrixOperations<double>::writeSubmatrix(*estimatedCovarianceMatrix, *covGyr, 3, 3, 6, 6, 3,
|
|
3);
|
|
|
|
mekfStatus = MekfStatus::INITIALIZED;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_INITIALIZED;
|
|
}
|
|
}
|
|
// no initialisation possible, no valid measurements
|
|
mekfStatus = MekfStatus::UNINITIALIZED;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_UNINITIALIZED;
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
|
const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
ReturnValue_t result = checkAvailableSensors(susData, mgmData, gyrData, attitudeEstimationData);
|
|
if (result != returnvalue::OK) {
|
|
reset(attitudeEstimationData);
|
|
return result;
|
|
}
|
|
|
|
double measSensMatrix[matrixDimensionFactor][6] = {{0}},
|
|
measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
|
measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0};
|
|
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
|
|
|
|
double kalmanGain[6][matrixDimensionFactor] = {{0}};
|
|
result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
|
|
if (result != returnvalue::OK) {
|
|
reset(attitudeEstimationData);
|
|
return result;
|
|
}
|
|
|
|
kfCovAposteriori(*kalmanGain, *measSensMatrix);
|
|
kfStateAposteriori(*kalmanGain, measVec, estVec);
|
|
kfPropagate(gyrData, timeDelta);
|
|
|
|
result = kfFiniteCheck(attitudeEstimationData);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
mekfStatus = MekfStatus::RUNNING;
|
|
updateDataSet(attitudeEstimationData);
|
|
return MEKF_RUNNING;
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::checkAvailableSensors(
|
|
const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
// Check for GYR Measurements
|
|
if (not gyrData->gyrVecTot.isValid()) {
|
|
mekfStatus = MekfStatus::NO_GYR_DATA;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_NO_GYR_DATA;
|
|
}
|
|
// Check for Model Calculations
|
|
if (not susData->sunIjkModel.isValid() or not mgmData->magIgrfModel.isValid()) {
|
|
mekfStatus = MekfStatus::NO_MODEL_VECTORS;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_NO_MODEL_VECTORS;
|
|
}
|
|
// Check Measurements available from SUS, MGM, STR
|
|
if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
|
|
not strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::NONE;
|
|
mekfStatus = MekfStatus::NO_SUS_MGM_STR_DATA;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_NO_SUS_MGM_STR_DATA;
|
|
}
|
|
if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::SUS_MGM_STR;
|
|
matrixDimensionFactor = 9;
|
|
} else if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
|
|
not strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::SUS_MGM;
|
|
matrixDimensionFactor = 6;
|
|
} else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
|
|
strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::SUS_STR;
|
|
matrixDimensionFactor = 6;
|
|
} else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
|
|
strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::MGM_STR;
|
|
matrixDimensionFactor = 6;
|
|
} else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
|
|
not strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::SUS;
|
|
matrixDimensionFactor = 3;
|
|
} else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
|
|
not strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::MGM;
|
|
matrixDimensionFactor = 3;
|
|
} else if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
|
|
strData.strQuat.valid) {
|
|
sensorsAvailable = SensorAvailability::STR;
|
|
matrixDimensionFactor = 3;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::kfUpdate(const acsctrl::SusDataProcessed *susData,
|
|
const acsctrl::MgmDataProcessed *mgmData,
|
|
double *measSensMatrix, double *measCovMatrix,
|
|
double *measVec, double *estVec) {
|
|
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0},
|
|
normSunI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0}, normMagEstB[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMagB, 3); // b2
|
|
VectorOperations<double>::normalize(susData->susVecTot.value, normSunB, 3); // b1
|
|
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMagI, 3); // r2
|
|
VectorOperations<double>::normalize(susData->sunIjkModel.value, normSunI, 3); // r1
|
|
QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB);
|
|
QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB);
|
|
|
|
// Measurement Sensitivity Matrix
|
|
double measSensMatrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // H11
|
|
double measSensMatrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // H22
|
|
double measSensMatrixStr[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // H33
|
|
MatrixOperations<double>::skewMatrix(normSunEstB, *measSensMatrixSun);
|
|
MatrixOperations<double>::skewMatrix(normMagEstB, *measSensMatrixMag);
|
|
|
|
double measVecQuat[3] = {0, 0, 0}, quatEstErr[3] = {0, 0, 0};
|
|
if (strData.strQuat.valid) {
|
|
double quatError[4] = {0, 0, 0, 0};
|
|
double invPropagatedQuat[4] = {0, 0, 0, 0};
|
|
QuaternionOperations::inverse(estimatedQuaternionBI, invPropagatedQuat);
|
|
QuaternionOperations::multiply(strData.strQuat.value, invPropagatedQuat, quatError);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
measVecQuat[i] = 2. * quatError[i] / quatError[3];
|
|
}
|
|
}
|
|
|
|
switch (sensorsAvailable) {
|
|
case SensorAvailability::SUS_MGM_STR:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
|
|
matrixDimensionFactor, 6, 3, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
|
|
matrixDimensionFactor, 6, 6, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 3, 3);
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 6, 6);
|
|
|
|
std::memcpy(measVec, normSunB, sizeof(normSunB));
|
|
std::memcpy(measVec + 3, normMagB, sizeof(normMagB));
|
|
std::memcpy(measVec + 6, measVecQuat, sizeof(measVecQuat));
|
|
|
|
std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
|
|
std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB));
|
|
std::memcpy(estVec + 6, quatEstErr, sizeof(quatEstErr));
|
|
return;
|
|
case SensorAvailability::SUS_MGM:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
|
|
matrixDimensionFactor, 6, 3, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 3, 3);
|
|
|
|
std::memcpy(measVec, normSunB, sizeof(normSunB));
|
|
std::memcpy(measVec + 3, normMagB, sizeof(normMagB));
|
|
|
|
std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
|
|
std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB));
|
|
return;
|
|
case SensorAvailability::SUS_STR:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
|
|
matrixDimensionFactor, 6, 3, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 3, 3);
|
|
|
|
std::memcpy(measVec, normSunB, sizeof(normSunB));
|
|
std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat));
|
|
|
|
std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
|
|
std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr));
|
|
return;
|
|
case SensorAvailability::MGM_STR:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
|
|
matrixDimensionFactor, 6, 3, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 3, 3);
|
|
|
|
std::memcpy(measVec, normMagB, sizeof(normMagB));
|
|
std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat));
|
|
|
|
std::memcpy(estVec, normMagEstB, sizeof(normMagEstB));
|
|
std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr));
|
|
return;
|
|
case SensorAvailability::SUS:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
|
|
std::memcpy(measVec, normSunB, sizeof(normSunB));
|
|
|
|
std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
|
|
return;
|
|
case SensorAvailability::MGM:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
|
|
std::memcpy(measVec, normMagB, sizeof(normMagB));
|
|
|
|
std::memcpy(estVec, normMagEstB, sizeof(normMagEstB));
|
|
return;
|
|
case SensorAvailability::STR:
|
|
MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
|
|
matrixDimensionFactor, 6, 0, 0);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
|
|
matrixDimensionFactor, 0, 0);
|
|
|
|
std::memcpy(measVec, measVecQuat, sizeof(measVecQuat));
|
|
|
|
std::memcpy(estVec, quatEstErr, sizeof(quatEstErr));
|
|
return;
|
|
default:
|
|
sif::error << "MEKF::Invalid SensorAvailability Flag" << std::endl;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::kfGain(
|
|
double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
// Kalman Gain: K = P * H' / (H * P * H' + R)
|
|
double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
|
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
|
residualCov[6][matrixDimensionFactor] = {{0}},
|
|
measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}};
|
|
|
|
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
|
|
matrixDimensionFactor, 6);
|
|
MatrixOperations<double>::multiply(*estimatedCovarianceMatrix, *measSensMatrixTransposed,
|
|
*residualCov, 6, 6, matrixDimensionFactor);
|
|
|
|
MatrixOperations<double>::multiply(measSensMatrix, *residualCov, *kalmanGainDen,
|
|
matrixDimensionFactor, 6, matrixDimensionFactor);
|
|
MatrixOperations<double>::add(*kalmanGainDen, measCovMatrix, *kalmanGainDen,
|
|
matrixDimensionFactor, matrixDimensionFactor);
|
|
ReturnValue_t result = MatrixOperations<double>::inverseMatrix(*kalmanGainDen, *invKalmanGainDen,
|
|
matrixDimensionFactor);
|
|
if (result != returnvalue::OK) {
|
|
mekfStatus = MekfStatus::COVARIANCE_INVERSION_FAILED;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_COVARIANCE_INVERSION_FAILED;
|
|
}
|
|
|
|
MatrixOperations<double>::multiply(*residualCov, *invKalmanGainDen, kalmanGain, 6,
|
|
matrixDimensionFactor, matrixDimensionFactor);
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *measSensMatrix) {
|
|
// Covariance Matrix: P_plus [k] = (I-K*H)*P_min
|
|
double helperMat[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
|
MatrixOperations<double>::multiply(kalmanGain, measSensMatrix, *helperMat, 6,
|
|
matrixDimensionFactor, 6);
|
|
MatrixOperations<double>::subtract(*EYE6, *helperMat, *helperMat, 6, 6);
|
|
MatrixOperations<double>::multiply(*helperMat, *estimatedCovarianceMatrix, *covAposteriori, 6, 6,
|
|
6);
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec,
|
|
double *estVec) {
|
|
double stateVecErr[6] = {0, 0, 0, 0, 0, 0};
|
|
double plantOutputDiff[matrixDimensionFactor] = {0};
|
|
VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor);
|
|
MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6,
|
|
matrixDimensionFactor, 1);
|
|
|
|
double quatAposteriori[3] = {stateVecErr[0], stateVecErr[1], stateVecErr[2]};
|
|
double biasAposteriori[3] = {stateVecErr[3], stateVecErr[4], stateVecErr[5]};
|
|
|
|
double quaternionIdentity[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
double quaternionIdentityUpperMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
double quaternionIdentityUpperMatrixHelper[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
double negQuatVec[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, estimatedQuaternionBI[3],
|
|
*quaternionIdentityUpperMatrix, 3, 3);
|
|
MatrixOperations<double>::skewMatrix(estimatedQuaternionBI, *quaternionIdentityUpperMatrixHelper);
|
|
MatrixOperations<double>::add(*quaternionIdentityUpperMatrix,
|
|
*quaternionIdentityUpperMatrixHelper,
|
|
*quaternionIdentityUpperMatrix, 3, 3);
|
|
|
|
VectorOperations<double>::mulScalar(estimatedQuaternionBI, -1, negQuatVec, 3);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(*quaternionIdentity, *quaternionIdentityUpperMatrix, 3,
|
|
3, 4, 3, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(*quaternionIdentity, negQuatVec, 1, 3, 4, 3, 3, 0);
|
|
|
|
double quatCorrection[4] = {0, 0, 0, 0};
|
|
MatrixOperations<double>::multiply(*quaternionIdentity, quatAposteriori, quatCorrection, 4, 3, 1);
|
|
VectorOperations<double>::mulScalar(quatCorrection, 0.5, quatCorrection, 4);
|
|
VectorOperations<double>::add(estimatedQuaternionBI, quatCorrection, estimatedQuaternionBI, 4);
|
|
QuaternionOperations::normalize(estimatedQuaternionBI);
|
|
|
|
VectorOperations<double>::add(estimatedBiasGyr, biasAposteriori, estimatedBiasGyr, 3);
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::kfPropagate(const acsctrl::GyrDataProcessed *gyrData,
|
|
const double timeDelta) {
|
|
// Estimated Rotation Rate
|
|
VectorOperations<double>::subtract(gyrData->gyrVecTot.value, estimatedBiasGyr, estimatedRotRate);
|
|
double estimatedRotRateNorm = VectorOperations<double>::norm(estimatedRotRate, 3);
|
|
double estimatedRotRateSkewed[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::skewMatrix(estimatedRotRate, *estimatedRotRateSkewed);
|
|
|
|
// Estimated Quaternion
|
|
double diffQuatMatrix[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}},
|
|
diffQuatMatrixHelper1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
diffQuatMatrixHelper2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, diffVector[3] = {0, 0, 0},
|
|
helperQuat[4] = {0, 0, 0, 0};
|
|
double diffScalar = std::cos(.5 * estimatedRotRateNorm * timeDelta);
|
|
VectorOperations<double>::mulScalar(
|
|
estimatedRotRate, std::sin(.5 * estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm,
|
|
diffVector, 3);
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, diffScalar, *diffQuatMatrixHelper1, 3, 3);
|
|
MatrixOperations<double>::skewMatrix(diffVector, *diffQuatMatrixHelper2);
|
|
MatrixOperations<double>::subtract(*diffQuatMatrixHelper1, *diffQuatMatrixHelper2,
|
|
*diffQuatMatrixHelper1, 3, 3);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, *diffQuatMatrixHelper1, 3, 3, 4, 4, 0,
|
|
0);
|
|
MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, diffVector, 3, 1, 4, 4, 0, 3);
|
|
VectorOperations<double>::mulScalar(diffVector, -1, diffVector, 3);
|
|
MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, diffVector, 1, 3, 4, 4, 3, 0);
|
|
diffQuatMatrix[3][3] = diffScalar;
|
|
|
|
std::memcpy(helperQuat, estimatedQuaternionBI, sizeof(helperQuat));
|
|
MatrixOperations<double>::multiply(*diffQuatMatrix, helperQuat, estimatedQuaternionBI, 4, 4, 1);
|
|
QuaternionOperations::normalize(estimatedQuaternionBI);
|
|
|
|
// Covariance Matrix: P_minus [k+1] = Phi*P_plus [k]*Phi'+G*Q*G'
|
|
// Phi [Discrete Error State Transition Matrix]
|
|
double errorStateTransition[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
|
|
double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
phi11L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
phi11R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
phi12L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
phi12R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*estimatedRotRateSkewed, std::sin(estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm,
|
|
*phi11L, 3, 3);
|
|
MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi11R, 3,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*phi11R,
|
|
(1 - std::cos(estimatedRotRateNorm * timeDelta)) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm),
|
|
*phi11R, 3, 3);
|
|
MatrixOperations<double>::subtract(*EYE3, *phi11L, *phi11, 3, 3);
|
|
MatrixOperations<double>::add(*phi11, *phi11R, *phi11, 3, 3);
|
|
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, -timeDelta, *phi12, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*estimatedRotRateSkewed,
|
|
(1 - std::cos(estimatedRotRateNorm * timeDelta)) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm),
|
|
*phi12L, 3, 3);
|
|
MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi12R, 3,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*phi12R,
|
|
(estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm),
|
|
*phi12R, 3, 3);
|
|
MatrixOperations<double>::add(*phi12, *phi12L, *phi12, 3, 3);
|
|
MatrixOperations<double>::subtract(*phi12, *phi12R, *phi12, 3, 3);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(*errorStateTransition, *phi11, 3, 3, 6, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(*errorStateTransition, *phi12, 3, 3, 6, 6, 0, 3);
|
|
|
|
// G [Discrete Transfer Matrix]
|
|
double transferMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
|
|
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
|
|
|
|
// Q [Discrete Time Covariance]
|
|
double timeCovariance[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
|
double q11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q21[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
if (estimatedRotRateNorm * timeDelta < M_PI / 10.) {
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*EYE3,
|
|
(sigmaGyrArw * sigmaGyrArw) * timeDelta +
|
|
((sigmaGyrBs * sigmaGyrBs) * (timeDelta * timeDelta * timeDelta)) / 3.,
|
|
*q11, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*EYE3, -(sigmaGyrBs * sigmaGyrBs * timeDelta * timeDelta) / 2., *q12, 3, 3);
|
|
} else {
|
|
double q11h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q11h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q12h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
|
q12h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, (sigmaGyrArw * sigmaGyrArw) * timeDelta, *q11,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, (timeDelta * timeDelta * timeDelta) / 3.,
|
|
*q11h1, 3, 3);
|
|
MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q11h2, 3,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*q11h2,
|
|
(2 * estimatedRotRateNorm * timeDelta - 2 * std::sin(estimatedRotRateNorm * timeDelta) -
|
|
((estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm) *
|
|
(timeDelta * timeDelta * timeDelta) / .3)) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm *
|
|
estimatedRotRateNorm * estimatedRotRateNorm),
|
|
*q11h2, 3, 3);
|
|
MatrixOperations<double>::add(*q11h1, *q11h2, *q11h1, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*q11h1, sigmaGyrBs * sigmaGyrBs, *q11h1, 3, 3);
|
|
MatrixOperations<double>::add(*q11, *q11h1, *q11, 3, 3);
|
|
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, -(timeDelta * timeDelta) / 2., *q12, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*estimatedRotRateSkewed,
|
|
(estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm),
|
|
*q12h1, 3, 3);
|
|
MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q12h2, 3,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(
|
|
*q12h2,
|
|
(((estimatedRotRateNorm * estimatedRotRateNorm) * (timeDelta * timeDelta)) / 2. +
|
|
std::cos(estimatedRotRateNorm * timeDelta) - 1) /
|
|
(estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm *
|
|
estimatedRotRateNorm),
|
|
*q12h2, 3, 3);
|
|
MatrixOperations<double>::add(*q12, *q12h1, *q12, 3, 3);
|
|
MatrixOperations<double>::subtract(*q12, *q12h2, *q12, 3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*q12, sigmaGyrBs * sigmaGyrBs, *q12, 3, 3);
|
|
}
|
|
|
|
MatrixOperations<double>::transpose(*q12, *q21, 3);
|
|
MatrixOperations<double>::multiplyScalar(*EYE3, sigmaGyrBs * sigmaGyrBs * timeDelta, *q22, 3, 3);
|
|
|
|
MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q11, 3, 3, 6, 6, 0, 0);
|
|
MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q12, 3, 3, 6, 6, 0, 3);
|
|
MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q21, 3, 3, 6, 6, 3, 0);
|
|
MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q22, 3, 3, 6, 6, 3, 3);
|
|
|
|
// Estimated Covariance
|
|
double errorStateTransitionTransposed[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
|
|
estCovarianceMatrixL[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
|
|
estCovarianceMatrixR[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
|
|
estCovarianceMatrixHelp[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
|
|
|
MatrixOperations<double>::transpose(*errorStateTransition, *errorStateTransitionTransposed, 6);
|
|
MatrixOperations<double>::multiply(*errorStateTransition, *covAposteriori,
|
|
*estCovarianceMatrixHelp, 6, 6, 6);
|
|
MatrixOperations<double>::multiply(*estCovarianceMatrixHelp, *errorStateTransitionTransposed,
|
|
*estCovarianceMatrixL, 6, 6, 6);
|
|
MatrixOperations<double>::multiply(*transferMatrix, *timeCovariance, *estCovarianceMatrixHelp, 6,
|
|
6, 6);
|
|
MatrixOperations<double>::multiply(*estCovarianceMatrixHelp, *transferMatrix,
|
|
*estCovarianceMatrixR, 6, 6, 6);
|
|
MatrixOperations<double>::add(*estCovarianceMatrixL, *estCovarianceMatrixR,
|
|
*estimatedCovarianceMatrix, 6, 6);
|
|
}
|
|
|
|
ReturnValue_t MultiplicativeKalmanFilter::kfFiniteCheck(
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
if (not(VectorOperations<double>::isFinite(estimatedQuaternionBI, 4)) or
|
|
not(VectorOperations<double>::isFinite(estimatedRotRate, 3)) or
|
|
not(MatrixOperations<double>::isFinite(*estimatedCovarianceMatrix, 6, 6))) {
|
|
mekfStatus = MekfStatus::NOT_FINITE;
|
|
updateDataSetWithoutData(attitudeEstimationData);
|
|
return MEKF_NOT_FINITE;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::updateDataSetWithoutData(
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
PoolReadGuard pg(attitudeEstimationData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(attitudeEstimationData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double));
|
|
attitudeEstimationData->quatMekf.setValid(false);
|
|
std::memcpy(attitudeEstimationData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double));
|
|
attitudeEstimationData->satRotRateMekf.setValid(false);
|
|
attitudeEstimationData->mekfStatus.value = mekfStatus;
|
|
attitudeEstimationData->mekfStatus.setValid(true);
|
|
}
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::updateDataSet(
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
|
PoolReadGuard pg(attitudeEstimationData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(attitudeEstimationData->quatMekf.value, estimatedQuaternionBI, 4 * sizeof(double));
|
|
attitudeEstimationData->quatMekf.setValid(true);
|
|
std::memcpy(attitudeEstimationData->satRotRateMekf.value, estimatedRotRate, 3 * sizeof(double));
|
|
attitudeEstimationData->satRotRateMekf.setValid(true);
|
|
attitudeEstimationData->mekfStatus.value = mekfStatus;
|
|
attitudeEstimationData->mekfStatus.setValid(true);
|
|
}
|
|
}
|
|
|
|
void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, bool valid,
|
|
bool allowStr) {
|
|
strData.strQuat.value[0] = qX;
|
|
strData.strQuat.value[1] = qY;
|
|
strData.strQuat.value[2] = qZ;
|
|
strData.strQuat.value[3] = qW;
|
|
strData.strQuat.valid = (valid and allowStr);
|
|
}
|