diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 0cb4f330..953a91a1 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -20,6 +20,7 @@ AcsController::AcsController(object_id_t objectId)
       gyrDataProcessed(this),
       gpsDataProcessed(this),
       mekfData(this),
+      ctrlValData(this),
       actuatorCmdData(this) {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
@@ -48,7 +49,9 @@ void AcsController::performControlOperation() {
           case SUBMODE_DETUMBLE:
             performDetumble();
             break;
-          case SUBMODE_PTG_GS:
+          case SUBMODE_PTG_TARGET:
+          case SUBMODE_PTG_NADIR:
+          case SUBMODE_PTG_INERTIAL:
             performPointingCtrl();
             break;
         }
@@ -80,7 +83,7 @@ void AcsController::performControlOperation() {
 
   // DEBUG : REMOVE AFTER COMPLETION
   mode = MODE_ON;
-  submode = SUBMODE_DETUMBLE;
+  submode = SUBMODE_SAFE;
   // DEBUG END
 }
 
@@ -90,7 +93,6 @@ void AcsController::performSafe() {
   //	another place since we have to do it for every mode regardless of safe or not
 
   ACS::SensorValues sensorValues;
-  ACS::OutputValues outputValues;
 
   timeval now;
   Clock::getClock_timeval(&now);
@@ -98,38 +100,54 @@ void AcsController::performSafe() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
+                     &mekfData, &validMekf);
 
   // Give desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   //	IF MEKF is working
-  double magMomMtq[3] = {0, 0, 0};
+  double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
   bool magMomMtqValid = false;
   if (validMekf == returnvalue::OK) {
-    safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
-                      (outputValues.magFieldModel), &(outputValues.magFieldModelValid),
-                      (outputValues.sunDirModel), &(outputValues.sunDirModelValid),
-                      (outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
-                      satRateSafe, magMomMtq, &magMomMtqValid);
+    safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
+                      mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
+                      susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
+                      mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
+                      sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   } else {
-    safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
-                        outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
-                        outputValues.magFieldEst, &(outputValues.magFieldEstValid),
-                        outputValues.magneticFieldVectorDerivative,
-                        &(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
-                        satRateSafe, magMomMtq, &magMomMtqValid);
+    safeCtrl.safeNoMekf(
+        now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
+        susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
+        mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+        mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
+        sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
   double dipolCmdUnits[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
 
+  {
+    PoolReadGuard pg(&ctrlValData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      double zeroQuat[4] = {0, 0, 0, 0};
+      std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
+      ctrlValData.tgtQuat.setValid(false);
+      std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
+      ctrlValData.errQuat.setValid(false);
+      ctrlValData.errAng.value = errAng;
+      ctrlValData.errAng.setValid(true);
+      ctrlValData.setValidity(true, false);
+    }
+  }
+
   //	Detumble check and switch
-  if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
-                                           acsParameters.detumbleParameter.omegaDetumbleStart) {
+  if (mekfData.satRotRateMekf.isValid() &&
+      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
+          acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
-  } else if (outputValues.satRateEstValid &&
-             VectorOperations<double>::norm(outputValues.satRateEst, 3) >
+  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
+             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
                  acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
   } else {
@@ -139,12 +157,31 @@ void AcsController::performSafe() {
     submode = SUBMODE_DETUMBLE;
     detumbleCounter = 0;
   }
-  //  	commanding.magnetorquesDipol();
+
+  {
+    PoolReadGuard pg(&actuatorCmdData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      double zeroVec[3] = {0, 0, 0, 0};
+      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
+      actuatorCmdData.rwTargetTorque.setValid(false);
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
+      actuatorCmdData.rwTargetSpeed.setValid(false);
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
+      actuatorCmdData.mtqTargetDipole.setValid(true);
+      actuatorCmdData.setValidity(true, false);
+    }
+  }
+  //  {
+  //	  PoolReadGuard pg(&dipoleSet);
+  //	  MutexGuard mg(torquer::lazyLock());
+  //	  torquer::NEW_ACTUATION_FLAG = true;
+  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  //torqueDuration);
+  //  }
 }
 
 void AcsController::performDetumble() {
   ACS::SensorValues sensorValues;
-  ACS::OutputValues outputValues;
 
   timeval now;
   Clock::getClock_timeval(&now);
@@ -152,20 +189,22 @@ void AcsController::performDetumble() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
+                     &mekfData, &validMekf);
 
   double magMomMtq[3] = {0, 0, 0};
-  detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
-                   &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
-                   &outputValues.magFieldEstValid, magMomMtq);
+  detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
+                   mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
+                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
   double dipolCmdUnits[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
 
-  if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
-                                           acsParameters.detumbleParameter.omegaDetumbleEnd) {
+  if (mekfData.satRotRateMekf.isValid() &&
+      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
+          acsParameters.detumbleParameter.omegaDetumbleEnd) {
     detumbleCounter++;
-  } else if (outputValues.satRateEstValid &&
-             VectorOperations<double>::norm(outputValues.satRateEst, 3) <
+  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
+             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
                  acsParameters.detumbleParameter.omegaDetumbleEnd) {
     detumbleCounter++;
   } else {
@@ -175,11 +214,31 @@ void AcsController::performDetumble() {
     submode = SUBMODE_SAFE;
     detumbleCounter = 0;
   }
+
+  {
+    PoolReadGuard pg(&actuatorCmdData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      double zeroVec[3] = {0, 0, 0, 0};
+      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
+      actuatorCmdData.rwTargetTorque.setValid(false);
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
+      actuatorCmdData.rwTargetSpeed.setValid(false);
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
+      actuatorCmdData.mtqTargetDipole.setValid(true);
+      actuatorCmdData.setValidity(true, false);
+    }
+  }
+  //  {
+  //	  PoolReadGuard pg(&dipoleSet);
+  //	  MutexGuard mg(torquer::lazyLock());
+  //	  torquer::NEW_ACTUATION_FLAG = true;
+  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  // torqueDuration);
+  //  }
 }
 
 void AcsController::performPointingCtrl() {
   ACS::SensorValues sensorValues;
-  ACS::OutputValues outputValues;
 
   timeval now;
   Clock::getClock_timeval(&now);
@@ -187,12 +246,14 @@ void AcsController::performPointingCtrl() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
+                     &mekfData, &validMekf);
 
   double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
-  guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
-  double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
-  guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
+  guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
+  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
+         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
+  guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@@ -207,11 +268,29 @@ void AcsController::performPointingCtrl() {
       &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
       rwTrqNs, cmdSpeedRws);
   double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
-  ptgCtrl.ptgDesaturation(
-      outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+  ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+                          mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
+                          &(sensorValues.rw2Set.currSpeed.value),
+                          &(sensorValues.rw3Set.currSpeed.value),
+                          &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
   actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
+
+  {
+    PoolReadGuard pg(&actuatorCmdData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double));
+      actuatorCmdData.setValidity(true, true);
+    }
+  }
+  //  {
+  //	  PoolReadGuard pg(&dipoleSet);
+  //	  MutexGuard mg(torquer::lazyLock());
+  //	  torquer::NEW_ACTUATION_FLAG = true;
+  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  // torqueDuration);
+  //  }
 }
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
@@ -286,6 +365,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
   poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
+  // Ctrl Values
+  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
+  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
   // Actuator CMD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
@@ -312,6 +396,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
       return &gpsDataProcessed;
     case acsctrl::MEKF_DATA:
       return &mekfData;
+    case acsctrl::CTRL_VAL_DATA:
+      return &ctrlValData;
     case acsctrl::ACTUATOR_CMD_DATA:
       return &actuatorCmdData;
     default:
@@ -328,7 +414,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
       return INVALID_SUBMODE;
     }
   } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
