eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

bugfixes
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-23 15:05:10 +01:00
parent 12539ba415
commit d37770422b
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -188,12 +188,12 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2]; initialCovarianceMatrix[5][5] = initGyroCov[2][2];
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
return KALMAN_INITIALIZED; return MEKF_INITIALIZED;
} else { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
validInit = false; validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); 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 int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
return KALMAN_NO_GYR_DATA; return MEKF_NO_GYR_DATA;
} }
// Check for Model Calculations // Check for Model Calculations
else if (!validSSModel || !validMagModel) { else if (!validSSModel || !validMagModel) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
return KALMAN_NO_MODEL_VECTORS; return MEKF_NO_MODEL_VECTORS;
} }
// Check Measurements available from SS, MAG, STR // Check Measurements available from SS, MAG, STR
if (validSS && validMagField_ && validSTR_) { if (validSS && validMagField_ && validSTR_) {
@ -853,7 +853,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF); int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (inversionFailed) { if (inversionFailed) {
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); 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)] // [K = P * H' / (H * P * H' + R)]
@ -1084,7 +1084,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return KALMAN_RUNNING; return MEKF_RUNNING;
} }
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { 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(initialQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return KALMAN_UNINITIALIZED; return MEKF_UNINITIALIZED;
} }
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,

2
mission/controller/acs/Navigation.h
View File

@ -22,7 +22,7 @@ class Navigation {
private: private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter; MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters; AcsParameters acsParameters;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
}; };
#endif /* ACS_NAVIGATION_H_ */ #endif /* ACS_NAVIGATION_H_ */