diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index d222de84..d90f3f51 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -27,7 +27,7 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } -void MultiplicativeKalmanFilter::init( +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::MekfData *mekfData) { // valids for "model measurements"? @@ -189,10 +189,12 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); + return KALMAN_INITIALIZED; } else { // no initialisation possible, no valid measurements validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); + return KALMAN_UNINITIALIZED; } } @@ -209,12 +211,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); - return KALMAN_NO_GYR_MEAS; + return KALMAN_NO_GYR_DATA; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); - return KALMAN_NO_MODEL; + return KALMAN_NO_MODEL_VECTORS; } // Check Measurements available from SS, MAG, STR if (validSS && validMagField_ && validSTR_) { @@ -852,7 +854,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); if (inversionFailed) { updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); - return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } // [K = P * H' / (H * P * H' + R)] @@ -1083,10 +1085,17 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); - return returnvalue::OK; + return KALMAN_RUNNING; } -void MultiplicativeKalmanFilter::reset() {} +void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *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); +} void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus) { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index e1b4f701..dd02cf9f 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -15,8 +15,7 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ -#include //uint8_t -#include /*purpose, timeval ?*/ +#include #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" @@ -30,7 +29,7 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(AcsParameters *acsParameters_); virtual ~MultiplicativeKalmanFilter(); - void reset(); // NOT YET DEFINED - should only reset all mekf variables + void reset(acsctrl::MekfData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm @@ -39,9 +38,10 @@ class MultiplicativeKalmanFilter { * 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, acsctrl::MekfData *mekfData); + ReturnValue_t 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::MekfData *mekfData); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -75,9 +75,14 @@ class MultiplicativeKalmanFilter { // 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); - static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2); - static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2); + static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4); + static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5); + static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED = + returnvalue::makeCode(IF_KAL_ID, 6); + static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7); + static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8); private: /*Parameters*/