-    if ((submode > 5) || (submode < 2)) {
+    if ((submode > 6) || (submode < 2)) {
       return INVALID_SUBMODE;
     } else {
       return returnvalue::OK;
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index dcd78621..18c8d824 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -8,7 +8,6 @@
 #include "acs/ActuatorCmd.h"
 #include "acs/Guidance.h"
 #include "acs/Navigation.h"
-#include "acs/OutputValues.h"
 #include "acs/SensorProcessing.h"
 #include "acs/control/Detumble.h"
 #include "acs/control/PtgCtrl.h"
@@ -27,8 +26,9 @@ class AcsController : public ExtendedControllerBase {
 
   static const Submode_t SUBMODE_SAFE = 2;
   static const Submode_t SUBMODE_DETUMBLE = 3;
-  static const Submode_t SUBMODE_PTG_GS = 4;
+  static const Submode_t SUBMODE_PTG_TARGET = 4;
   static const Submode_t SUBMODE_PTG_NADIR = 5;
+  static const Submode_t SUBMODE_PTG_INERTIAL = 6;
 
  protected:
   void performSafe();
@@ -83,7 +83,7 @@ class AcsController : public ExtendedControllerBase {
   PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
   PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
   PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
-  PoolEntry<float> mgmVecTotDer = PoolEntry<float>(3);
+  PoolEntry<double> mgmVecTotDer = PoolEntry<double>(3);
   PoolEntry<double> magIgrf = PoolEntry<double>(3);
 
   // SUSs
@@ -115,9 +115,9 @@ class AcsController : public ExtendedControllerBase {
   PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
   PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
   PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
-  PoolEntry<float> susVecTot = PoolEntry<float>(3);
-  PoolEntry<float> susVecTotDer = PoolEntry<float>(3);
-  PoolEntry<float> sunIjk = PoolEntry<float>(3);
+  PoolEntry<double> susVecTot = PoolEntry<double>(3);
+  PoolEntry<double> susVecTotDer = PoolEntry<double>(3);
+  PoolEntry<double> sunIjk = PoolEntry<double>(3);
 
   // GYRs
   acsctrl::GyrDataRaw gyrDataRaw;
@@ -144,6 +144,12 @@ class AcsController : public ExtendedControllerBase {
   PoolEntry<double> quatMekf = PoolEntry<double>(4);
   PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
 
+  // Ctrl Values
+  acsctrl::CtrlValData ctrlValData;
+  PoolEntry<double> tgtQuat = PoolEntry<double>(4);
+  PoolEntry<double> errQuat = PoolEntry<double>(4);
+  PoolEntry<double> errAng = PoolEntry<double>();
+
   // Actuator CMD
   acsctrl::ActuatorCmdData actuatorCmdData;
   PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index 0dba2016..2a0a9425 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -5,23 +5,21 @@
  *      Author: Robin Marquardt
  */
 
-
 #include "ActuatorCmd.h"
-#include "util/MathOperations.h"
-#include "util/CholeskyDecomposition.h"
-#include <cmath>
+
 #include <fsfw/globalfunctions/constants.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
 
-ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
-	acsParameters = *acsParameters_;
-}
+#include <cmath>
 
-ActuatorCmd::~ActuatorCmd(){
+#include "util/CholeskyDecomposition.h"
+#include "util/MathOperations.h"
 
-}
+ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
+
+ActuatorCmd::~ActuatorCmd() {}
 
 void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
                                 const int32_t *speedRw2, const int32_t *speedRw3,
@@ -57,22 +55,20 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
 }
 
 void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
+  //	Convert to Unit frame
+  MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
+                                     dipolMoment, dipolMomentUnits, 3, 3, 1);
+  //  Scaling along largest element if dipol exceeds maximum
+  double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
+  double maxValue = 0;
+  for (int i = 0; i < 3; i++) {
+    if (abs(dipolMomentUnits[i]) > maxDipol) {
+      maxValue = abs(dipolMomentUnits[i]);
+    }
+  }
 
-//	Convert to Unit frame
-	MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1);
-//  Scaling along largest element if dipol exceeds maximum
-	double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
-	double maxValue = 0;
-	for (int i = 0; i < 3; i++) {
-		if (abs(dipolMomentUnits[i]) > maxDipol) {
-			maxValue = abs(dipolMomentUnits[i]);
-		}
-	}
-
-	if (maxValue > maxDipol) {
-
-		double scalingFactor = maxDipol / maxValue;
-		VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
-
-	}
+  if (maxValue > maxDipol) {
+    double scalingFactor = maxDipol / maxValue;
+    VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
+  }
 }
diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h
index 1da2c89a..5cb3ff00 100644
--- a/mission/controller/acs/ActuatorCmd.h
+++ b/mission/controller/acs/ActuatorCmd.h
@@ -8,42 +8,40 @@
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_
 
-
 #include "AcsParameters.h"
-#include "SensorProcessing.h"
 #include "MultiplicativeKalmanFilter.h"
+#include "SensorProcessing.h"
 #include "SensorValues.h"
-#include "OutputValues.h"
 
-class ActuatorCmd{
-public:
-	ActuatorCmd(AcsParameters *acsParameters_); //Input mode ?
-	virtual ~ActuatorCmd();
+class ActuatorCmd {
+ public:
+  ActuatorCmd(AcsParameters *acsParameters_);  // Input mode ?
+  virtual ~ActuatorCmd();
 
-	/*
-	 * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction wheels, also
-	 * 								will calculate the needed revolutions per minute for the RWs, which will be given
-	 * 								as Input to the RWs
-	 * @param:	rwTrqIn 			given torque from pointing controller
-	 * 			rwTrqNS				Nullspace torque
-	 * 			rwCmdSpeed			output revolutions per minute for every reaction wheel
-	 */
-        void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
-                           const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn,
-                           const double *rwTrqNS, double *rwCmdSpeed);
+  /*
+   * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
+   * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
+   * as Input to the RWs
+   * @param:	rwTrqIn 			given torque from pointing controller
+   * 			rwTrqNS				Nullspace torque
+   * 			rwCmdSpeed			output revolutions per minute for every
+   * reaction wheel
+   */
+  void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
+                     const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS,
+                     double *rwCmdSpeed);
 
-        /*
-	 * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
-	 *
-	 * @param:	dipolMoment 		given dipol moment in spacecraft frame
-	 * 			dipolMomentUnits	resulting dipol moment for every unit
-	 */
-	void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
+  /*
+   * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
+   *
+   * @param:	dipolMoment 		given dipol moment in spacecraft frame
+   * 			dipolMomentUnits	resulting dipol moment for every unit
+   */
+  void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
 
-protected:
-
-private:
-	AcsParameters acsParameters;
+ protected:
+ private:
+  AcsParameters acsParameters;
 };
 
 #endif /* ACTUATORCMD_H_ */
diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt
index c949a067..a379197d 100644
--- a/mission/controller/acs/CMakeLists.txt
+++ b/mission/controller/acs/CMakeLists.txt
@@ -6,7 +6,6 @@ target_sources(
   		  Igrf13Model.cpp
   		  MultiplicativeKalmanFilter.cpp
   		  Navigation.cpp
-  		  OutputValues.cpp
   		  SensorProcessing.cpp
   		  SensorValues.cpp
   		  SusConverter.cpp)
diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index f6ebee5f..35a7295a 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -7,6 +7,7 @@
 
 #include "Guidance.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
@@ -29,8 +30,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
   //	memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
 }
 
-void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
-                             timeval now, double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                             acsctrl::SusDataProcessed *susDataProcessed, timeval now,
+                             double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation
   //-------------------------------------------------------------------------------------
@@ -77,10 +79,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double quatBJ[4] = {0, 0, 0, 0};
-  quatBJ[0] = outputValues->quatMekfBJ[0];
-  quatBJ[1] = outputValues->quatMekfBJ[1];
-  quatBJ[2] = outputValues->quatMekfBJ[2];
-  quatBJ[3] = outputValues->quatMekfBJ[3];
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+
   QuaternionOperations::toDcm(quatBJ, dcmBJ);
   MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
 
@@ -137,23 +137,16 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
     double sunDirJ[3] = {0, 0, 0};
     double sunDirB[3] = {0, 0, 0};
 
-    if (outputValues->sunDirModelValid) {
-      sunDirJ[0] = outputValues->sunDirModel[0];
-      sunDirJ[1] = outputValues->sunDirModel[1];
-      sunDirJ[2] = outputValues->sunDirModel[2];
+    if (susDataProcessed->sunIjkModel.isValid()) {
+      std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
       MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-    }
-
-    else {
-      sunDirB[0] = outputValues->sunDirEst[0];
-      sunDirB[1] = outputValues->sunDirEst[1];
-      sunDirB[2] = outputValues->sunDirEst[2];
+    } else {
+      std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
     }
 
     double exclAngle = acsParameters.strParameters.exclusionAngle,
            blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
            blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
-
     double sightAngleSun =
         VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
 
@@ -190,8 +183,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   }
 }
 
-void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
-                          double refSatRate[3], double quatError[3], double deltaRate[3]) {
+void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
   double quatRef[4] = {0, 0, 0, 0};
   quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
   quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
@@ -199,9 +192,7 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
   quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
 
   double satRate[3] = {0, 0, 0};
-  satRate[0] = outputValues->satRateMekf[0];
-  satRate[1] = outputValues->satRateMekf[1];
-  satRate[2] = outputValues->satRateMekf[2];
+  std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
   VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
   // valid checks ?
   double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
@@ -209,7 +200,6 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
                                {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
                                {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
 
-  double quatErrorComplete[4] = {0, 0, 0, 0};
   MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
 
   if (quatErrorComplete[3] < 0) {
@@ -225,8 +215,8 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
 }
 
 void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
-  if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() &&
-      sensorValues->rw4Set.isValid()) {
+  if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
+      sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
     rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
     rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
     rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h
index bf53d767..4c699561 100644
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@@ -8,35 +8,37 @@
 #ifndef GUIDANCE_H_
 #define GUIDANCE_H_
 
-
-#include "AcsParameters.h"
-#include "SensorValues.h"
-#include "OutputValues.h"
 #include <time.h>
 
+#include "../controllerdefinitions/AcsCtrlDefinitions.h"
+#include "AcsParameters.h"
+#include "SensorValues.h"
 
 class Guidance {
-public:
-	Guidance(AcsParameters *acsParameters_);
-	virtual ~Guidance();
+ public:
+  Guidance(AcsParameters *acsParameters_);
+  virtual ~Guidance();
 
-	void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
+  void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
 
-	// Function to get the target quaternion and refence rotation rate from gps position and position of the ground station
-	void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
-			double targetQuat[4], double refSatRate[3]);
+  // Function to get the target quaternion and refence rotation rate from gps position and position
+  // of the ground station
+  void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                     acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
+                     double refSatRate[3]);
 
-	// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
-	void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );
+  // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
+  // desired
+  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+                  double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
 
-//	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel
-//	maybe can be done in "commanding.h"
-	void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv);
+  //	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
+  // reation wheel 	maybe can be done in "commanding.h"
+  void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 
-
-private:
-	AcsParameters acsParameters;
-	bool strBlindAvoidFlag = false;
+ private:
+  AcsParameters acsParameters;
+  bool strBlindAvoidFlag = false;
 };
 
 #endif /* ACS_GUIDANCE_H_ */
diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp
index 75d3c9a1..fcd95b68 100644
--- a/mission/controller/acs/Igrf13Model.cpp
+++ b/mission/controller/acs/Igrf13Model.cpp
@@ -6,120 +6,120 @@
  */
 
 #include "Igrf13Model.h"
-#include "util/MathOperations.h"
-#include <math.h>
-#include <stdint.h>
-#include <string.h>
-//#include <time.h>
+
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
 
+#include "util/MathOperations.h"
 
-Igrf13Model::Igrf13Model(){
-}
-Igrf13Model::~Igrf13Model(){
-}
+Igrf13Model::Igrf13Model() {}
+Igrf13Model::~Igrf13Model() {}
 
 void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
