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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +1186 -1248
4 changed files with 1186 additions and 1248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8b23fd3dd2 - Show all commits

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

File diff suppressed because it is too large Load Diff

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

@ -7,101 +7,95 @@
* @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
* @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 "config/classIds.h"
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
//#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();
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
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: 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);
/* @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, 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_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);
// 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;
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;
/*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_);
/*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_ */

8
mission/controller/acs/Navigation.cpp
View File

@ -36,8 +36,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
}
else {
} else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
kalmanInit = true;
*mekfValid = 0;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init
//of kalman filter where does this class know from that kalman filter was not initialized ?
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the
// init of kalman filter where does this class know from that kalman filter was not
// initialized ?
}
}

4
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -123,7 +123,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
}
// calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
@ -145,7 +145,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations