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

Merge pull request 'Fix MEKF Inits' (#896) from fix-mekf-inits into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
Reviewed-on: #896
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2024-05-29 10:12:35 +02:00
commit e1f2514596
4 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -22,6 +22,8 @@ will consitute of a breaking change warranting a new major release:
- Small fix for transition failure handling of the MPSoC when the `START_MPSOC` action command - Small fix for transition failure handling of the MPSoC when the `START_MPSOC` action command
to the supervisor fails. to the supervisor fails.
- Fixed inits of arrays within the `MEKF` not being zeros.
- Corrected sigma of STR for `MEKF`.
# [v8.0.0] 2024-05-13 # [v8.0.0] 2024-05-13

2
mission/controller/acs/AcsParameters.h
View File

@ -941,7 +941,7 @@ class AcsParameters : public HasParametersIF {
} sunModelParameters; } sunModelParameters;
struct KalmanFilterParameters { struct KalmanFilterParameters {
double sensorNoiseStr = 0.1 * DEG2RAD; double sensorNoiseStr = 0.0028 * DEG2RAD;
double sensorNoiseSus = 8. * DEG2RAD; double sensorNoiseSus = 8. * DEG2RAD;
double sensorNoiseMgm = 4. * DEG2RAD; double sensorNoiseMgm = 4. * DEG2RAD;
double sensorNoiseGyr = 0.1 * DEG2RAD; double sensorNoiseGyr = 0.1 * DEG2RAD;

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

@ -114,12 +114,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
return result; return result;
} }
double measSensMatrix[matrixDimensionFactor][6] = {{0}}, double measSensMatrix[matrixDimensionFactor][6] = {},
measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {},
measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0}; measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {};
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
double kalmanGain[6][matrixDimensionFactor] = {{0}}; double kalmanGain[6][matrixDimensionFactor];
std::memset(kalmanGain, 0, sizeof(kalmanGain));
result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData); result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
reset(attitudeEstimationData); reset(attitudeEstimationData);
@ -342,10 +343,11 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain(
double *measSensMatrix, double *measCovMatrix, double *kalmanGain, double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
acsctrl::AttitudeEstimationData *attitudeEstimationData) { acsctrl::AttitudeEstimationData *attitudeEstimationData) {
// Kalman Gain: K = P * H' / (H * P * H' + R) // Kalman Gain: K = P * H' / (H * P * H' + R)
double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {},
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {};
residualCov[6][matrixDimensionFactor] = {{0}}, double residualCov[6][matrixDimensionFactor], measSensMatrixTransposed[6][matrixDimensionFactor];
measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}}; std::memset(residualCov, 0, sizeof(residualCov));
std::memset(measSensMatrixTransposed, 0, sizeof(measSensMatrixTransposed));
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed, MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
matrixDimensionFactor, 6); matrixDimensionFactor, 6);
@ -382,8 +384,7 @@ void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *me
void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec, void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec,
double *estVec) { double *estVec) {
double stateVecErr[6] = {0, 0, 0, 0, 0, 0}; double stateVecErr[6] = {0, 0, 0, 0, 0, 0}, plantOutputDiff[matrixDimensionFactor] = {};
double plantOutputDiff[matrixDimensionFactor] = {0};
VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor); VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor);
MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6, MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6,
matrixDimensionFactor, 1); matrixDimensionFactor, 1);

2
tmtc

@ -1 +1 @@
Subproject commit 8419a4edd773eb0eedba912f4678a5e28567e3ed Subproject commit 9a06c64dfac3f4283c2d5af72a9c095f4726480b