-		const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) {
+                               const double altitude, timeval timeOfMagMeasurement,
+                               double* magFieldModelInertial) {
+  double phi = longitude, theta = gcLatitude;  // geocentric
+  /* Here is the co-latitude needed*/
+  theta -= 90 * Math::PI / 180;
+  theta *= (-1);
 
-	double phi = longitude, theta = gcLatitude; //geocentric
-	/* Here is the co-latitude needed*/
-	theta -= 90*Math::PI/180;
-	theta *= (-1);
+  double rE = 6371200.0;  // radius earth [m]
+  /* Predefine recursive associated Legendre polynomials */
+  double P11 = 1;
+  double P10 = P11;    // P10 = P(n-1,m-0)
+  double dP11 = 0;     // derivative
+  double dP10 = dP11;  // derivative
 
-	double rE = 6371200.0; // radius earth [m]
-	/* Predefine recursive associated Legendre polynomials */
-	double P11 = 1;
-	double P10 = P11; //P10 = P(n-1,m-0)
-	double dP11 = 0; //derivative
-	double dP10 = dP11; //derivative
+  double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
 
-	double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
+  for (int m = 0; m <= igrfOrder; m++) {
+    for (int n = 1; n <= igrfOrder; n++) {
+      if (m <= n) {
+        /* Calculation of Legendre Polynoms (normalised) */
+        if (n == m) {
+          P2 = sin(theta) * P11;
+          dP2 = sin(theta) * dP11 - cos(theta) * P11;
+          P11 = P2;
+          P10 = P11;
+          P20 = 0;
+          dP11 = dP2;
+          dP10 = dP11;
+          dP20 = 0;
+        } else if (n == 1) {
+          P2 = cos(theta) * P10;
+          dP2 = cos(theta) * dP10 - sin(theta) * P10;
+          P20 = P10;
+          P10 = P2;
+          dP20 = dP10;
+          dP10 = dP2;
+        } else {
+          K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3));
+          P2 = cos(theta) * P10 - K * P20;
+          dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
+          P20 = P10;
+          P10 = P2;
+          dP20 = dP10;
+          dP10 = dP2;
+        }
+        /* gradient of scalar potential towards radius */
+        magFieldModel[0] +=
+            pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
+            ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
+        /* gradient of scalar potential towards phi */
+        magFieldModel[1] +=
+            pow(rE / (altitude + rE), (n + 2)) *
+            ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
+        /* gradient of scalar potential towards theta */
+        magFieldModel[2] +=
+            pow(rE / (altitude + rE), (n + 2)) *
+            ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
+      }
+    }
+  }
 
-	for (int m = 0; m <= igrfOrder; m++) {
-		for (int n = 1; n <= igrfOrder; n++) {
-			if (m <= n) {
-				/* Calculation of Legendre Polynoms (normalised) */
-				if (n == m) {
-					P2 = sin(theta) * P11;
-					dP2 = sin(theta) * dP11 - cos(theta) * P11;
-					P11 = P2;
-					P10 = P11;
-					P20 = 0;
-					dP11 = dP2;
-					dP10 = dP11;
-					dP20 = 0;
-				} else if (n == 1) {
-					P2 = cos(theta) * P10;
-					dP2 = cos(theta) * dP10 - sin(theta) * P10;
-					P20 = P10;
-					P10 = P2;
-					dP20 = dP10;
-					dP10 = dP2;
-				} else {
-					K = (pow((n - 1), 2) - pow(m, 2))
-							/ ((2 * n - 1) * (2 * n - 3));
-					P2 = cos(theta) * P10 - K * P20;
-					dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
-					P20 = P10;
-					P10 = P2;
-					dP20 = dP10;
-					dP10 = dP2;
-				}
-			/* gradient of scalar potential towards radius */
-			magFieldModel[0]+=pow(rE/(altitude+rE),(n+2))*(n+1)*
-					((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*P2);
-			/* gradient of scalar potential towards phi */
-			magFieldModel[1]+=pow(rE/(altitude+rE),(n+2))*
-					((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*dP2);
-			/* gradient of scalar potential towards theta */
-			magFieldModel[2]+=pow(rE/(altitude+rE),(n+2))*
-					((-updatedG[m][n-1]*sin(m*phi) + updatedH[m][n-1]*cos(m*phi))*P2*m);
-			}
-		}
-	}
+  magFieldModel[1] *= -1;
+  magFieldModel[2] *= (-1 / sin(theta));
 
-	magFieldModel[1] *= -1;
-	magFieldModel[2] *= (-1 / sin(theta));
+  /* Next step: transform into inertial KOS (IJK)*/
+  // Julean Centuries
+  double JD2000Floor = 0;
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
+  JD2000Floor = floor(JD2000);
+  double JC2000 = JD2000Floor / 36525;
 
-	/* Next step: transform into inertial KOS (IJK)*/
-	//Julean Centuries
-	double JD2000Floor = 0;
-	double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
-	JD2000Floor = floor(JD2000);
-	double JC2000 = JD2000Floor / 36525;
+  double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
+               0.000000026 * pow(JC2000, 3);  // greenwich sidereal time
+  gst *= PI / 180;                            // convert to radians
+  double sec =
+      (JD2000 - JD2000Floor) * 86400;  // Seconds on this day (Universal time) // FROM GPS ?
+  double omega0 = 0.00007292115;       // mean angular velocity earth [rad/s]
+  gst += omega0 * sec;
 
-	double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000,2)
-	- 0.000000026 * pow(JC2000,3); //greenwich sidereal time
-	gst *= PI/180; //convert to radians
-	double sec = (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
-	double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
-	gst +=omega0 * sec;
+  double lst = gst + longitude;  // local sidereal time [rad]
 
-	double lst = gst + longitude; //local sidereal time [rad]
+  magFieldModelInertial[0] = magFieldModel[0] * cos(theta) +
+                             magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst);
+  magFieldModelInertial[1] = magFieldModel[0] * cos(theta) +
+                             magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst);
+  magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
 
-
-
-    magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*cos(lst) - magFieldModel[1] * sin(lst);
-    magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*sin(lst) + magFieldModel[1] * cos(lst);
-    magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
-
-    double normVecMagFieldInert[3] = {0,0,0};
-	VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
+  double normVecMagFieldInert[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
 }
 
-void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement){
-
-	double JD2000Igrf = (2458850.0-2451545); //Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
-	double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
-	double days = ceil(JD2000-JD2000Igrf);
-	for(int i = 0;i <= igrfOrder; i++){
-		for(int j = 0;j <= (igrfOrder-1); j++){
-			updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days/365);
-			updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days/365);
-		}
-	}
+void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
+  double JD2000Igrf = (2458850.0 - 2451545);  // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
+  double days = ceil(JD2000 - JD2000Igrf);
+  for (int i = 0; i <= igrfOrder; i++) {
+    for (int j = 0; j <= (igrfOrder - 1); j++) {
+      updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365);
+      updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365);
+    }
+  }
 }
diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h
index d89865ca..aa7e8b73 100644
--- a/mission/controller/acs/Igrf13Model.h
+++ b/mission/controller/acs/Igrf13Model.h
@@ -16,103 +16,107 @@
 #ifndef IGRF13MODEL_H_
 #define IGRF13MODEL_H_
 
-#include <cmath>
+#include <fsfw/parameters/HasParametersIF.h>
 #include <stdint.h>
 #include <string.h>
 #include <time.h>
-#include <fsfw/parameters/HasParametersIF.h>
 
+#include <cmath>
 
 // Output should be transformed to [T] instead of [nT]
 // Updating Coefficients has to be implemented yet. Question, updating everyday ?
