diff --git a/src/modules/src/estimator_ukf.c b/src/modules/src/estimator_ukf.c
index e7fdbfff67..7ac20a30f0 100644
--- a/src/modules/src/estimator_ukf.c
+++ b/src/modules/src/estimator_ukf.c
@@ -204,7 +204,6 @@ static float baroOut;
 static float PxOut;
 static float PvxOut;
 static float PattxOut;
-static uint32_t tdoaCount;
 
 static bool useNavigationFilter = true;
 static bool resetNavigation = true;
@@ -352,7 +351,6 @@ static void errorUkfTask(void *parameters)
       accNed[1] = 0.0f;
       accNed[2] = 0.0f;
 
-      tdoaCount = 0;
       nanCounterFilter = 0;
 
       navigationInit();
@@ -826,59 +824,55 @@ static bool updateQueuedMeasurements(const uint32_t tick, Axis3f *gyroAverage)
       switch (m.type)
       {
         case MeasurementTypeTDOA:
-          if (tdoaCount >= 100)
+          //_________________________________________________________________________________
+          // UKF update - TDOA
+          //_________________________________________________________________________________
+          // compute mean tdoa observation
+          observation = 0.0f;
+          for (jj = 0; jj < (DIM_FILTER + 2); jj++)
           {
-            //_________________________________________________________________________________
-            // UKF update - TDOA
-            //_________________________________________________________________________________
-            // compute mean tdoa observation
-            observation = 0.0f;
-            for (jj = 0; jj < (DIM_FILTER + 2); jj++)
+            for (ii = 0; ii < DIM_FILTER; ii++)
             {
-              for (ii = 0; ii < DIM_FILTER; ii++)
-              {
-                tmpSigmaVec[ii] = sigmaPoints[ii][jj];
-              }
-              computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
-              observation = observation + weights[jj] * outTmp;
+              tmpSigmaVec[ii] = sigmaPoints[ii][jj];
             }
+            computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
+            observation = observation + weights[jj] * outTmp;
+          }
 
-            // initialize measurement and cross covariance
-            Pyy = 0.0f;
-            for (jj = 0; jj < DIM_FILTER; jj++)
+          // initialize measurement and cross covariance
+          Pyy = 0.0f;
+          for (jj = 0; jj < DIM_FILTER; jj++)
+          {
+            Pxy[jj] = 0.0f;
+          }
+
+          // loop over all sigma points
+          for (jj = 0; jj < (DIM_FILTER + 2); jj++)
+          {
+            for (ii = 0; ii < DIM_FILTER; ii++)
             {
-              Pxy[jj] = 0.0f;
+              tmpSigmaVec[ii] = sigmaPoints[ii][jj];
             }
+            computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
+            Pyy = Pyy + weights[jj] * (outTmp - observation) * (outTmp - observation);
 
-            // loop over all sigma points
-            for (jj = 0; jj < (DIM_FILTER + 2); jj++)
+            for (kk = 0; kk < DIM_FILTER; kk++)
             {
-              for (ii = 0; ii < DIM_FILTER; ii++)
-              {
-                tmpSigmaVec[ii] = sigmaPoints[ii][jj];
-              }
-              computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
-              Pyy = Pyy + weights[jj] * (outTmp - observation) * (outTmp - observation);
-
-              for (kk = 0; kk < DIM_FILTER; kk++)
-              {
-                Pxy[kk] = Pxy[kk] + weights[jj] * (tmpSigmaVec[kk] - xEst[kk]) * (outTmp - observation);
-              }
+              Pxy[kk] = Pxy[kk] + weights[jj] * (tmpSigmaVec[kk] - xEst[kk]) * (outTmp - observation);
             }
+          }
 
-            // Add TDOA Noise R
-            Pyy = Pyy + m.data.tdoa.stdDev * m.data.tdoa.stdDev;
-            innovation = m.data.tdoa.distanceDiff - observation;
+          // Add TDOA Noise R
+          Pyy = Pyy + m.data.tdoa.stdDev * m.data.tdoa.stdDev;
+          innovation = m.data.tdoa.distanceDiff - observation;
 
-            innoCheck = innovation * innovation / Pyy;
-            if (outlierFilterTdoaValidateIntegrator(&outlierFilterTdoaState, &m.data.tdoa, innovation, nowMs))
-            {
-              //	if(innoCheck<qualGateTdoa){ // TdoA outlier rejection
-              ukfUpdate(&Pxy[0], &Pyy, innovation);
-              //	}
-            }
+          innoCheck = innovation * innovation / Pyy;
+          if (outlierFilterTdoaValidateIntegrator(&outlierFilterTdoaState, &m.data.tdoa, innovation, nowMs))
+          {
+            //	if(innoCheck<qualGateTdoa){ // TdoA outlier rejection
+            ukfUpdate(&Pxy[0], &Pyy, innovation);
+            //	}
           }
-          tdoaCount++;
           break;
 
         case MeasurementTypeTOF:
diff --git a/src/modules/src/kalman_core/mm_tdoa.c b/src/modules/src/kalman_core/mm_tdoa.c
index a123f00adb..278973efea 100644
--- a/src/modules/src/kalman_core/mm_tdoa.c
+++ b/src/modules/src/kalman_core/mm_tdoa.c
@@ -30,73 +30,65 @@
 #include "outlierFilterTdoaSteps.h"
 #endif
 
-// TODO krri What is this used for? Do we still need it?
-TESTABLE_STATIC uint32_t tdoaCount = 0;
-
 void kalmanCoreUpdateWithTdoa(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa, const uint32_t nowMs, OutlierFilterTdoaState_t* outlierFilterState)
 {
-  if (tdoaCount >= 100)
-  {
-    /**
-     * Measurement equation:
-     * dR = dT + d1 - d0
-     */
-
-    float measurement = tdoa->distanceDiff;
-
-    // predict based on current state
-    float x = this->S[KC_STATE_X];
-    float y = this->S[KC_STATE_Y];
-    float z = this->S[KC_STATE_Z];
-
-    float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
-    float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
-
-    float dx1 = x - x1;
-    float dy1 = y - y1;
-    float dz1 = z - z1;
-
-    float dy0 = y - y0;
-    float dx0 = x - x0;
-    float dz0 = z - z0;
-
-    float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
-    float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
-
-    float predicted = d1 - d0;
-    float error = measurement - predicted;
-
-    float h[KC_STATE_DIM] = {0};
-    arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
-
-    if ((d0 != 0.0f) && (d1 != 0.0f)) {
-      h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
-      h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
-      h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
-
-    #if CONFIG_ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK
-      vector_t jacobian = {
-        .x = h[KC_STATE_X],
-        .y = h[KC_STATE_Y],
-        .z = h[KC_STATE_Z],
-      };
-
-      point_t estimatedPosition = {
-        .x = this->S[KC_STATE_X],
-        .y = this->S[KC_STATE_Y],
-        .z = this->S[KC_STATE_Z],
-      };
-
-      bool sampleIsGood = outlierFilterTdoaValidateSteps(tdoa, error, &jacobian, &estimatedPosition);
-      #else
-      bool sampleIsGood = outlierFilterTdoaValidateIntegrator(outlierFilterState, tdoa, error, nowMs);
-      #endif
-
-      if (sampleIsGood) {
-        kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
-      }
+  /**
+   * Measurement equation:
+   * dR = dT + d1 - d0
+   */
+
+  float measurement = tdoa->distanceDiff;
+
+  // predict based on current state
+  float x = this->S[KC_STATE_X];
+  float y = this->S[KC_STATE_Y];
+  float z = this->S[KC_STATE_Z];
+
+  float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
+  float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
+
+  float dx1 = x - x1;
+  float dy1 = y - y1;
+  float dz1 = z - z1;
+
+  float dy0 = y - y0;
+  float dx0 = x - x0;
+  float dz0 = z - z0;
+
+  float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
+  float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
+
+  float predicted = d1 - d0;
+  float error = measurement - predicted;
+
+  float h[KC_STATE_DIM] = {0};
+  arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+
+  if ((d0 != 0.0f) && (d1 != 0.0f)) {
+    h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
+    h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
+    h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
+
+  #if CONFIG_ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK
+    vector_t jacobian = {
+      .x = h[KC_STATE_X],
+      .y = h[KC_STATE_Y],
+      .z = h[KC_STATE_Z],
+    };
+
+    point_t estimatedPosition = {
+      .x = this->S[KC_STATE_X],
+      .y = this->S[KC_STATE_Y],
+      .z = this->S[KC_STATE_Z],
+    };
+
+    bool sampleIsGood = outlierFilterTdoaValidateSteps(tdoa, error, &jacobian, &estimatedPosition);
+    #else
+    bool sampleIsGood = outlierFilterTdoaValidateIntegrator(outlierFilterState, tdoa, error, nowMs);
+    #endif
+
+    if (sampleIsGood) {
+      kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
     }
   }
-
-  tdoaCount++;
 }
diff --git a/test/modules/src/kalman_core/test_mm_tdoa.c b/test/modules/src/kalman_core/test_mm_tdoa.c
index 32bcbde437..3b46a4f20c 100644
--- a/test/modules/src/kalman_core/test_mm_tdoa.c
+++ b/test/modules/src/kalman_core/test_mm_tdoa.c
@@ -12,16 +12,10 @@ static kalmanCoreData_t this;
 static float expectedHm[KC_STATE_DIM];
 static OutlierFilterTdoaState_t outlierFilterTdoaState;
 
-// Instrumented in code under test
-extern uint32_t tdoaCount;
-
 void setUp(void) {
   memset(&this, 0, sizeof(this));
   memset(&expectedHm, 0, sizeof(expectedHm));
 
-  // Make sure we pass the tdoaCount counter
-  tdoaCount = 100;
-
   initKalmanCoreScalarUpdateExpectationsSingleCall();
 
   outlierFilterTdoaReset_Ignore();