diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp
index 95a4046d..3e4a22c4 100644
--- a/mission/controller/acs/AttitudeEstimation.cpp
+++ b/mission/controller/acs/AttitudeEstimation.cpp
@@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
     qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   } else {
     // Scalar Part
     qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   }
   // Low Pass