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

returnvalues for all mekf termination cases now
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-21 16:56:13 +01:00
parent 0ec0d551a3
commit 793e6f4119
2 changed files with 29 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<double>::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<double>::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) {

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

@ -15,8 +15,7 @@
#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include <stdint.h>
#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*/