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

removed AcsParameters as part of Navigation

Browse Source
This commit is contained in:
Marius Eggert
2023-02-27 16:37:46 +01:00
parent fd4be08796
commit 92f5a8bf89
4 changed files with 64 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,33 +12,22 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
/*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0, 0, 0, 1},
initialCovarianceMatrix{{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}} {
loadAcsParameters(acsParameters_);
}
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
ReturnValue_t MultiplicativeKalmanFilter::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) { // valids for "model measurements"?
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
return MEKF_INITIALIZED;
} else {
// no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
}
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::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 sampleTime,
acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::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) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime +
1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
double fact12_0 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
(1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) +
cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(sampleTime, 3);
double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
pow(normRotEst, 5);
double fact11_2 =
(2 * normRotEst * acsParameters->onBoardParams.sampleTime -
2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
double fact11_2 =
(1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*identityMatrix3,
acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3);
double fact12_2 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// Propagated Quaternion
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

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

@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter {
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData);
@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter {
*/
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);
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
@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter {
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 sampleTime, acsctrl::MekfData *mekfData);
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter {
private:
/*Parameters*/
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
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;
uint8_t sensorsAvail = 0;
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);

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

@ -8,9 +8,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
acsParameters = *acsParameters_;
}
Navigation::Navigation() {}
Navigation::~Navigation() {}
@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData) {
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
acsParameters);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
return mekfStatus;
}
}

6
mission/controller/acs/Navigation.h
View File

@ -9,19 +9,19 @@
class Navigation {
public:
Navigation(AcsParameters *acsParameters_);
Navigation();
virtual ~Navigation();
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
};