fixed int32_t to double warnings. reformats
This commit is contained in:
parent
0d3509b991
commit
8b23fd3dd2
File diff suppressed because it is too large
Load Diff
@ -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_ */
|
||||
|
||||
@ -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 ?
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user