Robin Mueller
bbd27eca76
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
103 lines
4.8 KiB
C++
103 lines
4.8 KiB
C++
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
|
#define MULTIPLICATIVEKALMANFILTER_H_
|
|
|
|
#include <stdint.h>
|
|
|
|
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
|
#include "AcsParameters.h"
|
|
#include "eive/resultClassIds.h"
|
|
|
|
class MultiplicativeKalmanFilter {
|
|
/* @brief: This class handles the calculation of an estimated quaternion and the gyro 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();
|
|
virtual ~MultiplicativeKalmanFilter();
|
|
|
|
ReturnValue_t reset(acsctrl::MekfData *mekfData);
|
|
|
|
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
|
|
* quaternion through the QUEST algorithm
|
|
* @param: magneticField_ magnetic field vector in the body frame
|
|
* sunDir_ sun direction vector in the body frame
|
|
* sunDirJ sun direction vector in the ECI frame
|
|
* magFieldJ magnetic field vector in the ECI frame
|
|
*/
|
|
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,
|
|
AcsParameters *acsParameters);
|
|
|
|
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
|
|
* for the current step after the initalization
|
|
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
|
|
* rateGYRs_ Estimated satellite rotation rate from the
|
|
* Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_
|
|
* sun direction vector in the body frame sunDirJ sun direction vector in the ECI
|
|
* frame magFieldJ magnetic field vector in the ECI frame
|
|
* outputQuat Stores the calculated quaternion
|
|
* outputSatRate Stores the adjusted satellite rate
|
|
* @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
|
|
* the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
|
|
* calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
|
|
* RETURN_OK else
|
|
*/
|
|
ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
|
|
const bool validGYRs_, 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,
|
|
AcsParameters *acsParameters);
|
|
|
|
enum MekfStatus : uint8_t {
|
|
UNINITIALIZED = 0,
|
|
NO_GYR_DATA = 1,
|
|
NO_MODEL_VECTORS = 2,
|
|
NO_SUS_MGM_STR_DATA = 3,
|
|
COVARIANCE_INVERSION_FAILED = 4,
|
|
INITIALIZED = 10,
|
|
RUNNING = 11,
|
|
};
|
|
|
|
// resetting Mekf
|
|
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_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
|
|
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
|
|
|
|
private:
|
|
/*Parameters*/
|
|
double quaternion_STR_SB[4];
|
|
|
|
/*States*/
|
|
double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
|
|
double initialCovarianceMatrix[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}};
|
|
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
|
|
uint8_t sensorsAvail = 0;
|
|
|
|
/*Outputs*/
|
|
double quatBJ[4]; /* Output Quaternion */
|
|
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
|
/*Parameter INIT*/
|
|
/*Functions*/
|
|
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
|
|
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
|
|
double satRotRate[3]);
|
|
};
|
|
|
|
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|