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

Telemetry related Changes #394

Merged
muellerr merged 39 commits from eggert/acs-dataset-stuff into develop 2023-02-22 19:56:24 +01:00
Conversation 0 Commits 39 Files Changed 17 +53 -69
3 changed files with 53 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 0ec0d551a3 - Show all commits

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

@ -14,7 +14,7 @@
/*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0.5, 0.5, 0.5, 0.5},
: 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_);
@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
void MultiplicativeKalmanFilter::reset() {}
void 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) { // valids for "model measurements"?
const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
@ -190,9 +188,11 @@ void MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
} else {
// no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
}
}
@ -208,32 +208,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false;
updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
return KALMAN_NO_GYR_MEAS;
}
// Check for Model Calculations
else if (!validSSModel || !validMagModel) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false;
updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
return KALMAN_NO_MODEL;
}
// Check Measurements available from SS, MAG, STR
@ -260,17 +240,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MDF = 3;
} else {
sensorsAvail = 8; // no measurements
validMekf = false;
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
return returnvalue::FAILED;
}
@ -881,17 +851,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (inversionFailed) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->setValidity(false, true);
}
}
validMekf = false;
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}
@ -1121,20 +1081,40 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true;
// Discrete Time Step
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return returnvalue::OK;
}
// Check for new data in measurement -> SensorProcessing ?
void MultiplicativeKalmanFilter::reset() {}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
mekfData->quatMekf.setValid(false);
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
mekfData->satRotRateMekf.setValid(false);
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, false);
}
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus, double quat[4],
double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, true);
}
}
return returnvalue::OK;
}

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

@ -35,13 +35,13 @@ class MultiplicativeKalmanFilter {
/* @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
* 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);
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization
@ -63,6 +63,16 @@ class MultiplicativeKalmanFilter {
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
NO_GYR_DATA = 1,
NO_MODEL_VECTORS = 2,
NO_SUS_MGM_STR_DATA = 3,
COVARIANCE_INVERSION_FAILED = 4,
INITIALIZED = 10,
RUNNING = 11,
};
// resetting Mekf
static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
@ -80,16 +90,17 @@ class MultiplicativeKalmanFilter {
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_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);
};
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

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

@ -1,10 +1,3 @@
/*
* Navigation.cpp
*
* Created on: 23 May 2022
* Author: Robin Marquardt
*/
#include "Navigation.h"
#include <fsfw/globalfunctions/math/MatrixOperations.h>
@ -46,7 +39,7 @@ void 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());
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
kalmanInit = true;
*mekfValid = returnvalue::OK;