From d37770422b4cfa706b5fdc1fbd833f65aa52fc83 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 23 Feb 2023 15:05:10 +0100 Subject: [PATCH] bugfixes --- .../controller/acs/MultiplicativeKalmanFilter.cpp | 14 +++++++------- mission/controller/acs/Navigation.h | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 723a9bec..43110554 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -188,12 +188,12 @@ ReturnValue_t MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); - return KALMAN_INITIALIZED; + return MEKF_INITIALIZED; } else { // no initialisation possible, no valid measurements validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); - return KALMAN_UNINITIALIZED; + return MEKF_UNINITIALIZED; } } @@ -210,12 +210,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_DATA; + return MEKF_NO_GYR_DATA; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); - return KALMAN_NO_MODEL_VECTORS; + return MEKF_NO_MODEL_VECTORS; } // Check Measurements available from SS, MAG, STR if (validSS && validMagField_ && validSTR_) { @@ -853,7 +853,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_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } // [K = P * H' / (H * P * H' + R)] @@ -1084,7 +1084,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); - return KALMAN_RUNNING; + return MEKF_RUNNING; } ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { @@ -1094,7 +1094,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); - return KALMAN_UNINITIALIZED; + return MEKF_UNINITIALIZED; } void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 86baeacf..cf9e81e3 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -22,7 +22,7 @@ class Navigation { private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; AcsParameters acsParameters; - ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED; + ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; }; #endif /* ACS_NAVIGATION_H_ */