diff --git a/CHANGELOG.md b/CHANGELOG.md index d7df147d..67fea724 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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 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 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; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index b66f0531..fc61ca0e 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -114,12 +114,13 @@ 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]; + std::memset(kalmanGain, 0, sizeof(kalmanGain)); result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData); if (result != returnvalue::OK) { reset(attitudeEstimationData); @@ -342,10 +343,11 @@ 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] = {}; + 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] = {0}; + 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); diff --git a/tmtc b/tmtc index 8419a4ed..9a06c64d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8419a4edd773eb0eedba912f4678a5e28567e3ed +Subproject commit 9a06c64dfac3f4283c2d5af72a9c095f4726480b