-class Igrf13Model/*:public HasParametersIF*/{
+class Igrf13Model /*:public HasParametersIF*/ {
+ public:
+  Igrf13Model();
+  virtual ~Igrf13Model();
 
-public:
-	Igrf13Model();
-	virtual ~Igrf13Model();
+  // Main Function
+  void magFieldComp(const double longitude, const double gcLatitude, const double altitude,
+                    timeval timeOfMagMeasurement, double* magFieldModelInertial);
+  // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is
+  // oriented from the satellite to earth COM Difference up to 25 km, which is 5 % of the total
+  // flight altitude
+  /* Inputs:
+   * - longitude: geocentric longitude [rad]
+   * - latitude: geocentric latitude [rad]
+   * - altitude:  [m]
+   * - timeOfMagMeasurement: time of actual measurement [s]
+   *
+   * Outputs:
+   * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
 
-	// Main Function
-	void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial);
-	// Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM
-	// Difference up to 25 km, which is 5 % of the total flight altitude
-	/* Inputs:
-	 * - longitude: geocentric longitude [rad]
-	 * - latitude: geocentric latitude [rad]
-	 * - altitude:  [m]
-	 * - timeOfMagMeasurement: time of actual measurement [s]
-	 *
-	 * Outputs:
-	 * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
+  // Coefficient wary over year, could be updated sometimes.
+  void updateCoeffGH(timeval timeOfMagMeasurement);  // Secular variation (SV)
+  double magFieldModel[3];
 
+ private:
+  const double coeffG[14][13] = {
+      {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
+      {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
+      {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
+      {0.0, 0.0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
+      {0.0, 0.0, 0.0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3},
+      {0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8},
+      {0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -11.9, -2.4, -0.6, -0.5, 0.4},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.8, 0.2, 0.1, 0.1},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, -1.1, 0.5},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}};  // [m][n] in nT
 
-	// Coefficient wary over year, could be updated sometimes.
-	void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV)
-	double magFieldModel[3];
+  const double coeffH[14][13] = {
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
+      {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
+      {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
+      {0.0, 0.0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4},
+      {0.0, 0.0, 0.0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4},
+      {0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6, -0.1, -3.0, 0.2, 0.5},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.8, -2.0, -0.9, 0.5},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.6, 0.0, -0.4},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}};  // [m][n] in nT
 
-private:
-	const double coeffG[14][13] = {{-29404.8,-2499.6, 1363.2, 903.0,-234.3,  66.0, 80.6, 23.7,  5.0,-1.9, 3.0,-2.0, 0.1},
-	                               { -1450.9, 2982.0,-2381.2, 809.5, 363.2,  65.5,-76.7,  9.7,  8.4,-6.2,-1.4,-0.1,-0.9},
-	                               {     0.0, 1677.0, 1236.2,  86.3, 187.8,  72.9, -8.2,-17.6,  2.9,-0.1,-2.5, 0.5, 0.5},
-	                               {     0.0,    0.0,  525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
-	                               {     0.0,    0.0,    0.0,  48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3},
-	                               {     0.0,    0.0,    0.0,   0.0,  13.5,  13.5,  6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0, -64.7, -7.2, 13.7,  1.1,-0.9,-0.7, 0.3, 0.0},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  9.8,-16.5,  8.8, 1.9,-0.1, 0.5, 0.8},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,-11.9,-2.4,-0.6,-0.5, 0.4},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,-3.8, 0.2, 0.1, 0.1},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0, 0.0, 3.1,-1.1, 0.5},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0, 0.0, 0.0,-0.3,-0.5},
-	                               {     0.0,    0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0, 0.0, 0.0, 0.0,-0.4}}; // [m][n] in nT
+  const double svG[14][13] = {
+      {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, -12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, -5.1, 1.3, -1.4, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.9, 0.0, -0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.9, -0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};  // [m][n] in nT
 
-	const double coeffH[14][13] = {{   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0  },
-		                           {4652.5,-2991.6, -82.1, 281.9,  47.7,-19.1,-51.5,  8.4,-23.4, 3.4, 0.0,-1.2,-0.9},
-		                           {   0.0, -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6},
-		                           {   0.0,    0.0,-543.4, 199.7,-121.2, 52.8,  2.2, 12.8,  9.8, 3.6,-0.6, 1.4, 1.4},
-		                           {   0.0,    0.0,   0.0,-349.7,  32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4},
-		                           {   0.0,    0.0,   0.0,   0.0,  98.9,  8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0, 68.1,-27.2,  3.6,  7.8,-0.1,-0.2, 0.8,-0.1},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0, -1.8, -6.9,  0.4,-4.3,-1.7,-0.2, 0.3},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  2.8, -1.4,-3.4,-1.6, 0.6,-0.1},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  9.6,-0.1,-3.0, 0.2, 0.5},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,-8.8,-2.0,-0.9, 0.5},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0, 0.0,-2.6, 0.0,-0.4},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0, 0.0, 0.0, 0.5,-0.4},
-		                           {   0.0,    0.0,   0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0, 0.0, 0.0, 0.0,-0.6}}; // [m][n] in nT
+  const double svH[14][13] = {
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, -5.0, 3.0, 0.8, -0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.3, 0.0, -1.1, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};  // [m][n] in nT
 
-	const double svG[14][13] = {{5.7,-11.0,  2.2,-1.2,-0.3,-0.5,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0 ,0.0},
-			                    {7.4, -7.0, -5.9,-1.6, 0.5,-0.3,-0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0, -2.1,  3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.9, 0.0,-0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {0.0,  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
-
-	const double svH[14][13] = {{  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {-25.9,-30.2, 6.0,-0.1, 0.0, 0.0, 0.6,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
-			                    {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
-
-	double updatedG[14][13];
-	double updatedH[14][13];
-	static const int igrfOrder = 13; // degree of truncation
+  double updatedG[14][13];
+  double updatedH[14][13];
+  static const int igrfOrder = 13;  // degree of truncation
 };
 
 #endif /* IGRF13MODEL_H_ */
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 8cfa0ad3..cc136f61 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -7,6 +7,7 @@
 
 #include "MultiplicativeKalmanFilter.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
@@ -34,11 +35,11 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
 void MultiplicativeKalmanFilter::reset() {}
 
 void MultiplicativeKalmanFilter::init(
-    const double *magneticField_, const bool *validMagField_, const double *sunDir_,
-    const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
-    const bool *validMagModel) {  // valids for "model measurements"?
+    const double *magneticField_, const bool validMagField_, const double *sunDir_,
+    const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
+    const bool validMagModel) {  // valids for "model measurements"?
   // check for valid mag/sun
-  if (*validMagField_ && *validSS && *validSSModel && *validMagModel) {
+  if (validMagField_ && validSS && validSSModel && validMagModel) {
     validInit = true;
     // AcsParameters mekfEstParams;
     // loadAcsParameters(&mekfEstParams);
@@ -208,48 +209,78 @@ void MultiplicativeKalmanFilter::init(
 
 // --------------- MEKF DISCRETE TIME STEP -------------------------------
 ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
-    const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
-    const bool *validGYRs_, const double *magneticField_, const bool *validMagField_,
-    const double *sunDir_, const bool *validSS, const double *sunDirJ, const bool *validSSModel,
-    const double *magFieldJ, const bool *validMagModel, double *outputQuat, double *outputSatRate) {
+    const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
+    const bool validGYRs_, const double *magneticField_, const bool validMagField_,
+    const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
+    const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
   // Check for GYR Measurements
   // AcsParameters mekfEstParams;
   // loadAcsParameters(&mekfEstParams);
   int MDF = 0;  // Matrix Dimension Factor
-  if (!(*validGYRs_)) {
+  if (!validGYRs_) {
+    {
+      PoolReadGuard pg(mekfData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
+        double zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+        mekfData->setValidity(false, true);
+      }
+    }
     validMekf = false;
     return KALMAN_NO_GYR_MEAS;
   }
   // Check for Model Calculations
-  else if (!(*validSSModel) || !(*validMagModel)) {
+  else if (!validSSModel || !validMagModel) {
+    {
+      PoolReadGuard pg(mekfData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
+        double zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+        mekfData->setValidity(false, true);
+      }
+    }
     validMekf = false;
     return KALMAN_NO_MODEL;
   }
   // Check Measurements available from SS, MAG, STR
-  if (*validSS && *validMagField_ && *validSTR_) {
+  if (validSS && validMagField_ && validSTR_) {
     sensorsAvail = 1;
     MDF = 9;
-  } else if (*validSS && *validMagField_ && !(*validSTR_)) {
+  } else if (validSS && validMagField_ && !validSTR_) {
     sensorsAvail = 2;
     MDF = 6;
-  } else if (*validSS && !(*validMagField_) && *validSTR_) {
+  } else if (validSS && !validMagField_ && validSTR_) {
     sensorsAvail = 3;
     MDF = 6;
-  } else if (!(*validSS) && *validMagField_ && *validSTR_) {
+  } else if (!validSS && validMagField_ && validSTR_) {
     sensorsAvail = 4;
     MDF = 6;
-  } else if (*validSS && !(*validMagField_) && !(*validSTR_)) {
+  } else if (validSS && !validMagField_ && !(validSTR_)) {
     sensorsAvail = 5;
     MDF = 3;
-  } else if (!(*validSS) && *validMagField_ && !(*validSTR_)) {
+  } else if (!validSS && validMagField_ && !validSTR_) {
     sensorsAvail = 6;
     MDF = 3;
-  } else if (!(*validSS) && !(*validMagField_) && *validSTR_) {
+  } else if (!validSS && !validMagField_ && validSTR_) {
     sensorsAvail = 7;
     MDF = 3;
   } else {
     sensorsAvail = 8;  // no measurements
     validMekf = false;
+    {
+      PoolReadGuard pg(mekfData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
+        double zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+        mekfData->setValidity(false, true);
+      }
+    }
     return returnvalue::FAILED;
   }
 
@@ -289,7 +320,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
 
   double measVecQuat[3] = {0, 0, 0};
-  if (*validSTR_) {
+  if (validSTR_) {
     double quatError[4] = {0, 0, 0, 0};
     double invPropagatedQuat[4] = {0, 0, 0, 0};
     QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat);
@@ -913,11 +944,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4);
   VectorOperations<double>::normalize(quatBJ, quatBJ, 4);
 
-  outputQuat[0] = quatBJ[0];
-  outputQuat[1] = quatBJ[1];
-  outputQuat[2] = quatBJ[2];
-  outputQuat[3] = quatBJ[3];
-
   double updatedGyroBias[3] = {0, 0, 0};
   VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
   // Bias GYR State
@@ -940,11 +966,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
                               {rotRateEst[2], 0, -rotRateEst[0]},
                               {-rotRateEst[1], rotRateEst[0], 0}};
 
-  // Corrected Sat Rate via Bias
-  outputSatRate[0] = rotRateEst[0];
-  outputSatRate[1] = rotRateEst[1];
-  outputSatRate[2] = rotRateEst[2];
-
   // Discrete Process Noise Covariance Q
   double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                       {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
@@ -1144,5 +1165,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   // Check for new data in measurement -> SensorProcessing ?
 
+  {
+    PoolReadGuard pg(mekfData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
+      std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
+      mekfData->setValidity(true, true);
+    }
+  }
+
   return returnvalue::OK;
 }
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index f1d2d7a0..5eb47418 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -18,10 +18,9 @@
 #include <stdint.h>  //uint8_t
 #include <time.h>    /*purpose, timeval ?*/
 
-#include "config/classIds.h"
-//#include <_timeval.h>
-
+#include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
+#include "config/classIds.h"
 
 class MultiplicativeKalmanFilter {
  public:
@@ -40,9 +39,9 @@ class MultiplicativeKalmanFilter {
    * 			sunDirJ 		sun direction vector in the ECI frame
    * 			magFieldJ 		magnetic field vector in the ECI frame
    */
-  void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_,
-            const bool *validSS, const double *sunDirJ, const bool *validSSModel,
-            const double *magFieldJ, const bool *validMagModel);
+  void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
+            const bool validSS, const double *sunDirJ, const bool validSSModel,
+            const double *magFieldJ, const bool validMagModel);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
    * for the current step after the initalization
@@ -58,11 +57,11 @@ class MultiplicativeKalmanFilter {
    * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
    * 							RETURN_OK else
    */
-  ReturnValue_t mekfEst(const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
-                        const bool *validGYRs_, const double *magneticField_,
-                        const bool *validMagField_, const double *sunDir_, const bool *validSS,
-                        const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
-                        const bool *validMagModel, double *outputQuat, double *outputSatRate);
+  ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
+                        const bool validGYRs_, const double *magneticField_,
+                        const bool validMagField_, const double *sunDir_, const bool validSS,
+                        const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
+                        const bool validMagModel, acsctrl::MekfData *mekfData);
 
   // Declaration of Events (like init) and memberships
   // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp
index 2c1596e5..921ae604 100644
--- a/mission/controller/acs/Navigation.cpp
+++ b/mission/controller/acs/Navigation.cpp
@@ -21,28 +21,34 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
 
 Navigation::~Navigation() {}
 
-void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+void Navigation::useMekf(ACS::SensorValues *sensorValues,
+                         acsctrl::GyrDataProcessed *gyrDataProcessed,
+                         acsctrl::MgmDataProcessed *mgmDataProcessed,
+                         acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
                          ReturnValue_t *mekfValid) {
   double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatJBValid =
-      (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() &&
-       sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid());
+  bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
+                     sensorValues->strSet.caliQy.isValid() &&
+                     sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
 
   if (kalmanInit) {
     *mekfValid = multiplicativeKalmanFilter.mekfEst(
-        quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid,
-        outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst,
-        &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
-        outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
-        outputValues->satRateMekf);  // VALIDS FOR QUAT AND RATE ??
+        quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
+        gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
+        mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
+        susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
+        susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
+        mgmDataProcessed->magIgrfModel.isValid(),
+        mekfData);  // VALIDS FOR QUAT AND RATE ??
   } else {
-    multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
-                                    outputValues->sunDirEst, &outputValues->sunDirEstValid,
-                                    outputValues->sunDirModel, &outputValues->sunDirModelValid,
-                                    outputValues->magFieldModel, &outputValues->magFieldModelValid);
+    multiplicativeKalmanFilter.init(
+        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
+        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
+        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
+        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
     kalmanInit = true;
-    *mekfValid = 0;
+    *mekfValid = returnvalue::OK;
 
     //		Maybe we need feedback from kalmanfilter to identify if there was a problem with the
     // init of kalman filter 		where does this class know from that kalman filter was not
diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h
index 6691b8aa..2474cb67 100644
--- a/mission/controller/acs/Navigation.h
+++ b/mission/controller/acs/Navigation.h
@@ -8,28 +8,28 @@
 #ifndef NAVIGATION_H_
 #define NAVIGATION_H_
 
-
+#include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
-#include "SensorProcessing.h"
 #include "MultiplicativeKalmanFilter.h"
+#include "SensorProcessing.h"
 #include "SensorValues.h"
-#include "OutputValues.h"
 
-class Navigation{
-public:
-	Navigation(AcsParameters *acsParameters_); //Input mode ?
-	virtual ~Navigation();
+class Navigation {
+ public:
+  Navigation(AcsParameters *acsParameters_);  // Input mode ?
+  virtual ~Navigation();
 
-	void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid);
-	void processSensorData();
+  void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed,
+               acsctrl::MgmDataProcessed *mgmDataProcessed,
+               acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
+               ReturnValue_t *mekfValid);
+  void processSensorData();
 
-protected:
-
-private:
-	MultiplicativeKalmanFilter multiplicativeKalmanFilter;
-	AcsParameters acsParameters;
-	bool kalmanInit = false;
+ protected:
+ private:
+  MultiplicativeKalmanFilter multiplicativeKalmanFilter;
+  AcsParameters acsParameters;
+  bool kalmanInit = false;
 };
 
 #endif /* ACS_NAVIGATION_H_ */
-
diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp
deleted file mode 100644
index 42730fb3..00000000
--- a/mission/controller/acs/OutputValues.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "OutputValues.h"
-
-namespace ACS {
-
-OutputValues::OutputValues() {}
-
-OutputValues::~OutputValues() {}
-
-}  // namespace ACS
diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h
deleted file mode 100644
index bb1ed007..00000000
--- a/mission/controller/acs/OutputValues.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
-
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-
-#ifndef OUTPUTVALUES_H_
-#define OUTPUTVALUES_H_
-
-namespace ACS {
-
-class OutputValues {
- public:
-  OutputValues();
-  virtual ~OutputValues();
-
-  double magFieldEst[3];  // sensor fusion (G)  // output value
-  bool magFieldEstValid;
-  double magFieldModel[3];  // igrf (IJK)  //
-  bool magFieldModelValid;
-  double magneticFieldVectorDerivative[3];
-  bool magneticFieldVectorDerivativeValid;
-
-  bool mgmUpdated;  // ToDo: relic from FLP. most likely not used
-
-  double sunDirEst[3];  // sensor fusion (G)
-  bool sunDirEstValid;
-  double sunDirModel[3];  // sun model (IJK)
-  bool sunDirModelValid;
-
-  double quatMekfBJ[4];  // mekf
-  bool quatMekfBJValid;
-
-  double satRateEst[3];
-  bool satRateEstValid;
-  double satRateMekf[3];  // after mekf with correction of bias
-  bool satRateMekfValid;
-  double sunVectorDerivative[3];
-  bool sunVectorDerivativeValid;
-
-  double gcLatitude;   // geocentric latitude, radian
-  double gdLongitude;  // Radian longitude
-};
-}  // namespace ACS
-
-#endif /*OUTPUTVALUES_H_*/
diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h
index 11d2ebeb..61bd7a96 100644
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@@ -11,7 +11,6 @@
 
 #include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
-#include "OutputValues.h"
 #include "SensorValues.h"
 #include "SusConverter.h"
 #include "config/classIds.h"
diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp
index 152f866c..18f0d16f 100644
--- a/mission/controller/acs/SensorValues.cpp
+++ b/mission/controller/acs/SensorValues.cpp
@@ -90,4 +90,3 @@ ReturnValue_t SensorValues::update() {
 }
 
 }  // namespace ACS
-
diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
index 67903c7c..7421226d 100644
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -32,34 +32,27 @@ void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
 
 }
 
-
-ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
-		const double *magField, const bool *magFieldValid,
-		double *magMom) {
-
-	if (!magRateValid || !magFieldValid) {
-		return DETUMBLE_NO_SENSORDATA;
-	}
-	double gain = detumbleCtrlParameters->gainD;
-	double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
-	VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
-	return returnvalue::OK;
-
+ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
+                                const double *magField, const bool magFieldValid, double *magMom) {
+  if (!magRateValid || !magFieldValid) {
+    return DETUMBLE_NO_SENSORDATA;
+  }
+  double gain = detumbleCtrlParameters->gainD;
+  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
+  VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
+  return returnvalue::OK;
 }
 
-ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) {
+ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
+                                    double *magMom) {
+  if (!magRateValid) {
+    return DETUMBLE_NO_SENSORDATA;
+  }
 
-	if (!magRateValid) {
-		return DETUMBLE_NO_SENSORDATA;
-	}
-
-	double dipolMax = magnetorquesParameter->DipolMax;
-	for (int i = 0; i<3; i++) {
-
-		magMom[i] = - dipolMax * sign(magRate[i]);
-
-	}
-
-	return returnvalue::OK;
+  double dipolMax = magnetorquesParameter->DipolMax;
+  for (int i = 0; i < 3; i++) {
+    magMom[i] = -dipolMax * sign(magRate[i]);
+  }
 
+  return returnvalue::OK;
 }
diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h
index b85aaf86..375f67aa 100644
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@@ -9,7 +9,6 @@
 #define ACS_CONTROL_DETUMBLE_H_
 
 #include "../SensorValues.h"
-#include "../OutputValues.h"
 #include "../AcsParameters.h"
 #include "../config/classIds.h"
 #include <string.h>
@@ -32,13 +31,12 @@ public:
 	 */
 	void loadAcsParameters(AcsParameters *acsParameters_);
 
-	ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid,
-			const double *magField, const bool *magFieldValid,
-			double *magMom);
+        ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
+                              const double *magField, const bool magFieldValid, double *magMom);
 
-	ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
+        ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
 
-private:
+       private:
 	AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
 	AcsParameters::MagnetorquesParameter* magnetorquesParameter;
 };
diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp
index 7160fb47..e06b576f 100644
--- a/mission/controller/acs/control/PtgCtrl.cpp
+++ b/mission/controller/acs/control/PtgCtrl.cpp
@@ -112,7 +112,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
 
 }
 
