diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index c6431aa2..d4f5f1e1 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[9][6] {}, + measCovMatrix[9][9]{}, + measVec[9] = {}, estVec[9] {}; kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); - double kalmanGain[6][matrixDimensionFactor] = {{0}}; + double kalmanGain[6][9] = {}; 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[6][matrixDimensionFactor] = {{0}}, - invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}}, - residualCov[6][matrixDimensionFactor] = {{0}}, - measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}}; + double kalmanGainDen[6][9] {}, + invKalmanGainDen[9][9] {}, + residualCov[6][9] {}, + measSensMatrixTransposed[6][9]{}; MatrixOperations::transpose(measSensMatrix, *measSensMatrixTransposed, matrixDimensionFactor, 6);