147 lines
6.3 KiB
C++
147 lines
6.3 KiB
C++
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
|
#define MULTIPLICATIVEKALMANFILTER_H_
|
|
|
|
#include <common/config/eive/resultClassIds.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <mission/controller/acs/AcsParameters.h>
|
|
#include <mission/controller/acs/SensorValues.h>
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
|
|
|
class MultiplicativeKalmanFilter {
|
|
/* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by
|
|
* means of the spacecraft attitude sensors
|
|
*
|
|
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
|
|
* Marquardt
|
|
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
|
|
*/
|
|
public:
|
|
/* @brief: Constructor
|
|
*/
|
|
MultiplicativeKalmanFilter(AcsParameters *acsParameters);
|
|
virtual ~MultiplicativeKalmanFilter();
|
|
|
|
ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
|
|
ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
|
|
const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
|
|
ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
|
|
const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
|
|
void updateStandardDeviations(const AcsParameters *acsParameters);
|
|
|
|
void setStrData(const double qX, const double qY, const double qZ, const double qW,
|
|
const bool valid, const bool allowStr);
|
|
|
|
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
|
|
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
|
|
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
|
|
static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4);
|
|
static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
|
|
static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
|
|
returnvalue::makeCode(IF_MEKF_ID, 6);
|
|
static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
|
|
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
|
|
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
|
|
|
|
private:
|
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
|
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
|
static constexpr double ZERO_MAT66[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}};
|
|
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
|
|
static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
|
static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
|
|
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
|
|
|
|
enum MekfStatus : uint8_t {
|
|
UNINITIALIZED = 0,
|
|
NO_GYR_DATA = 1,
|
|
NO_MODEL_VECTORS = 2,
|
|
NO_SUS_MGM_STR_DATA = 3,
|
|
COVARIANCE_INVERSION_FAILED = 4,
|
|
NOT_FINITE = 5,
|
|
INITIALIZED = 10,
|
|
RUNNING = 11,
|
|
};
|
|
|
|
enum SensorAvailability : uint8_t {
|
|
NONE = 0,
|
|
SUS_MGM_STR = 1,
|
|
SUS_MGM = 2,
|
|
SUS_STR = 3,
|
|
MGM_STR = 4,
|
|
SUS = 5,
|
|
MGM = 6,
|
|
STR = 7,
|
|
};
|
|
|
|
MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
|
|
|
|
struct StrData {
|
|
struct StrQuat {
|
|
double value[4] = {0, 0, 0, 0};
|
|
bool valid = false;
|
|
} strQuat;
|
|
} strData;
|
|
|
|
// Standard Deviations
|
|
double sigmaSus = 0;
|
|
double sigmaMgm = 0;
|
|
double sigmaStr = 0;
|
|
double sigmaGyr = 0;
|
|
// sigmaV
|
|
double sigmaGyrArw = 0;
|
|
// sigmaU
|
|
double sigmaGyrBs = 0;
|
|
|
|
// Covariance Matrices
|
|
double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
double covAposteriori[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}};
|
|
|
|
// Sensor Availability
|
|
SensorAvailability sensorsAvailable = SensorAvailability::NONE;
|
|
uint8_t matrixDimensionFactor = 0;
|
|
|
|
// Estimated States
|
|
double estimatedQuaternionBI[4] = {0, 0, 0, 1};
|
|
double estimatedBiasGyr[3] = {0, 0, 0};
|
|
double estimatedRotRate[3] = {0, 0, 0};
|
|
double estimatedCovarianceMatrix[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}};
|
|
|
|
// Functions
|
|
ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
|
|
const acsctrl::MgmDataProcessed *mgmData,
|
|
const acsctrl::GyrDataProcessed *gyrData,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
|
|
void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
|
|
double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
|
|
ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
|
|
void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
|
|
void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
|
|
|
|
ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
|
|
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
|
};
|
|
|
|
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|