Marius Eggert
96bbde7bbc
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
108 lines
4.3 KiB
C++
108 lines
4.3 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 "acs/config/classIds.h"
|
|
//#include <_timeval.h>
|
|
|
|
#include "AcsParameters.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
|
|
* rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [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_RMU_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 *rateRMUs_, const bool *validRMUs_,
|
|
const double *magneticField_, const bool *validMagField_,
|
|
const double *sunDir_, const bool *validSS,
|
|
const double *sunDirJ, const bool *validSSModel,
|
|
const double *magFieldJ,const bool *validMagModel,
|
|
double *outputQuat, double *outputSatRate);
|
|
|
|
|
|
// 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_RMU_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 biasRMU[3]; /*Between measured and estimated sat Rate*/
|
|
/*Parameter INIT*/
|
|
//double alpha, gamma, beta;
|
|
/*Functions*/
|
|
void loadAcsParameters(AcsParameters *acsParameters_);
|
|
};
|
|
|
|
|
|
|
|
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|