From 6025ea5663e4f46c08140abccaa709f4bf9e8539 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 14 May 2024 15:35:20 +0200 Subject: [PATCH 1/5] fixed ub with initalization --- .../acs/MultiplicativeKalmanFilter.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index b66f0531..e938b0c4 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -114,12 +114,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( return result; } - double measSensMatrix[matrixDimensionFactor][6] = {{0}}, - measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, - measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0}; + double measSensMatrix[matrixDimensionFactor][6] = {}, + measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {}, + measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {}; kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); - double kalmanGain[6][matrixDimensionFactor] = {{0}}; + double kalmanGain[6][matrixDimensionFactor] = {}; result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData); if (result != returnvalue::OK) { reset(attitudeEstimationData); @@ -342,10 +342,10 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain( double *measSensMatrix, double *measCovMatrix, double *kalmanGain, acsctrl::AttitudeEstimationData *attitudeEstimationData) { // Kalman Gain: K = P * H' / (H * P * H' + R) - double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, - invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, - residualCov[6][matrixDimensionFactor] = {{0}}, - measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}}; + double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}, + invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}, + residualCov[6][matrixDimensionFactor] = {}, + measSensMatrixTransposed[6][matrixDimensionFactor] = {}; MatrixOperations::transpose(measSensMatrix, *measSensMatrixTransposed, matrixDimensionFactor, 6); @@ -383,7 +383,7 @@ void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *me void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec) { double stateVecErr[6] = {0, 0, 0, 0, 0, 0}; - double plantOutputDiff[matrixDimensionFactor] = {0}; + double plantOutputDiff[matrixDimensionFactor] = {}; VectorOperations::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor); MatrixOperations::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6, matrixDimensionFactor, 1); From 32271a98fffe91d685a25de50b3bff5211ccfca2 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 15 May 2024 10:04:04 +0200 Subject: [PATCH 2/5] go home compiler, you're drunk --- .../controller/acs/MultiplicativeKalmanFilter.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index e938b0c4..fc61ca0e 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -119,7 +119,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {}; kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); - double kalmanGain[6][matrixDimensionFactor] = {}; + double kalmanGain[6][matrixDimensionFactor]; + std::memset(kalmanGain, 0, sizeof(kalmanGain)); result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData); if (result != returnvalue::OK) { reset(attitudeEstimationData); @@ -343,9 +344,10 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain( acsctrl::AttitudeEstimationData *attitudeEstimationData) { // Kalman Gain: K = P * H' / (H * P * H' + R) double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}, - invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}, - residualCov[6][matrixDimensionFactor] = {}, - measSensMatrixTransposed[6][matrixDimensionFactor] = {}; + invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}; + double residualCov[6][matrixDimensionFactor], measSensMatrixTransposed[6][matrixDimensionFactor]; + std::memset(residualCov, 0, sizeof(residualCov)); + std::memset(measSensMatrixTransposed, 0, sizeof(measSensMatrixTransposed)); MatrixOperations::transpose(measSensMatrix, *measSensMatrixTransposed, matrixDimensionFactor, 6); @@ -382,8 +384,7 @@ void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *me void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec) { - double stateVecErr[6] = {0, 0, 0, 0, 0, 0}; - double plantOutputDiff[matrixDimensionFactor] = {}; + double stateVecErr[6] = {0, 0, 0, 0, 0, 0}, plantOutputDiff[matrixDimensionFactor] = {}; VectorOperations::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor); MatrixOperations::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6, matrixDimensionFactor, 1); From ba219fbe7d2d39061573508739d86f029d2be54d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 15 May 2024 10:05:32 +0200 Subject: [PATCH 3/5] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 360ff839..1bea778b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Fixed inits of arrays within the `MEKF` not being zeros. + # [v8.0.0] 2024-05-13 - `eive-tmtc` v7.0.0 From 4720ab9a356496e1e71477ed61e422903370dc25 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 15 May 2024 11:07:12 +0200 Subject: [PATCH 4/5] corrected str sigma --- mission/controller/acs/AcsParameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index dfb99c2c..4ed80e83 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -941,7 +941,7 @@ class AcsParameters : public HasParametersIF { } sunModelParameters; struct KalmanFilterParameters { - double sensorNoiseStr = 0.1 * DEG2RAD; + double sensorNoiseStr = 0.0028 * DEG2RAD; double sensorNoiseSus = 8. * DEG2RAD; double sensorNoiseMgm = 4. * DEG2RAD; double sensorNoiseGyr = 0.1 * DEG2RAD; From aa5a148800cab5b91e8a9a7f1acf4e0a4c62bcf6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 15 May 2024 11:07:46 +0200 Subject: [PATCH 5/5] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1bea778b..1ea92439 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ will consitute of a breaking change warranting a new major release: ## Fixed - Fixed inits of arrays within the `MEKF` not being zeros. +- Corrected sigma of STR for `MEKF`. # [v8.0.0] 2024-05-13