-void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
+void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
                               int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
                               int32_t *speedRw3, double *mgtDpDes) {
   if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h
index be67187d..64b4110c 100644
--- a/mission/controller/acs/control/PtgCtrl.h
+++ b/mission/controller/acs/control/PtgCtrl.h
@@ -19,7 +19,6 @@
 #include <time.h>
 
 #include "../AcsParameters.h"
-#include "../OutputValues.h"
 #include "../SensorValues.h"
 #include "../config/classIds.h"
 
@@ -45,7 +44,7 @@ class PtgCtrl {
   void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
                         const double *rwPseudoInv, double *torqueRws);
 
-  void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
+  void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
                        int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
                        double *mgtDpDes);
 
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index c892fc05..54ae27ee 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -28,12 +28,12 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
 }
 
-ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
-                                 double *magFieldModel, bool *magFieldModelValid,
-                                 double *sunDirModel, bool *sunDirModelValid, double *satRateMekf,
-                                 bool *rateMekfValid, double *sunDirRef, double *satRatRef,
-                                 double *outputMagMomB, bool *outputValid) {
-  if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) {
+ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
+                                 double *magFieldModel, bool magFieldModelValid,
+                                 double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
+                                 bool rateMekfValid, double *sunDirRef, double *satRatRef,
+                                 double *outputAngle, double *outputMagMomB, bool *outputValid) {
+  if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
     *outputValid = false;
     return SAFECTRL_MEKF_INPUT_INVALID;
   }
