diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index f54893a2..d222de84 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -14,7 +14,7 @@ /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0.5, 0.5, 0.5, 0.5}, + : initialQuaternion{0, 0, 0, 1}, initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { loadAcsParameters(acsParameters_); @@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } -void MultiplicativeKalmanFilter::reset() {} - void MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel) { // valids for "model measurements"? + const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; @@ -190,9 +188,11 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; + updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); } else { // no initialisation possible, no valid measurements validInit = false; + updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); } } @@ -208,32 +208,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; + updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); return KALMAN_NO_GYR_MEAS; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; + updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); return KALMAN_NO_MODEL; } // Check Measurements available from SS, MAG, STR @@ -260,17 +240,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MDF = 3; } else { sensorsAvail = 8; // no measurements - validMekf = false; - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } + updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA); return returnvalue::FAILED; } @@ -881,17 +851,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c double invResidualCov[MDF][MDF] = {{0}}; int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); if (inversionFailed) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; + updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } @@ -1121,20 +1081,40 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - validMekf = true; - // Discrete Time Step + updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); + return returnvalue::OK; +} - // Check for new data in measurement -> SensorProcessing ? +void MultiplicativeKalmanFilter::reset() {} +void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, + MekfStatus mekfStatus) { { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double)); + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + mekfData->quatMekf.setValid(false); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->satRotRateMekf.setValid(false); + mekfData->mekfStatus = mekfStatus; + mekfData->setValidity(true, false); + } + } +} + +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *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); } } - - return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index a2f81272..e1b4f701 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -35,13 +35,13 @@ class MultiplicativeKalmanFilter { /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm * @param: magneticField_ magnetic field vector in the body frame - * sunDir_ sun direction vector in the body frame - * sunDirJ sun direction vector in the ECI frame - * magFieldJ magnetic field vector in the ECI frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame */ void init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel); + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -63,6 +63,16 @@ class MultiplicativeKalmanFilter { const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + enum MekfStatus : uint8_t { + UNINITIALIZED = 0, + NO_GYR_DATA = 1, + NO_MODEL_VECTORS = 2, + NO_SUS_MGM_STR_DATA = 3, + COVARIANCE_INVERSION_FAILED = 4, + INITIALIZED = 10, + RUNNING = 11, + }; + // resetting Mekf static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); @@ -80,16 +90,17 @@ class MultiplicativeKalmanFilter { double initialQuaternion[4]; /*after reset?QUEST*/ double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - bool validMekf; uint8_t sensorsAvail; /*Outputs*/ double quatBJ[4]; /* Output Quaternion */ double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ - // double alpha, gamma, beta; /*Functions*/ void loadAcsParameters(AcsParameters *acsParameters_); + void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); + void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], + double satRotRate[3]); }; #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index a9de5817..59803f0f 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,10 +1,3 @@ -/* - * Navigation.cpp - * - * Created on: 23 May 2022 - * Author: Robin Marquardt - */ - #include "Navigation.h" #include @@ -46,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); kalmanInit = true; *mekfValid = returnvalue::OK;