101 lines
4.4 KiB
C++
101 lines
4.4 KiB
C++
/*
|
|
* MultiplicativeKalmanFilter.h
|
|
*
|
|
* Created on: 4 Feb 2022
|
|
* Author: Robin Marquardt
|
|
*
|
|
* @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
|
|
*/
|
|
|
|
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
|
#define MULTIPLICATIVEKALMANFILTER_H_
|
|
|
|
#include <stdint.h> //uint8_t
|
|
#include <time.h> /*purpose, timeval ?*/
|
|
|
|
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
|
#include "AcsParameters.h"
|
|
#include "config/classIds.h"
|
|
|
|
class MultiplicativeKalmanFilter {
|
|
public:
|
|
/* @brief: Constructor
|
|
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
|
*/
|
|
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
|
|
virtual ~MultiplicativeKalmanFilter();
|
|
|
|
void reset(); // NOT YET DEFINED - should only reset all mekf variables
|
|
|
|
/* @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
|
|
*/
|
|
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);
|
|
|
|
/* @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);
|
|
|
|
// Declaration of Events (like init) and memberships
|
|
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
|
|
// (/config/returnvalues/classIDs.h) static const Event RESET =
|
|
// MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
|
|
// resetting Mekf
|
|
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
|
|
static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01);
|
|
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
|
|
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
|
|
|
|
private:
|
|
/*Parameters*/
|
|
AcsParameters::InertiaEIVE *inertiaEIVE;
|
|
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
|
|
double quaternion_STR_SB[4];
|
|
bool validInit;
|
|
double sampleTime = 0.1;
|
|
|
|
/*States*/
|
|
double initialQuaternion[4]; /*after reset?QUEST*/
|
|
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
|
|
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
|
|
bool validMekf;
|
|
uint8_t sensorsAvail;
|
|
|
|
/*Outputs*/
|
|
double quatBJ[4]; /* Output Quaternion */
|
|
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
|
/*Parameter INIT*/
|
|
// double alpha, gamma, beta;
|
|
/*Functions*/
|
|
void loadAcsParameters(AcsParameters *acsParameters_);
|
|
};
|
|
|
|
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|