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

reseting the MEKF now also resets its state in navigation
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 09:45:11 +01:00
parent 29f1dd0f8e
commit 205bd4648e
4 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1088,13 +1088,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
return KALMAN_RUNNING; return KALMAN_RUNNING;
} }
void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1}; 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}, 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}}; {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(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;
} }
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,

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

@ -29,7 +29,7 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter(AcsParameters *acsParameters_); MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter();
void reset(acsctrl::MekfData *mekfData); ReturnValue_t reset(acsctrl::MekfData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first /* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm * quaternion through the QUEST algorithm

23
mission/controller/acs/Navigation.cpp
View File

@ -25,26 +25,25 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) { if (mekfStatus == MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED) {
return multiplicativeKalmanFilter.mekfEst( mekfStatus = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value, quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
} else { return mekfStatus;
ReturnValue_t result;
result = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
kalmanInit = true;
return result;
} }
} }
void Navigation::resetMekf(acsctrl::MekfData *mekfData) { void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
multiplicativeKalmanFilter.reset(mekfData); mekfStatus = multiplicativeKalmanFilter.reset(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;
bool kalmanInit = false; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED;
}; };
#endif /* ACS_NAVIGATION_H_ */ #endif /* ACS_NAVIGATION_H_ */