@@ -80,31 +80,34 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
   VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
   double normMag = VectorOperations<double>::norm(magFieldB, 3);
   VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
+
+  *outputAngle = alpha;
   *outputValid = true;
 
   return returnvalue::OK;
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
-void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
-                          bool *sunRateBValid, double *magFieldB, bool *magFieldBValid,
-                          double *magRateB, bool *magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputMagMomB, bool *outputValid) {
+void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
+                          double *magRateB, bool magRateBValid, double *sunDirRef,
+                          double *satRateRef, double *outputAngle, double *outputMagMomB,
+                          bool *outputValid) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
     *outputValid = false;
     return;
   }
 
-  //	normalize sunDir and magDir
+  // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(magFieldB, magDirB, 3);
   VectorOperations<double>::normalize(susDirB, susDirB, 3);
 
-  //	Cosinus angle between sunDir and magDir
+  // Cosinus angle between sunDir and magDir
   double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
 
-  //	Rate parallel to sun direction and magnetic field direction
+  // Rate parallel to sun direction and magnetic field direction
   double rateParaSun = 0, rateParaMag = 0;
   double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
   dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
@@ -113,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
   rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
   rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
 
-  //	Full rate or estimate
+  // Full rate or estimate
   double estSatRate[3] = {0, 0, 0};
   double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
@@ -123,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
   VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
   VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
 
-  /*    Only valid if angle between sun direction and magnetic field direction
-        is sufficiently large */
+  /* Only valid if angle between sun direction and magnetic field direction
+     is sufficiently large */
 
   double sinAngle = 0;
   sinAngle = sin(acos(cos(cosAngleSunMag)));
@@ -169,9 +172,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
   double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
   VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
 
-  outputMagMomB[0] = magMomB[0];
-  outputMagMomB[1] = magMomB[1];
-  outputMagMomB[2] = magMomB[2];
-
+  std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
+  *outputAngle = angleAlignErr;
   *outputValid = true;
 }
diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h
index 70426c9f..72e45f08 100644
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/mission/controller/acs/control/SafeCtrl.h
@@ -13,7 +13,6 @@
 #include <time.h>
 
 #include "../AcsParameters.h"
-#include "../OutputValues.h"
 #include "../SensorValues.h"
 #include "../config/classIds.h"
 
@@ -27,16 +26,16 @@ class SafeCtrl {
 
   void loadAcsParameters(AcsParameters *acsParameters_);
 
-  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel,
-                         bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid,
-                         double *satRateMekf, bool *rateMekfValid, double *sunDirRef,
+  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
+                         bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
+                         double *satRateMekf, bool rateMekfValid, double *sunDirRef,
                          double *satRatRef,  // From Guidance (!)
-                         double *outputMagMomB, bool *outputValid);
+                         double *outputAngle, double *outputMagMomB, bool *outputValid);
 
-  void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
-                  bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB,
-                  bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB,
-                  bool *outputValid);
+  void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+                  bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
+                  bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
+                  double *outputMagMomB, bool *outputValid);
 
   void idleSunPointing();  // with reaction wheels
 
diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
index bb0a40ae..1b07218f 100644
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
@@ -17,6 +17,7 @@ enum SetIds : uint32_t {
   GYR_PROCESSED_DATA,
   GPS_PROCESSED_DATA,
   MEKF_DATA,
+  CTRL_VAL_DATA,
   ACTUATOR_CMD_DATA
 };
 
@@ -88,6 +89,10 @@ enum PoolIds : lp_id_t {
   // MEKF
   SAT_ROT_RATE_MEKF,
   QUAT_MEKF,
+  // Ctrl Values
+  TGT_QUAT,
+  ERROR_QUAT,
+  ERROR_ANG,
   // Actuator Cmd
   RW_TARGET_TORQUE,
   RW_TARGET_SPEED,
@@ -102,6 +107,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2;
 static constexpr uint8_t MEKF_SET_ENTRIES = 2;
+static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
 
 /**
@@ -133,9 +139,9 @@ class MgmDataProcessed : public StaticLocalDataSet<MGM_SET_PROCESSED_ENTRIES> {
   lp_vec_t<float, 3> mgm2vec = lp_vec_t<float, 3>(sid.objectId, MGM_2_VEC, this);
   lp_vec_t<float, 3> mgm3vec = lp_vec_t<float, 3>(sid.objectId, MGM_3_VEC, this);
   lp_vec_t<float, 3> mgm4vec = lp_vec_t<float, 3>(sid.objectId, MGM_4_VEC, this);
-  lp_vec_t<float, 3> mgmVecTot = lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT, this);
-  lp_vec_t<float, 3> mgmVecTotDerivative =
-      lp_vec_t<float, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
+  lp_vec_t<double, 3> mgmVecTot = lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT, this);
+  lp_vec_t<double, 3> mgmVecTotDerivative =
+      lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
   lp_vec_t<double, 3> magIgrfModel = lp_vec_t<double, 3>(sid.objectId, MAG_IGRF_MODEL, this);
 
  private:
@@ -179,9 +185,9 @@ class SusDataProcessed : public StaticLocalDataSet<SUS_SET_PROCESSED_ENTRIES> {
   lp_vec_t<float, 3> sus9vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
   lp_vec_t<float, 3> sus10vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
   lp_vec_t<float, 3> sus11vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
-  lp_vec_t<float, 3> susVecTot = lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT, this);
-  lp_vec_t<float, 3> susVecTotDerivative =
-      lp_vec_t<float, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
+  lp_vec_t<double, 3> susVecTot = lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT, this);
+  lp_vec_t<double, 3> susVecTotDerivative =
+      lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
   lp_vec_t<double, 3> sunIjkModel = lp_vec_t<double, 3>(sid.objectId, SUN_IJK_MODEL, this);
 
  private:
@@ -232,6 +238,17 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
  private:
 };
 
+class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
+ public:
+  CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
+
+  lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
+  lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
+  lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
+
+ private:
+};
+
 class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
  public:
   ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}