MEKF Fixes #843

Merged
meggert merged 13 commits from mekf-fix into main 2024-02-29 13:08:25 +01:00
7 changed files with 760 additions and 1214 deletions

View File

@ -30,6 +30,7 @@ will consitute of a breaking change warranting a new major release:
and is triggered by the `AcsController` now.
- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
- Fixed calculation of desaturation torque for faulty RWs.
- Fixed bugs within the `MEKF` and simplified the code.
## Changed
@ -50,6 +51,8 @@ will consitute of a breaking change warranting a new major release:
- Updated STR handler to unlock and allow using the secondary firmware slot.
- STR handling for new BlobStats TM set.
- Added new action command to update the standard deviations within the `MEKF` from the
`AcsParameters`.
# [v7.6.1] 2024-02-05

View File

@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
sdcMan(sdcMan),
attitudeEstimation(&acsParameters),
fusedRotationEstimation(&acsParameters),
navigation(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
ptgCtrl(&acsParameters),
@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
}
case UPDATE_TLE: {
if (size != 69 * 2) {
return INVALID_PARAMETERS;
return HasActionsIF::INVALID_PARAMETERS;
}
ReturnValue_t result = updateTle(data, data + 69, false);
if (result != returnvalue::OK) {
@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
}
std::memcpy(tle + 69, line2, 69);
actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
return EXECUTION_FINISHED;
return HasActionsIF::EXECUTION_FINISHED;
}
case (UPDATE_MEKF_STANDARD_DEVIATIONS):
navigation.updateMekfStandardDeviations(&acsParameters);
return HasActionsIF::EXECUTION_FINISHED;
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
@ -176,7 +180,7 @@ void AcsController::performAttitudeControl() {
mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &attitudeEstimationData, &acsParameters);
&susDataProcessed, timeDelta, &attitudeEstimationData);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {

View File

@ -113,6 +113,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
static const DeviceCommandId_t UPDATE_TLE = 0x3;
static const DeviceCommandId_t READ_TLE = 0x4;
static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;

File diff suppressed because it is too large Load Diff

View File

@ -1,14 +1,16 @@
#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_
#include <stdint.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "eive/resultClassIds.h"
#include <common/config/eive/resultClassIds.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class MultiplicativeKalmanFilter {
/* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
/* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by
* means of the spacecraft attitude sensors
*
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter {
public:
/* @brief: Constructor
*/
MultiplicativeKalmanFilter();
MultiplicativeKalmanFilter(AcsParameters *acsParameters);
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
/* @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
*/
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::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
const acsctrl::MgmDataProcessed *mgmData,
const acsctrl::GyrDataProcessed *gyrData,
acsctrl::AttitudeEstimationData *attitudeEstimationData);
/* @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, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters);
ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
const acsctrl::MgmDataProcessed *mgmData,
const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
NO_GYR_DATA = 1,
NO_MODEL_VECTORS = 2,
NO_SUS_MGM_STR_DATA = 3,
COVARIANCE_INVERSION_FAILED = 4,
NOT_FINITE = 5,
INITIALIZED = 10,
RUNNING = 11,
};
void updateStandardDeviations(const AcsParameters *acsParameters);
void setStrData(const double qX, const double qY, const double qZ, const double qW,
const bool valid);
// resetting Mekf
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter {
private:
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
static constexpr double ZERO_MAT66[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}};
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
static constexpr double EYE6[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}};
/*Parameters*/
double quaternion_STR_SB[4];
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
NO_GYR_DATA = 1,
NO_MODEL_VECTORS = 2,
NO_SUS_MGM_STR_DATA = 3,
COVARIANCE_INVERSION_FAILED = 4,
NOT_FINITE = 5,
INITIALIZED = 10,
RUNNING = 11,
};
/*States*/
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 = 0;
enum SensorAvailability : uint8_t {
NONE = 0,
SUS_MGM_STR = 1,
SUS_MGM = 2,
SUS_STR = 3,
MGM_STR = 4,
SUS = 5,
MGM = 6,
STR = 7,
};
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]);
MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
struct StrData {
struct StrQuat {
double value[4] = {0, 0, 0, 0};
bool valid = false;
} strQuat;
} strData;
// Standard Deviations
double sigmaSus = 0;
double sigmaMgm = 0;
double sigmaStr = 0;
double sigmaGyr = 0;
// sigmaV
double sigmaGyrArw = 0;
// sigmaU
double sigmaGyrBs = 0;
// Covariance Matrices
double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double covAposteriori[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}};
// Sensor Availability
SensorAvailability sensorsAvailable = SensorAvailability::NONE;
uint8_t matrixDimensionFactor = 0;
// Estimated States
double estimatedQuaternionBI[4] = {0, 0, 0, 1};
double estimatedBiasGyr[3] = {0, 0, 0};
double estimatedRotRate[3] = {0, 0, 0};
double estimatedCovarianceMatrix[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}};
// Functions
ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
const acsctrl::MgmDataProcessed *mgmData,
const acsctrl::GyrDataProcessed *gyrData,
acsctrl::AttitudeEstimationData *attitudeEstimationData);
void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
acsctrl::AttitudeEstimationData *attitudeEstimationData);
void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
};
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

View File

@ -1,40 +1,27 @@
#include "Navigation.h"
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
Navigation::Navigation() {}
Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
Navigation::~Navigation() {}
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::AttitudeEstimationData *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.isTrustWorthy.value;
ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
const acsctrl::GyrDataProcessed *gyrDataProcessed,
const acsctrl::MgmDataProcessed *mgmDataProcessed,
const acsctrl::SusDataProcessed *susDataProcessed,
const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
multiplicativeKalmanFilter.setStrData(
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
sensorValues->strSet.caliQx.isValid());
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
mekfStatus = multiplicativeKalmanFilter.init(
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,
acsParameters);
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
gyrDataProcessed, attitudeEstimationData);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), 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, acsParameters);
susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
return mekfStatus;
}
}
@ -79,3 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
return sgp4Propagator.initialize(line1, line2);
}
void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
}

View File

@ -5,24 +5,24 @@
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
#include <mission/controller/acs/SensorProcessing.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class Navigation {
public:
Navigation();
Navigation(AcsParameters *acsParameters);
virtual ~Navigation();
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
const acsctrl::GyrDataProcessed *gyrDataProcessed,
const acsctrl::MgmDataProcessed *mgmDataProcessed,
const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData);
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
void updateMekfStandardDeviations(const AcsParameters *acsParameters);
protected:
private: