Merge pull request 'MEKF Fixes' (#843) from mekf-fix into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #843 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
bdb7f29b99
@ -30,6 +30,7 @@ will consitute of a breaking change warranting a new major release:
|
|||||||
and is triggered by the `AcsController` now.
|
and is triggered by the `AcsController` now.
|
||||||
- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
|
- 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 calculation of desaturation torque for faulty RWs.
|
||||||
|
- Fixed bugs within the `MEKF` and simplified the code.
|
||||||
|
|
||||||
## Changed
|
## 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.
|
- Updated STR handler to unlock and allow using the secondary firmware slot.
|
||||||
- STR handling for new BlobStats TM set.
|
- 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
|
# [v7.6.1] 2024-02-05
|
||||||
|
|
||||||
|
@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
|
|||||||
sdcMan(sdcMan),
|
sdcMan(sdcMan),
|
||||||
attitudeEstimation(&acsParameters),
|
attitudeEstimation(&acsParameters),
|
||||||
fusedRotationEstimation(&acsParameters),
|
fusedRotationEstimation(&acsParameters),
|
||||||
|
navigation(&acsParameters),
|
||||||
guidance(&acsParameters),
|
guidance(&acsParameters),
|
||||||
safeCtrl(&acsParameters),
|
safeCtrl(&acsParameters),
|
||||||
ptgCtrl(&acsParameters),
|
ptgCtrl(&acsParameters),
|
||||||
@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
|||||||
}
|
}
|
||||||
case UPDATE_TLE: {
|
case UPDATE_TLE: {
|
||||||
if (size != 69 * 2) {
|
if (size != 69 * 2) {
|
||||||
return INVALID_PARAMETERS;
|
return HasActionsIF::INVALID_PARAMETERS;
|
||||||
}
|
}
|
||||||
ReturnValue_t result = updateTle(data, data + 69, false);
|
ReturnValue_t result = updateTle(data, data + 69, false);
|
||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
|||||||
}
|
}
|
||||||
std::memcpy(tle + 69, line2, 69);
|
std::memcpy(tle + 69, line2, 69);
|
||||||
actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
|
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: {
|
default: {
|
||||||
return HasActionsIF::INVALID_ACTION_ID;
|
return HasActionsIF::INVALID_ACTION_ID;
|
||||||
}
|
}
|
||||||
@ -176,7 +180,7 @@ void AcsController::performAttitudeControl() {
|
|||||||
mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
|
mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
|
||||||
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
|
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
|
||||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||||
&susDataProcessed, &attitudeEstimationData, &acsParameters);
|
&susDataProcessed, timeDelta, &attitudeEstimationData);
|
||||||
|
|
||||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
|
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
|
||||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||||
|
@ -113,6 +113,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
|
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
|
||||||
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
||||||
static const DeviceCommandId_t READ_TLE = 0x4;
|
static const DeviceCommandId_t READ_TLE = 0x4;
|
||||||
|
static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
|
||||||
|
|
||||||
ReturnValue_t initialize() override;
|
ReturnValue_t initialize() override;
|
||||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||||
|
File diff suppressed because it is too large
Load Diff
@ -1,14 +1,16 @@
|
|||||||
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
||||||
#define MULTIPLICATIVEKALMANFILTER_H_
|
#define MULTIPLICATIVEKALMANFILTER_H_
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <common/config/eive/resultClassIds.h>
|
||||||
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||||
#include "AcsParameters.h"
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||||
#include "eive/resultClassIds.h"
|
#include <mission/controller/acs/AcsParameters.h>
|
||||||
|
#include <mission/controller/acs/SensorValues.h>
|
||||||
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||||
|
|
||||||
class MultiplicativeKalmanFilter {
|
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
|
* means of the spacecraft attitude sensors
|
||||||
*
|
*
|
||||||
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
|
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
|
||||||
@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter {
|
|||||||
public:
|
public:
|
||||||
/* @brief: Constructor
|
/* @brief: Constructor
|
||||||
*/
|
*/
|
||||||
MultiplicativeKalmanFilter();
|
MultiplicativeKalmanFilter(AcsParameters *acsParameters);
|
||||||
virtual ~MultiplicativeKalmanFilter();
|
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
|
ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
|
||||||
* quaternion through the QUEST algorithm
|
const acsctrl::MgmDataProcessed *mgmData,
|
||||||
* @param: magneticField_ magnetic field vector in the body frame
|
const acsctrl::GyrDataProcessed *gyrData,
|
||||||
* sunDir_ sun direction vector in the body frame
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||||
* 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);
|
|
||||||
|
|
||||||
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
|
ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
|
||||||
* for the current step after the initalization
|
const acsctrl::MgmDataProcessed *mgmData,
|
||||||
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
|
const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
|
||||||
* rateGYRs_ Estimated satellite rotation rate from the
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||||
* 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);
|
|
||||||
|
|
||||||
enum MekfStatus : uint8_t {
|
void updateStandardDeviations(const AcsParameters *acsParameters);
|
||||||
UNINITIALIZED = 0,
|
|
||||||
NO_GYR_DATA = 1,
|
void setStrData(const double qX, const double qY, const double qZ, const double qW,
|
||||||
NO_MODEL_VECTORS = 2,
|
const bool valid);
|
||||||
NO_SUS_MGM_STR_DATA = 3,
|
|
||||||
COVARIANCE_INVERSION_FAILED = 4,
|
|
||||||
NOT_FINITE = 5,
|
|
||||||
INITIALIZED = 10,
|
|
||||||
RUNNING = 11,
|
|
||||||
};
|
|
||||||
|
|
||||||
// resetting Mekf
|
|
||||||
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_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_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
|
||||||
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
|
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
|
||||||
@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter {
|
|||||||
private:
|
private:
|
||||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||||
static constexpr double ZERO_VEC4[4] = {0, 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*/
|
enum MekfStatus : uint8_t {
|
||||||
double quaternion_STR_SB[4];
|
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*/
|
enum SensorAvailability : uint8_t {
|
||||||
double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
|
NONE = 0,
|
||||||
double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
SUS_MGM_STR = 1,
|
||||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
SUS_MGM = 2,
|
||||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
SUS_STR = 3,
|
||||||
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
|
MGM_STR = 4,
|
||||||
uint8_t sensorsAvail = 0;
|
SUS = 5,
|
||||||
|
MGM = 6,
|
||||||
|
STR = 7,
|
||||||
|
};
|
||||||
|
|
||||||
/*Outputs*/
|
MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
|
||||||
double quatBJ[4]; /* Output Quaternion */
|
|
||||||
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
struct StrData {
|
||||||
/*Parameter INIT*/
|
struct StrQuat {
|
||||||
/*Functions*/
|
double value[4] = {0, 0, 0, 0};
|
||||||
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
|
bool valid = false;
|
||||||
void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
|
} strQuat;
|
||||||
double quat[4], double satRotRate[3]);
|
} 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_ */
|
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|
||||||
|
@ -1,40 +1,27 @@
|
|||||||
#include "Navigation.h"
|
#include "Navigation.h"
|
||||||
|
|
||||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
|
||||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
||||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
Navigation::Navigation() {}
|
|
||||||
|
|
||||||
Navigation::~Navigation() {}
|
Navigation::~Navigation() {}
|
||||||
|
|
||||||
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
|
||||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
const acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
const acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||||
acsctrl::SusDataProcessed *susDataProcessed,
|
const acsctrl::SusDataProcessed *susDataProcessed,
|
||||||
acsctrl::AttitudeEstimationData *mekfData,
|
const double timeDelta,
|
||||||
AcsParameters *acsParameters) {
|
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
||||||
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
multiplicativeKalmanFilter.setStrData(
|
||||||
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
|
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
||||||
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
|
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
|
||||||
|
sensorValues->strSet.caliQx.isValid());
|
||||||
|
|
||||||
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
|
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
|
||||||
mekfStatus = multiplicativeKalmanFilter.init(
|
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
|
||||||
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
|
gyrDataProcessed, attitudeEstimationData);
|
||||||
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
|
|
||||||
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
|
|
||||||
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
|
|
||||||
acsParameters);
|
|
||||||
return mekfStatus;
|
return mekfStatus;
|
||||||
} else {
|
} else {
|
||||||
mekfStatus = multiplicativeKalmanFilter.mekfEst(
|
mekfStatus = multiplicativeKalmanFilter.mekfEst(
|
||||||
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
|
susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
|
||||||
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);
|
|
||||||
return mekfStatus;
|
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) {
|
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
|
||||||
return sgp4Propagator.initialize(line1, line2);
|
return sgp4Propagator.initialize(line1, line2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
|
||||||
|
multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
|
||||||
|
}
|
||||||
|
@ -5,24 +5,24 @@
|
|||||||
#include <mission/acs/defs.h>
|
#include <mission/acs/defs.h>
|
||||||
#include <mission/controller/acs/AcsParameters.h>
|
#include <mission/controller/acs/AcsParameters.h>
|
||||||
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
||||||
#include <mission/controller/acs/SensorProcessing.h>
|
|
||||||
#include <mission/controller/acs/SensorValues.h>
|
#include <mission/controller/acs/SensorValues.h>
|
||||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||||
|
|
||||||
class Navigation {
|
class Navigation {
|
||||||
public:
|
public:
|
||||||
Navigation();
|
Navigation(AcsParameters *acsParameters);
|
||||||
virtual ~Navigation();
|
virtual ~Navigation();
|
||||||
|
|
||||||
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
|
ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
|
||||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
const acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
const acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||||
acsctrl::SusDataProcessed *susDataProcessed,
|
const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
|
||||||
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
|
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||||
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
|
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
|
||||||
|
|
||||||
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
|
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
|
||||||
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
|
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
|
||||||
|
void updateMekfStandardDeviations(const AcsParameters *acsParameters);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
private:
|
private:
|
||||||
|
Loading…
Reference in New Issue
Block a user