diff --git a/CHANGELOG.md b/CHANGELOG.md
index 19fc3d9b..20ad4f6e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -18,10 +18,19 @@ will consitute of a breaking change warranting a new major release:
 
 Requires firmware update for new FPGA design where reset line is routed into the software.
 
+## Added
+
+- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset
+  the `MEKF` on its own once. This way, there will not be an event spam and operators will have
+  to look into the reason of wrong outputs. To restore the reset ability, an action command has
+  been added.
+- Contingency handling for non-working I2C bus bug. Reboot the system if the I2C is not working.
+
 ## Fixed
 
 - Fixed transition for dual power lane assemblies: When going from dual side submode to single side
   submode, perform logical commanding first, similarly to when going to OFF mode.
+- GPS time is only set to valid if at least one satellite is in view.
 
 ## Changed
 
@@ -36,6 +45,7 @@ Requires firmware update for new FPGA design where reset line is routed into the
   the unique parameter ID 0. The update of this mode requires a PTME reset. Therefore, it will only
   be performed when the transmitter is off to avoid weird bugs.
 - Connect and handle reset line for the PTME core in the software now.
+- Safe mode controller failure event now only triggers once per minute.
 
 # [v1.38.0] 2023-03-17
 
@@ -78,8 +88,7 @@ eive-tmtc: v2.19.1
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/462
 - Add `PcduHandlerDummy` component.
 - Added parameter for timeout until `MEKF_INVALID_MODE_VIOLATION` event is triggered.
-- EIVE system: Add boot mode which is also the initial mode. The fallback mode of the boot mode
-  will be the SAFE mode. The boot mode can also be used to switch as many devices as possible OFF.
+
 ## Fixed
 
 - Pointing control of the `AcsController` was still expecting submodes instead of modes.
@@ -105,9 +114,6 @@ eive-tmtc: v2.19.1
   commanding is done autonomously by the COM subsystem internally or by the operator. This prevents
   the transmitter from going off during fallbacks to the SAFE mode, which might not always be
   desired.
-
-## Changed
-
 - Initialize switch states to a special `SWITCH_STATE_UNKNOWN` (2) variable. Return
   `PowerSwitchIF::SWITCH_UNKNOWN` in switch state getter if this is the state.
 - Wait 1 second before commanding SAFE mode. This ensures or at least increases the chance that
diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
index 32b61bca..aabf8e69 100644
--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-15 10:10:04
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateEvents.h"
 
diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
index 81710911..ad27211f 100644
--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 169 translations.
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateObjects.h"
 
diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 085d27de..61e7ee67 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -31,14 +31,15 @@
 xsc::Chip CoreController::CURRENT_CHIP = xsc::Chip::NO_CHIP;
 xsc::Copy CoreController::CURRENT_COPY = xsc::Copy::NO_COPY;
 
-CoreController::CoreController(object_id_t objectId)
+CoreController::CoreController(object_id_t objectId, const std::atomic_uint16_t& i2cErrors)
     : ExtendedControllerBase(objectId, 5),
       cmdExecutor(4096),
       cmdReplyBuf(4096, true),
       cmdRepliesSizes(128),
       opDivider5(5),
       opDivider10(10),
-      hkSet(this) {
+      hkSet(this),
+      i2cErrors(i2cErrors) {
   cmdExecutor.setRingBuffer(&cmdReplyBuf, &cmdRepliesSizes);
   try {
     sdcMan = SdCardManager::instance();
@@ -107,6 +108,12 @@ void CoreController::performControlOperation() {
   sdStateMachine();
   performMountedSdCardOperations();
   readHkData();
+  if(i2cErrors >= 5) {
+    bool protOpPerformed = false;
+    triggerEvent(I2C_UNAVAILABLE_REBOOT);
+    gracefulShutdownTasks(CURRENT_CHIP, CURRENT_COPY, protOpPerformed);
+    std::system("xsc_boot_copy -r");
+  }
   if (shellCmdIsExecuting) {
     bool replyReceived = false;
     // TODO: We could read the data in the ring buffer and send it as an action data reply.
diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h
index fa07ec59..cd504e3c 100644
--- a/bsp_q7s/core/CoreController.h
+++ b/bsp_q7s/core/CoreController.h
@@ -15,6 +15,7 @@
 #include "fsfw/controller/ExtendedControllerBase.h"
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
 #include "mission/trace.h"
+#include <atomic>
 
 class Timer;
 class SdCardManager;
@@ -131,8 +132,9 @@ class CoreController : public ExtendedControllerBase {
   //! P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1.
   //! P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.
   static constexpr Event INDIVIDUAL_BOOT_COUNTS = event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO);
+  static constexpr Event I2C_UNAVAILABLE_REBOOT = event::makeEvent(SUBSYSTEM_ID, 10, severity::MEDIUM);
 
-  CoreController(object_id_t objectId);
+  CoreController(object_id_t objectId, const std::atomic_uint16_t& i2cErrors);
   virtual ~CoreController();
 
   ReturnValue_t initialize() override;
@@ -262,6 +264,7 @@ class CoreController : public ExtendedControllerBase {
   PoolEntry<float> plVoltageEntry = PoolEntry<float>(0.0);
 
   core::HkSet hkSet;
+  const std::atomic_uint16_t& i2cErrors;
 
 #if OBSW_SD_CARD_MUST_BE_ON == 1
   bool remountAttemptFlag = true;
diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index 6a6ae95b..112b7995 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -123,6 +123,7 @@ using gpio::Levels;
 
 ResetArgs RESET_ARGS_GNSS;
 std::atomic_bool LINK_STATE = CcsdsIpCoreHandler::LINK_DOWN;
+std::atomic_uint16_t I2C_FATAL_ERRORS = 0;
 
 void Factory::setStaticFrameworkObjectIds() {
   PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR;
@@ -961,7 +962,7 @@ void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
   auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY);
   imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
 
-  new ImtqPollingTask(objects::IMTQ_POLLING);
+  new ImtqPollingTask(objects::IMTQ_POLLING, I2C_FATAL_ERRORS);
   I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE);
   auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie,
                                      pcdu::Switches::PDU1_CH3_MGT_5V);
diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h
index c0da31d4..66ece5cd 100644
--- a/bsp_q7s/core/ObjectFactory.h
+++ b/bsp_q7s/core/ObjectFactory.h
@@ -12,6 +12,7 @@
 #include <mission/tmtc/PusTmFunnel.h>
 
 #include <string>
+#include <atomic>
 
 class LinuxLibgpioIF;
 class SerialComIF;
@@ -22,6 +23,8 @@ class HealthTableIF;
 class AcsBoardAssembly;
 class GpioIF;
 
+extern std::atomic_uint16_t I2C_FATAL_ERRORS;
+
 namespace ObjectFactory {
 
 struct CcsdsComponentArgs {
diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp
index 5daab861..f3ce12e1 100644
--- a/bsp_q7s/fmObjectFactory.cpp
+++ b/bsp_q7s/fmObjectFactory.cpp
@@ -38,7 +38,7 @@ void ObjectFactory::produce(void* args) {
   q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF);
   gpioCallbacks::disableAllDecoder(gpioComIF);
 
-  new CoreController(objects::CORE_CONTROLLER);
+  new CoreController(objects::CORE_CONTROLLER, I2C_FATAL_ERRORS);
   createPcduComponents(gpioComIF, &pwrSwitcher);
   auto* stackHandler = new Stack5VHandler(*pwrSwitcher);
 
diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv
index 882bdd05..af3e0146 100644
--- a/generators/bsp_hosted_returnvalues.csv
+++ b/generators/bsp_hosted_returnvalues.csv
@@ -470,8 +470,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv
index 971d2e73..c509e7dc 100644
--- a/generators/bsp_q7s_returnvalues.csv
+++ b/generators/bsp_q7s_returnvalues.csv
@@ -581,8 +581,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp
index 32b61bca..aabf8e69 100644
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-15 10:10:04
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateEvents.h"
 
diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp
index ede7df22..127858d3 100644
--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 173 translations.
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateObjects.h"
 
diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp
index 18461c14..8b26da5c 100644
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@@ -302,7 +302,10 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   // TIME is set for every message, no need for a counter
   bool timeValid = false;
   if (TIME_SET == (TIME_SET & gps.set)) {
-    timeValid = true;
+    // To prevent totally incorrect times from being declared valid.
+    if(gpsSet.satInView.isValid() and gpsSet.satInView.value >= 1) {
+      timeValid = true;
+    }
     timeval time = {};
 #if LIBGPS_VERSION_MINOR <= 17
     gpsSet.unixSeconds.value = std::floor(gps.fix.time);
diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp
index fbd5f847..a0e59412 100644
--- a/linux/devices/ImtqPollingTask.cpp
+++ b/linux/devices/ImtqPollingTask.cpp
@@ -10,7 +10,9 @@
 
 #include "fsfw/FSFW.h"
 
-ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) {
+ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask,
+    std::atomic_uint16_t& i2cFatalErrors): SystemObject(imtqPollingTask),
+    i2cFatalErrors(i2cFatalErrors) {
   semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
   semaphore->acquire();
   ipcLock = MutexFactory::instance()->createMutex();
@@ -427,12 +429,20 @@ ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t repl
     if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
       sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
                    << strerror(errno) << std::endl;
+      if(errno == EBUSY) {
+        i2cFatalErrors++;
+      }
     }
 
     int written = write(fd, cmdBuf.data(), cmdLen);
     if (written < 0) {
       sif::error << "IMTQ: Failed to send with error code " << errno
                  << ". Error description: " << strerror(errno) << std::endl;
+      // This is a weird issue which sometimes occurs on debug builds. All I2C buses are busy
+      // for all writes,
+      if(errno == EBUSY) {
+        i2cFatalErrors++;
+      }
       return returnvalue::FAILED;
     } else if (static_cast<size_t>(written) != cmdLen) {
       sif::error << "IMTQ: Could not write all bytes" << std::endl;
diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h
index cb2d3882..efe6a01b 100644
--- a/linux/devices/ImtqPollingTask.h
+++ b/linux/devices/ImtqPollingTask.h
@@ -8,12 +8,13 @@
 #include "fsfw/objectmanager/SystemObject.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "mission/devices/devicedefinitions/imtqHelpers.h"
+#include <atomic>
 
 class ImtqPollingTask : public SystemObject,
                         public ExecutableObjectIF,
                         public DeviceCommunicationIF {
  public:
-  ImtqPollingTask(object_id_t imtqPollingTask);
+  ImtqPollingTask(object_id_t imtqPollingTask, std::atomic_uint16_t& i2cFatalErrors);
 
   ReturnValue_t performOperation(uint8_t operationCode) override;
   ReturnValue_t initialize() override;
@@ -28,6 +29,7 @@ class ImtqPollingTask : public SystemObject,
   ReturnValue_t comStatus = returnvalue::OK;
   MutexIF* ipcLock;
   MutexIF* bufLock;
+  std::atomic_uint16_t& i2cFatalErrors;
   I2cCookie* i2cCookie = nullptr;
   const char* i2cDev = nullptr;
   address_t i2cAddr = 0;
diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp
index 32b61bca..aabf8e69 100644
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-15 10:10:04
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateEvents.h"
 
diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp
index ede7df22..127858d3 100644
--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 173 translations.
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-21 12:45:57
  */
 #include "translateObjects.h"
 
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index e3a5ed5e..b275960b 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -55,6 +55,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
       navigation.resetMekf(&mekfData);
       return HasActionsIF::EXECUTION_FINISHED;
     }
+    case RESTORE_MEKF_NONFINITE_RECOVERY: {
+      mekfLost = false;
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -149,6 +153,10 @@ void AcsController::performSafe() {
       triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+      mekfLost = true;
+    }
   } else {
     mekfInvalidFlag = false;
   }
@@ -178,7 +186,7 @@ void AcsController::performSafe() {
       safeCtrlFailureFlag = true;
     }
     safeCtrlFailureCounter++;
-    if (safeCtrlFailureCounter > 50) {
+    if (safeCtrlFailureCounter > 150) {
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
     }
@@ -231,6 +239,9 @@ void AcsController::performDetumble() {
       triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+    }
   } else {
     mekfInvalidFlag = false;
   }
@@ -282,6 +293,9 @@ void AcsController::performPointingCtrl() {
       triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+    }
     if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
       // Trigger this so STR FDIR can set the device faulty.
       EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 3c309bd4..fd17d606 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -64,6 +64,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint16_t mekfInvalidCounter = 0;
   bool safeCtrlFailureFlag = false;
   uint8_t safeCtrlFailureCounter = 0;
+  uint8_t resetMekfCount = 0;
+  bool mekfLost = false;
 
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipolMtqs[3] = {0, 0, 0};
@@ -78,6 +80,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   /** Device command IDs */
   static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
   static const DeviceCommandId_t RESET_MEKF = 0x1;
+  static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
 
   static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
   static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 77a3ef00..88568573 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
+  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
+      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
+    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
+    return MEKF_NOT_FINITE;
+  }
+
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
   return MEKF_RUNNING;
 }
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index ceb98339..fe749890 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -74,8 +75,9 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
   static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
       returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
 
  private:
   /*Parameters*/
diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index f8537740..b344451a 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -404,6 +404,26 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
+
+  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
+    for (uint8_t i = 0; i < size; i++) {
+      if (not isfinite(inputVector[i])) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
+    for (uint8_t col = 0; col < cols; col++) {
+      for (uint8_t row = 0; row < rows; row++) {
+        if (not isfinite(inputMatrix[row * cols + cols])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */