diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index ac2e5752..a8fd1576 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -47,71 +47,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
         mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
         mgm4ValueNoBias[3] = {0, 0, 0};
-  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
-        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
         mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
+  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
+        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
 
   if (mgm0valid) {
-    VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
+                                      mgm0ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
+                                      mgm0ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
                                       mgm0ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
-                                      mgm0ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm1valid) {
-    VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
+                                      mgm1ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
+                                      mgm1ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
                                       mgm1ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
-                                      mgm1ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm2valid) {
-    VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
+                                      mgm2ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
+                                      mgm2ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
                                       mgm2ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
-                                      mgm2ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm3valid) {
-    VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
+                                      mgm3ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
+                                      mgm3ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
                                       mgm3ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
-                                      mgm3ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm4valid) {
-    float mgm4ValueNT[3];
-    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3);  // uT to nT
-    VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
+    float mgm4ValueUT[3];
+    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3);  // nT to uT
+    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
+                                      mgm4ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
                                       mgm4ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
                                       mgm4ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
-                                      mgm4ValueBody, 3, 3, 1);
+
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
+      sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
     }
   }
@@ -148,15 +149,15 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   {
     PoolReadGuard pg(mgmDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm0vec.setValid(mgm0valid);
-      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm1vec.setValid(mgm1valid);
-      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm2vec.setValid(mgm2valid);
-      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm3vec.setValid(mgm3valid);
-      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm4vec.setValid(mgm4valid);
       std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
       mgmDataProcessed->mgmVecTot.setValid(true);