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

fixed usage of wrong sample time for MEKF
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-12-19 14:34:20 +01:00
parent c338c4fb58
commit d9d7559097
3 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -197,11 +197,14 @@ void MultiplicativeKalmanFilter::init(
} }
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst( ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const double *rateGYRs_, const bool validGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *magneticField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validMagField_, const double *sunDir_,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { const bool validSS, const double *sunDirJ,
const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
// Check for GYR Measurements // Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {

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

@ -61,7 +61,7 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_, const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData); const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships // Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter {
AcsParameters::KalmanFilterParameters *kalmanFilterParameters; AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4]; double quaternion_STR_SB[4];
bool validInit; bool validInit;
double sampleTime = 0.1;
/*States*/ /*States*/
double initialQuaternion[4]; /*after reset?QUEST*/ double initialQuaternion[4]; /*after reset?QUEST*/

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

@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
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(), mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
mekfData); // VALIDS FOR QUAT AND RATE ?? mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init( multiplicativeKalmanFilter.init(