From 5c46bf350280e519ff5cbbce9d84e75b0ca0edfe Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sun, 22 Jan 2023 17:27:45 +0100
Subject: [PATCH 1/2] Added sub sampler

---
 src/modules/interface/axis3fSubSampler.h |  62 ++++++++++++++
 src/modules/src/Kbuild                   |   1 +
 src/modules/src/axis3fSubSampler.c       |  54 ++++++++++++
 test/modules/src/test_axis3fSubSampler.c | 100 +++++++++++++++++++++++
 4 files changed, 217 insertions(+)
 create mode 100644 src/modules/interface/axis3fSubSampler.h
 create mode 100644 src/modules/src/axis3fSubSampler.c
 create mode 100644 test/modules/src/test_axis3fSubSampler.c

diff --git a/src/modules/interface/axis3fSubSampler.h b/src/modules/interface/axis3fSubSampler.h
new file mode 100644
index 0000000000..837becd899
--- /dev/null
+++ b/src/modules/interface/axis3fSubSampler.h
@@ -0,0 +1,62 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "imu_types.h"
+
+typedef struct {
+  Axis3f sum;
+  uint32_t count;
+  float conversionFactor;
+
+  Axis3f subSample;
+} Axis3fSubSampler_t;
+
+/**
+ * @brief Initialize sub sampler
+ *
+ * @param this  Pointer to sub sampler
+ * @param conversionFactor  Conversion factor used for unit conversion.
+ */
+void axis3fSubSamplerInit(Axis3fSubSampler_t* this, const float conversionFactor);
+
+/**
+ * @brief Accumulate a sample
+ *
+ * @param this  Pointer to sub sampler
+ * @param sample  The sample to accumulate
+ */
+void axis3fSubSamplerAccumulate(Axis3fSubSampler_t* this, const Axis3f* sample);
+
+/**
+ * @brief Compute the sub sample, uses simple averaging of samples. The sub sample is multiplied with the conversion
+ * factor and the result is stored in the subSample member of the Axis3fSubSampler_t.
+ *
+ * @param this  Pointer to sub sampler
+ * @return Axis3f*  Pointer to the resulting sub sample
+ */
+Axis3f* axis3fSubSamplerFinalize(Axis3fSubSampler_t* this);
diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild
index ff16deb6bc..ee63c82779 100644
--- a/src/modules/src/Kbuild
+++ b/src/modules/src/Kbuild
@@ -27,6 +27,7 @@ obj-y += eventtrigger.o
 obj-y += extrx.o
 obj-y += health.o
 obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_supervisor.o
+obj-y += axis3fSubSampler.o
 obj-y += log.o
 obj-y += mem.o
 obj-y += msp.o
diff --git a/src/modules/src/axis3fSubSampler.c b/src/modules/src/axis3fSubSampler.c
new file mode 100644
index 0000000000..d28b8f83b9
--- /dev/null
+++ b/src/modules/src/axis3fSubSampler.c
@@ -0,0 +1,54 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <string.h>
+#include "axis3fSubSampler.h"
+
+void axis3fSubSamplerInit(Axis3fSubSampler_t* this, const float conversionFactor) {
+  memset(this, 0, sizeof(Axis3fSubSampler_t));
+  this->conversionFactor = conversionFactor;
+}
+
+void axis3fSubSamplerAccumulate(Axis3fSubSampler_t* this, const Axis3f* sample) {
+  this->sum.x += sample->x;
+  this->sum.y += sample->y;
+  this->sum.z += sample->z;
+
+  this->count++;
+}
+
+Axis3f* axis3fSubSamplerFinalize(Axis3fSubSampler_t* this) {
+  if (this->count > 0) {
+    this->subSample.x = this->sum.x * this->conversionFactor / this->count;
+    this->subSample.y = this->sum.y * this->conversionFactor / this->count;
+    this->subSample.z = this->sum.z * this->conversionFactor / this->count;
+
+    // Reset
+    this->count = 0;
+    this->sum = (Axis3f){.axis={0}};
+  }
+
+  return &this->subSample;
+}
diff --git a/test/modules/src/test_axis3fSubSampler.c b/test/modules/src/test_axis3fSubSampler.c
new file mode 100644
index 0000000000..290e3c8d0b
--- /dev/null
+++ b/test/modules/src/test_axis3fSubSampler.c
@@ -0,0 +1,100 @@
+// File under test axis3fSubSampler.c
+#include "axis3fSubSampler.h"
+
+#include "unity.h"
+
+Axis3fSubSampler_t subSampler;
+
+const Axis3f sample1 = {.x=1.0, .y=2.0, .z=3.0};
+const Axis3f sample2 = {.x=4.0, .y=5.0, .z=6.0};
+const Axis3f sample3 = {.x=7.0, .y=8.0, .z=9.0};
+
+void setUp(void) {
+}
+
+void tearDown(void) {
+  // Empty
+}
+
+
+void testThatOneSampleIsUnchanged() {
+  // Fixture
+  axis3fSubSamplerInit(&subSampler, 1.0);
+
+  axis3fSubSamplerAccumulate(&subSampler, &sample1);
+
+  // Test
+  Axis3f* actual = axis3fSubSamplerFinalize(&subSampler);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(sample1.x, actual->x);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.y, actual->y);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.z, actual->z);
+}
+
+
+void testThatSamplesAreAveraged() {
+  // Fixture
+  axis3fSubSamplerInit(&subSampler, 1.0);
+
+  axis3fSubSamplerAccumulate(&subSampler, &sample1);
+  axis3fSubSamplerAccumulate(&subSampler, &sample2);
+  axis3fSubSamplerAccumulate(&subSampler, &sample3);
+
+  // Test
+  Axis3f* actual = axis3fSubSamplerFinalize(&subSampler);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(4.0, actual->x);
+  TEST_ASSERT_EQUAL_FLOAT(5.0, actual->y);
+  TEST_ASSERT_EQUAL_FLOAT(6.0, actual->z);
+}
+
+
+void testThatResultIsUnchangedWhenNoNewSamplesAreAccumulated() {
+  // Fixture
+  axis3fSubSamplerInit(&subSampler, 1.0);
+  axis3fSubSamplerAccumulate(&subSampler, &sample1);
+  axis3fSubSamplerFinalize(&subSampler);
+  // The sub sampler has now been finalized
+
+  // Test
+  Axis3f* actual = axis3fSubSamplerFinalize(&subSampler);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(sample1.x, actual->x);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.y, actual->y);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.z, actual->z);
+}
+
+void testThatResultIsUpdatedWhenNewSamplesAreAccumulated() {
+  // Fixture
+  axis3fSubSamplerInit(&subSampler, 1.0);
+  axis3fSubSamplerAccumulate(&subSampler, &sample1);
+  axis3fSubSamplerFinalize(&subSampler);
+  // The sub sampler has now been finalized
+
+  // Test
+  axis3fSubSamplerAccumulate(&subSampler, &sample2);
+  Axis3f* actual = axis3fSubSamplerFinalize(&subSampler);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(sample2.x, actual->x);
+  TEST_ASSERT_EQUAL_FLOAT(sample2.y, actual->y);
+  TEST_ASSERT_EQUAL_FLOAT(sample2.z, actual->z);
+}
+
+void testThatResultIsMultipliedWithConversionFactor() {
+  // Fixture
+  axis3fSubSamplerInit(&subSampler, 3.0);
+
+  axis3fSubSamplerAccumulate(&subSampler, &sample1);
+
+  // Test
+  Axis3f* actual = axis3fSubSamplerFinalize(&subSampler);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(sample1.x * 3.0f, actual->x);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.y * 3.0f, actual->y);
+  TEST_ASSERT_EQUAL_FLOAT(sample1.z * 3.0f, actual->z);
+}

From 1c15a9e60a940d8ee7ebdc18b24e60b356c34fd0 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sun, 22 Jan 2023 17:28:55 +0100
Subject: [PATCH 2/2] Refactoring of kalman estimator

---
 .../interface/kalman_core/kalman_core.h       |  24 ++-
 .../interface/kalman_core/mm_sweep_angles.h   |   2 +-
 src/modules/interface/outlierFilter.h         |   8 +-
 src/modules/interface/stabilizer_types.h      |   2 -
 src/modules/src/estimator_kalman.c            | 176 +++++-------------
 src/modules/src/kalman_core/kalman_core.c     |  69 ++++---
 src/modules/src/kalman_core/mm_sweep_angles.c |   4 +-
 src/modules/src/outlierFilter.c               |  34 ++--
 test/modules/src/test_outlier_filter.c        |  12 --
 9 files changed, 131 insertions(+), 200 deletions(-)

diff --git a/src/modules/interface/kalman_core/kalman_core.h b/src/modules/interface/kalman_core/kalman_core.h
index a5c471fc0d..a0c0428c7b 100644
--- a/src/modules/interface/kalman_core/kalman_core.h
+++ b/src/modules/interface/kalman_core/kalman_core.h
@@ -93,6 +93,12 @@ typedef struct {
 
   // Quaternion used for initial orientation [w,x,y,z]
   float initialQuaternion[4];
+
+  // Tracks whether an update to the state has been made, and the state therefore requires finalization
+  bool isUpdated;
+
+  uint32_t lastPredictionMs;
+  uint32_t lastProcessNoiseUpdateMs;
 } kalmanCoreData_t;
 
 // The parameters used by the filter
@@ -129,7 +135,7 @@ typedef struct {
 void kalmanCoreDefaultParams(kalmanCoreParams_t *params);
 
 /*  - Initialize Kalman State */
-void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params);
+void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs);
 
 /*  - Measurement updates based on sensors */
 
@@ -141,15 +147,21 @@ void kalmanCoreUpdateWithBaro(kalmanCoreData_t *this, const kalmanCoreParams_t *
  *
  * The filter progresses as:
  *  - Predicting the current state forward */
-void kalmanCorePredict(kalmanCoreData_t *this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying);
+void kalmanCorePredict(kalmanCoreData_t *this, Axis3f *acc, Axis3f *gyro, const uint32_t nowMs, bool quadIsFlying);
 
-void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, float dt);
+void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs);
 
-/*  - Finalization to incorporate attitude error into body attitude */
-void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick);
+/**
+ * @brief Finalization to incorporate attitude error into body attitude
+ *
+ * @param this Core data
+ * @return true The state was finalized
+ * @return false The state was not changed and did not require finalization
+ */
+bool kalmanCoreFinalize(kalmanCoreData_t* this);
 
 /*  - Externalization to move the filter's internal state into the external state expected by other modules */
-void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick);
+void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc);
 
 void kalmanCoreDecoupleXY(kalmanCoreData_t* this);
 
diff --git a/src/modules/interface/kalman_core/mm_sweep_angles.h b/src/modules/interface/kalman_core/mm_sweep_angles.h
index 4c77d8ebf3..836ca3c745 100644
--- a/src/modules/interface/kalman_core/mm_sweep_angles.h
+++ b/src/modules/interface/kalman_core/mm_sweep_angles.h
@@ -29,4 +29,4 @@
 #include "outlierFilter.h"
 
 // Measurement of sweep angles from a Lighthouse base station
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick, OutlierFilterLhState_t* sweepOutlierFilterState);
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t nowMs, OutlierFilterLhState_t* sweepOutlierFilterState);
diff --git a/src/modules/interface/outlierFilter.h b/src/modules/interface/outlierFilter.h
index ea6a81e0c8..c479a02abf 100644
--- a/src/modules/interface/outlierFilter.h
+++ b/src/modules/interface/outlierFilter.h
@@ -31,11 +31,11 @@ bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
 bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
 
 typedef struct {
-    uint32_t openingTime;
-    int32_t openingWindow;
+    uint32_t openingTimeMs;
+    int32_t openingWindowMs;
 } OutlierFilterLhState_t;
-bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now);
-void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now);
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t nowMs);
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t nowMs);
 
 
 #endif // __OUTLIER_FILTER_H__
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index 0dac66ff01..afe13a6c8f 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -65,8 +65,6 @@ typedef struct vec3_s acc_t;
 
 /* Orientation as a quaternion */
 typedef struct quaternion_s {
-  uint32_t timestamp;
-
   union {
     struct {
       float q0;
diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c
index 0400a15302..4ee8b29bc1 100644
--- a/src/modules/src/estimator_kalman.c
+++ b/src/modules/src/estimator_kalman.c
@@ -73,6 +73,7 @@
 #include "param.h"
 #include "physicalConstants.h"
 #include "supervisor.h"
+#include "axis3fSubSampler.h"
 
 #include "statsCnt.h"
 #include "rateSupervisor.h"
@@ -139,13 +140,10 @@ NO_DMA_CCM_SAFE_ZERO_INIT static kalmanCoreData_t coreData;
 
 static bool isInit = false;
 
-static Axis3f accAccumulator;
-static Axis3f gyroAccumulator;
-static uint32_t accAccumulatorCount;
-static uint32_t gyroAccumulatorCount;
+static Axis3fSubSampler_t accSubSampler;
+static Axis3fSubSampler_t gyroSubSampler;
 static Axis3f accLatest;
 static Axis3f gyroLatest;
-static bool quadIsFlying = false;
 
 static OutlierFilterLhState_t sweepOutlierFilterState;
 
@@ -167,8 +165,8 @@ static STATS_CNT_RATE_DEFINE(finalizeCounter, ONE_SECOND);
 
 static rateSupervisor_t rateSupervisorContext;
 
-#define WARNING_HOLD_BACK_TIME M2T(2000)
-static uint32_t warningBlockTime = 0;
+#define WARNING_HOLD_BACK_TIME_MS 2000
+static uint32_t warningBlockTimeMs = 0;
 
 #ifdef KALMAN_USE_BARO_UPDATE
 static const bool useBaroUpdate = true;
@@ -177,8 +175,7 @@ static const bool useBaroUpdate = false;
 #endif
 
 static void kalmanTask(void* parameters);
-static bool predictStateForward(uint32_t osTick, float dt);
-static bool updateQueuedMeasurements(const uint32_t tick);
+static void updateQueuedMeasurements(const uint32_t nowMs, const bool quadIsFlying);
 
 STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(kalmanTask, KALMAN_TASK_STACKSIZE);
 
@@ -204,84 +201,57 @@ bool estimatorKalmanTaskTest() {
 static void kalmanTask(void* parameters) {
   systemWaitStart();
 
-  uint32_t lastPrediction = xTaskGetTickCount();
-  uint32_t nextPrediction = xTaskGetTickCount();
-  uint32_t lastPNUpdate = xTaskGetTickCount();
+  uint32_t nowMs = T2M(xTaskGetTickCount());
+  uint32_t nextPredictionMs = nowMs;
 
-  rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), ONE_SECOND, PREDICT_RATE - 1, PREDICT_RATE + 1, 1);
+  rateSupervisorInit(&rateSupervisorContext, nowMs, ONE_SECOND, PREDICT_RATE - 1, PREDICT_RATE + 1, 1);
 
   while (true) {
     xSemaphoreTake(runTaskSemaphore, portMAX_DELAY);
+    nowMs = T2M(xTaskGetTickCount()); // would be nice if this had a precision higher than 1ms...
 
-    // If the client triggers an estimator reset via parameter update
     if (resetEstimation) {
       estimatorKalmanInit();
       resetEstimation = false;
     }
 
-    // Tracks whether an update to the state has been made, and the state therefore requires finalization
-    bool doneUpdate = false;
-
-    uint32_t osTick = xTaskGetTickCount(); // would be nice if this had a precision higher than 1ms...
+    bool quadIsFlying = supervisorIsFlying();
 
   #ifdef KALMAN_DECOUPLE_XY
     kalmanCoreDecoupleXY(&coreData);
   #endif
 
     // Run the system dynamics to predict the state forward.
-    if (osTick >= nextPrediction) { // update at the PREDICT_RATE
-      float dt = T2S(osTick - lastPrediction);
-      if (predictStateForward(osTick, dt)) {
-        lastPrediction = osTick;
-        doneUpdate = true;
-        STATS_CNT_RATE_EVENT(&predictionCounter);
-      }
+    if (nowMs >= nextPredictionMs) {
+      axis3fSubSamplerFinalize(&accSubSampler);
+      axis3fSubSamplerFinalize(&gyroSubSampler);
 
-      nextPrediction = nextPrediction + S2T(1.0f / PREDICT_RATE);
-      if (osTick > nextPrediction) {
-        // Overrun
-        nextPrediction = osTick + S2T(1.0f / PREDICT_RATE);
-      }
-      if (!rateSupervisorValidate(&rateSupervisorContext, T2M(osTick))) {
-        DEBUG_PRINT("WARNING: Kalman prediction rate low (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
-      }
-    }
+      kalmanCorePredict(&coreData, &accSubSampler.subSample, &gyroSubSampler.subSample, nowMs, quadIsFlying);
+      nextPredictionMs = nowMs + (1000.0f / PREDICT_RATE);
 
-    /**
-     * Add process noise every loop, rather than every prediction
-     */
-    {
-      float dt = T2S(osTick - lastPNUpdate);
-      if (dt > 0.0f) {
-        kalmanCoreAddProcessNoise(&coreData, &coreParams, dt);
-        lastPNUpdate = osTick;
-      }
-    }
+      STATS_CNT_RATE_EVENT(&predictionCounter);
 
-    {
-      if(updateQueuedMeasurements(osTick)) {
-        doneUpdate = true;
+      if (!rateSupervisorValidate(&rateSupervisorContext, nowMs)) {
+        DEBUG_PRINT("WARNING: Kalman prediction rate low (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
       }
     }
 
-    /**
-     * If an update has been made, the state is finalized:
-     * - the attitude error is moved into the body attitude quaternion,
-     * - the body attitude is converted into a rotation matrix for the next prediction, and
-     * - correctness of the covariance matrix is ensured
-     */
+    // Add process noise every loop, rather than every prediction
+    kalmanCoreAddProcessNoise(&coreData, &coreParams, nowMs);
+
+    updateQueuedMeasurements(nowMs, quadIsFlying);
 
-    if (doneUpdate)
+    if (kalmanCoreFinalize(&coreData))
     {
-      kalmanCoreFinalize(&coreData, osTick);
       STATS_CNT_RATE_EVENT(&finalizeCounter);
-      if (! kalmanSupervisorIsStateWithinBounds(&coreData)) {
-        resetEstimation = true;
+    }
 
-        if (osTick > warningBlockTime) {
-          warningBlockTime = osTick + WARNING_HOLD_BACK_TIME;
-          DEBUG_PRINT("State out of bounds, resetting\n");
-        }
+    if (! kalmanSupervisorIsStateWithinBounds(&coreData)) {
+      resetEstimation = true;
+
+      if (nowMs > warningBlockTimeMs) {
+        warningBlockTimeMs = nowMs + WARNING_HOLD_BACK_TIME_MS;
+        DEBUG_PRINT("State out of bounds, resetting\n");
       }
     }
 
@@ -290,15 +260,14 @@ static void kalmanTask(void* parameters) {
      * This is done every round, since the external state includes some sensor data
      */
     xSemaphoreTake(dataMutex, portMAX_DELAY);
-    kalmanCoreExternalizeState(&coreData, &taskEstimatorState, &accLatest, osTick);
+    kalmanCoreExternalizeState(&coreData, &taskEstimatorState, &accLatest);
     xSemaphoreGive(dataMutex);
 
     STATS_CNT_RATE_EVENT(&updateCounter);
   }
 }
 
-void estimatorKalman(state_t *state, const uint32_t tick)
-{
+void estimatorKalman(state_t *state, const uint32_t tick) {
   // This function is called from the stabilizer loop. It is important that this call returns
   // as quickly as possible. The dataMutex must only be locked short periods by the task.
   xSemaphoreTake(dataMutex, portMAX_DELAY);
@@ -310,40 +279,7 @@ void estimatorKalman(state_t *state, const uint32_t tick)
   xSemaphoreGive(runTaskSemaphore);
 }
 
-static bool predictStateForward(uint32_t osTick, float dt) {
-  if (gyroAccumulatorCount == 0
-      || accAccumulatorCount == 0)
-  {
-    return false;
-  }
-
-  // gyro is in deg/sec but the estimator requires rad/sec
-  Axis3f gyroAverage;
-  gyroAverage.x = gyroAccumulator.x * DEG_TO_RAD / gyroAccumulatorCount;
-  gyroAverage.y = gyroAccumulator.y * DEG_TO_RAD / gyroAccumulatorCount;
-  gyroAverage.z = gyroAccumulator.z * DEG_TO_RAD / gyroAccumulatorCount;
-
-  // accelerometer is in Gs but the estimator requires ms^-2
-  Axis3f accAverage;
-  accAverage.x = accAccumulator.x * GRAVITY_MAGNITUDE / accAccumulatorCount;
-  accAverage.y = accAccumulator.y * GRAVITY_MAGNITUDE / accAccumulatorCount;
-  accAverage.z = accAccumulator.z * GRAVITY_MAGNITUDE / accAccumulatorCount;
-
-  // reset for next call
-  accAccumulator = (Axis3f){.axis={0}};
-  accAccumulatorCount = 0;
-  gyroAccumulator = (Axis3f){.axis={0}};
-  gyroAccumulatorCount = 0;
-
-  quadIsFlying = supervisorIsFlying();
-  kalmanCorePredict(&coreData, &accAverage, &gyroAverage, dt, quadIsFlying);
-
-  return true;
-}
-
-
-static bool updateQueuedMeasurements(const uint32_t tick) {
-  bool doneUpdate = false;
+static void updateQueuedMeasurements(const uint32_t nowMs, const bool quadIsFlying) {
   /**
    * Sensor measurements can come in sporadically and faster than the stabilizer loop frequency,
    * we therefore consume all measurements since the last loop, rather than accumulating
@@ -361,15 +297,12 @@ static bool updateQueuedMeasurements(const uint32_t tick) {
           // standard KF update
           kalmanCoreUpdateWithTDOA(&coreData, &m.data.tdoa);
         }
-        doneUpdate = true;
         break;
       case MeasurementTypePosition:
         kalmanCoreUpdateWithPosition(&coreData, &m.data.position);
-        doneUpdate = true;
         break;
       case MeasurementTypePose:
         kalmanCoreUpdateWithPose(&coreData, &m.data.pose);
-        doneUpdate = true;
         break;
       case MeasurementTypeDistance:
         if(robustTwr){
@@ -379,67 +312,51 @@ static bool updateQueuedMeasurements(const uint32_t tick) {
             // standard KF update
             kalmanCoreUpdateWithDistance(&coreData, &m.data.distance);
         }
-        doneUpdate = true;
         break;
       case MeasurementTypeTOF:
         kalmanCoreUpdateWithTof(&coreData, &m.data.tof);
-        doneUpdate = true;
         break;
       case MeasurementTypeAbsoluteHeight:
         kalmanCoreUpdateWithAbsoluteHeight(&coreData, &m.data.height);
-        doneUpdate = true;
         break;
       case MeasurementTypeFlow:
         kalmanCoreUpdateWithFlow(&coreData, &m.data.flow, &gyroLatest);
-        doneUpdate = true;
         break;
       case MeasurementTypeYawError:
         kalmanCoreUpdateWithYawError(&coreData, &m.data.yawError);
-        doneUpdate = true;
         break;
       case MeasurementTypeSweepAngle:
-        kalmanCoreUpdateWithSweepAngles(&coreData, &m.data.sweepAngle, tick, &sweepOutlierFilterState);
-        doneUpdate = true;
+        kalmanCoreUpdateWithSweepAngles(&coreData, &m.data.sweepAngle, nowMs, &sweepOutlierFilterState);
         break;
       case MeasurementTypeGyroscope:
-        gyroAccumulator.x += m.data.gyroscope.gyro.x;
-        gyroAccumulator.y += m.data.gyroscope.gyro.y;
-        gyroAccumulator.z += m.data.gyroscope.gyro.z;
+        axis3fSubSamplerAccumulate(&gyroSubSampler, &m.data.gyroscope.gyro);
         gyroLatest = m.data.gyroscope.gyro;
-        gyroAccumulatorCount++;
         break;
       case MeasurementTypeAcceleration:
-        accAccumulator.x += m.data.acceleration.acc.x;
-        accAccumulator.y += m.data.acceleration.acc.y;
-        accAccumulator.z += m.data.acceleration.acc.z;
+        axis3fSubSamplerAccumulate(&accSubSampler, &m.data.acceleration.acc);
         accLatest = m.data.acceleration.acc;
-        accAccumulatorCount++;
         break;
       case MeasurementTypeBarometer:
         if (useBaroUpdate) {
           kalmanCoreUpdateWithBaro(&coreData, &coreParams, m.data.barometer.baro.asl, quadIsFlying);
-          doneUpdate = true;
         }
         break;
       default:
         break;
     }
   }
-
-  return doneUpdate;
 }
 
 // Called when this estimator is activated
 void estimatorKalmanInit(void)
 {
-  accAccumulator = (Axis3f){.axis = {0}};
-  gyroAccumulator = (Axis3f){.axis = {0}};
+  axis3fSubSamplerInit(&accSubSampler, GRAVITY_MAGNITUDE);
+  axis3fSubSamplerInit(&gyroSubSampler, DEG_TO_RAD);
 
-  accAccumulatorCount = 0;
-  gyroAccumulatorCount = 0;
   outlierFilterReset(&sweepOutlierFilterState, 0);
 
-  kalmanCoreInit(&coreData, &coreParams);
+  uint32_t nowMs = T2M(xTaskGetTickCount());
+  kalmanCoreInit(&coreData, &coreParams, nowMs);
 }
 
 bool estimatorKalmanTest(void)
@@ -461,13 +378,7 @@ void estimatorKalmanGetEstimatedRot(float * rotationMatrix) {
  * Variables and results from the Extended Kalman Filter
  */
 LOG_GROUP_START(kalman)
-/**
- * @brief Nonzero if the drone is in flight
- *
- *  Note: This is the same as sys.flying. Perhaps remove this one?
- */
-  LOG_ADD(LOG_UINT8, inFlight, &quadIsFlying)
-  /**
+ /**
  * @brief State position in the global frame x
  *
  *   Note: This is similar to stateEstimate.x.
@@ -582,7 +493,7 @@ LOG_GROUP_START(kalman)
 LOG_GROUP_STOP(kalman)
 
 LOG_GROUP_START(outlierf)
-  LOG_ADD(LOG_INT32, lhWin, &sweepOutlierFilterState.openingWindow)
+  LOG_ADD(LOG_INT32, lhWin, &sweepOutlierFilterState.openingWindowMs)
 LOG_GROUP_STOP(outlierf)
 
 /**
@@ -594,7 +505,6 @@ PARAM_GROUP_START(kalman)
  * @brief Reset the kalman estimator
  */
   PARAM_ADD_CORE(PARAM_UINT8, resetEstimation, &resetEstimation)
-  PARAM_ADD(PARAM_UINT8, quadIsFlying, &quadIsFlying)
 /**
  * @brief Nonzero to use robust TDOA method (default: 0)
  */
diff --git a/src/modules/src/kalman_core/kalman_core.c b/src/modules/src/kalman_core/kalman_core.c
index 82877fdcc6..758f47ab83 100644
--- a/src/modules/src/kalman_core/kalman_core.c
+++ b/src/modules/src/kalman_core/kalman_core.c
@@ -152,7 +152,7 @@ void kalmanCoreDefaultParams(kalmanCoreParams_t* params)
   params->initialYaw = 0.0;
 }
 
-void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params)
+void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs)
 {
   // Reset all data to 0 (like upon system reset)
   memset(this, 0, sizeof(kalmanCoreData_t));
@@ -203,6 +203,10 @@ void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params)
   this->Pm.pData = (float*)this->P;
 
   this->baroReferenceHeight = 0.0;
+
+  this->isUpdated = false;
+  this->lastPredictionMs = nowMs;
+  this->lastProcessNoiseUpdateMs = nowMs;
 }
 
 void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise)
@@ -273,6 +277,8 @@ void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm,
   }
 
   assertStateNotNaN(this);
+
+  this->isUpdated = true;
 }
 
 void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, arm_matrix_instance_f32 *Km, arm_matrix_instance_f32 *P_w_m, float error)
@@ -309,6 +315,7 @@ void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm
     }
     assertStateNotNaN(this);
 
+    this->isUpdated = true;
 }
 
 void kalmanCoreUpdateWithBaro(kalmanCoreData_t *this, const kalmanCoreParams_t *params, float baroAsl, bool quadIsFlying)
@@ -327,7 +334,7 @@ void kalmanCoreUpdateWithBaro(kalmanCoreData_t *this, const kalmanCoreParams_t *
   kalmanCoreScalarUpdate(this, &H, meas - this->S[KC_STATE_Z], params->measNoiseBaro);
 }
 
-void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
+static void predictDt(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
 {
   /* Here we discretize (euler forward) and linearise the quadrocopter dynamics in order
    * to push the covariance forward.
@@ -560,24 +567,30 @@ void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float
   float norm = arm_sqrt(tmpq0*tmpq0 + tmpq1*tmpq1 + tmpq2*tmpq2 + tmpq3*tmpq3) + EPS;
   this->q[0] = tmpq0/norm; this->q[1] = tmpq1/norm; this->q[2] = tmpq2/norm; this->q[3] = tmpq3/norm;
   assertStateNotNaN(this);
+
+  this->isUpdated = true;
+}
+
+void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, const uint32_t nowMs, bool quadIsFlying) {
+  float dt = (nowMs - this->lastPredictionMs) / 1000.0f;
+  predictDt(this, acc, gyro, dt, quadIsFlying);
+  this->lastPredictionMs = nowMs;
 }
 
-void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, float dt)
+
+static void addProcessNoiseDt(kalmanCoreData_t *this, const kalmanCoreParams_t *params, float dt)
 {
-  if (dt>0)
-  {
-    this->P[KC_STATE_X][KC_STATE_X] += powf(params->procNoiseAcc_xy*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
-    this->P[KC_STATE_Y][KC_STATE_Y] += powf(params->procNoiseAcc_xy*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
-    this->P[KC_STATE_Z][KC_STATE_Z] += powf(params->procNoiseAcc_z*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
+  this->P[KC_STATE_X][KC_STATE_X] += powf(params->procNoiseAcc_xy*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
+  this->P[KC_STATE_Y][KC_STATE_Y] += powf(params->procNoiseAcc_xy*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
+  this->P[KC_STATE_Z][KC_STATE_Z] += powf(params->procNoiseAcc_z*dt*dt + params->procNoiseVel*dt + params->procNoisePos, 2);  // add process noise on position
 
-    this->P[KC_STATE_PX][KC_STATE_PX] += powf(params->procNoiseAcc_xy*dt + params->procNoiseVel, 2); // add process noise on velocity
-    this->P[KC_STATE_PY][KC_STATE_PY] += powf(params->procNoiseAcc_xy*dt + params->procNoiseVel, 2); // add process noise on velocity
-    this->P[KC_STATE_PZ][KC_STATE_PZ] += powf(params->procNoiseAcc_z*dt + params->procNoiseVel, 2); // add process noise on velocity
+  this->P[KC_STATE_PX][KC_STATE_PX] += powf(params->procNoiseAcc_xy*dt + params->procNoiseVel, 2); // add process noise on velocity
+  this->P[KC_STATE_PY][KC_STATE_PY] += powf(params->procNoiseAcc_xy*dt + params->procNoiseVel, 2); // add process noise on velocity
+  this->P[KC_STATE_PZ][KC_STATE_PZ] += powf(params->procNoiseAcc_z*dt + params->procNoiseVel, 2); // add process noise on velocity
 
-    this->P[KC_STATE_D0][KC_STATE_D0] += powf(params->measNoiseGyro_rollpitch * dt + params->procNoiseAtt, 2);
-    this->P[KC_STATE_D1][KC_STATE_D1] += powf(params->measNoiseGyro_rollpitch * dt + params->procNoiseAtt, 2);
-    this->P[KC_STATE_D2][KC_STATE_D2] += powf(params->measNoiseGyro_yaw * dt + params->procNoiseAtt, 2);
-  }
+  this->P[KC_STATE_D0][KC_STATE_D0] += powf(params->measNoiseGyro_rollpitch * dt + params->procNoiseAtt, 2);
+  this->P[KC_STATE_D1][KC_STATE_D1] += powf(params->measNoiseGyro_rollpitch * dt + params->procNoiseAtt, 2);
+  this->P[KC_STATE_D2][KC_STATE_D2] += powf(params->measNoiseGyro_yaw * dt + params->procNoiseAtt, 2);
 
   for (int i=0; i<KC_STATE_DIM; i++) {
     for (int j=i; j<KC_STATE_DIM; j++) {
@@ -595,10 +608,22 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t
   assertStateNotNaN(this);
 }
 
+void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs) {
+  float dt = (nowMs - this->lastProcessNoiseUpdateMs) / 1000.0f;
+  if (dt > 0.0f) {
+    addProcessNoiseDt(this, params, dt);
+    this->lastProcessNoiseUpdateMs = nowMs;
+  }
+}
 
-
-void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
+bool kalmanCoreFinalize(kalmanCoreData_t* this)
 {
+  // Only finalize if data is updated
+  if (! this->isUpdated) {
+    return false;
+  }
+
+
   // Matrix to rotate the attitude covariances once updated
   NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
   static arm_matrix_instance_f32 Am = {KC_STATE_DIM, KC_STATE_DIM, (float *)A};
@@ -709,13 +734,15 @@ void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
   }
 
   assertStateNotNaN(this);
+
+  this->isUpdated = false;
+  return true;
 }
 
-void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick)
+void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc)
 {
   // position state is already in world frame
   state->position = (point_t){
-      .timestamp = tick,
       .x = this->S[KC_STATE_X],
       .y = this->S[KC_STATE_Y],
       .z = this->S[KC_STATE_Z]
@@ -723,7 +750,6 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
 
   // velocity is in body frame and needs to be rotated to world frame
   state->velocity = (velocity_t){
-      .timestamp = tick,
       .x = this->R[0][0]*this->S[KC_STATE_PX] + this->R[0][1]*this->S[KC_STATE_PY] + this->R[0][2]*this->S[KC_STATE_PZ],
       .y = this->R[1][0]*this->S[KC_STATE_PX] + this->R[1][1]*this->S[KC_STATE_PY] + this->R[1][2]*this->S[KC_STATE_PZ],
       .z = this->R[2][0]*this->S[KC_STATE_PX] + this->R[2][1]*this->S[KC_STATE_PY] + this->R[2][2]*this->S[KC_STATE_PZ]
@@ -733,7 +759,6 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
   // Furthermore, the legacy code requires acc.z to be acceleration without gravity.
   // Finally, note that these accelerations are in Gs, and not in m/s^2, hence - 1 for removing gravity
   state->acc = (acc_t){
-      .timestamp = tick,
       .x = this->R[0][0]*acc->x + this->R[0][1]*acc->y + this->R[0][2]*acc->z,
       .y = this->R[1][0]*acc->x + this->R[1][1]*acc->y + this->R[1][2]*acc->z,
       .z = this->R[2][0]*acc->x + this->R[2][1]*acc->y + this->R[2][2]*acc->z - 1
@@ -746,7 +771,6 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
 
   // Save attitude, adjusted for the legacy CF2 body coordinate system
   state->attitude = (attitude_t){
-      .timestamp = tick,
       .roll = roll*RAD_TO_DEG,
       .pitch = -pitch*RAD_TO_DEG,
       .yaw = yaw*RAD_TO_DEG
@@ -755,7 +779,6 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
   // Save quaternion, hopefully one day this could be used in a better controller.
   // Note that this is not adjusted for the legacy coordinate system
   state->attitudeQuaternion = (quaternion_t){
-      .timestamp = tick,
       .w = this->q[0],
       .x = this->q[1],
       .y = this->q[2],
diff --git a/src/modules/src/kalman_core/mm_sweep_angles.c b/src/modules/src/kalman_core/mm_sweep_angles.c
index 17c0aae934..e81c347fe2 100644
--- a/src/modules/src/kalman_core/mm_sweep_angles.c
+++ b/src/modules/src/kalman_core/mm_sweep_angles.c
@@ -27,7 +27,7 @@
 #include "outlierFilter.h"
 
 
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *sweepInfo, const uint32_t tick, OutlierFilterLhState_t* sweepOutlierFilterState) {
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *sweepInfo, const uint32_t nowMs, OutlierFilterLhState_t* sweepOutlierFilterState) {
   // Rotate the sensor position from CF reference frame to global reference frame,
   // using the CF roatation matrix
   vec3d s;
@@ -65,7 +65,7 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme
   const float measuredSweepAngle = sweepInfo->measuredSweepAngle;
   const float error = measuredSweepAngle - predictedSweepAngle;
 
-  if (outlierFilterValidateLighthouseSweep(sweepOutlierFilterState, r, error, tick)) {
+  if (outlierFilterValidateLighthouseSweep(sweepOutlierFilterState, r, error, nowMs)) {
     // Calculate H vector (in the rotor reference frame)
     const float z_tan_t = z * tan_t;
     const float qNum = r2 - z_tan_t * z_tan_t;
diff --git a/src/modules/src/outlierFilter.c b/src/modules/src/outlierFilter.c
index a6fab9f948..ce9d436cc1 100644
--- a/src/modules/src/outlierFilter.c
+++ b/src/modules/src/outlierFilter.c
@@ -103,44 +103,44 @@ bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float e
 }
 
 
-#define LH_TICKS_PER_FRAME (1000 / 120)
-static const int32_t lhMinWindowTime = -2 * LH_TICKS_PER_FRAME;
-static const int32_t lhMaxWindowTime = 5 * LH_TICKS_PER_FRAME;
-static const int32_t lhBadSampleWindowChange = -LH_TICKS_PER_FRAME;
-static const int32_t lhGoodSampleWindowChange = LH_TICKS_PER_FRAME / 2;
+#define LH_MS_PER_FRAME (1000 / 120)
+static const int32_t lhMinWindowTimeMs = -2 * LH_MS_PER_FRAME;
+static const int32_t lhMaxWindowTimeMs = 5 * LH_MS_PER_FRAME;
+static const int32_t lhBadSampleWindowChangeMs = -LH_MS_PER_FRAME;
+static const int32_t lhGoodSampleWindowChangeMs = LH_MS_PER_FRAME / 2;
 static const float lhMaxError = 0.05f;
 
-void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now) {
-  this->openingTime = now;
-  this->openingWindow = lhMinWindowTime;
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t nowMs) {
+  this->openingTimeMs = nowMs;
+  this->openingWindowMs = lhMinWindowTimeMs;
 }
 
 
-bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now) {
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t nowMs) {
   // float error = distanceToBs * tan(angleError);
   // We use an approximattion
   float error = distanceToBs * angleError;
 
   bool isGoodSample = (fabsf(error) < lhMaxError);
   if (isGoodSample) {
-    this->openingWindow += lhGoodSampleWindowChange;
-    if (this->openingWindow > lhMaxWindowTime) {
-      this->openingWindow = lhMaxWindowTime;
+    this->openingWindowMs += lhGoodSampleWindowChangeMs;
+    if (this->openingWindowMs > lhMaxWindowTimeMs) {
+      this->openingWindowMs = lhMaxWindowTimeMs;
     }
   } else {
-    this->openingWindow += lhBadSampleWindowChange;
-    if (this->openingWindow < lhMinWindowTime) {
-      this->openingWindow = lhMinWindowTime;
+    this->openingWindowMs += lhBadSampleWindowChangeMs;
+    if (this->openingWindowMs < lhMinWindowTimeMs) {
+      this->openingWindowMs = lhMinWindowTimeMs;
     }
   }
 
   bool result = true;
-  bool isFilterClosed = (now < this->openingTime);
+  bool isFilterClosed = (nowMs < this->openingTimeMs);
   if (isFilterClosed) {
     result = isGoodSample;
   }
 
-  this->openingTime = now + this->openingWindow;
+  this->openingTimeMs = nowMs + this->openingWindowMs;
 
   return result;
 }
diff --git a/test/modules/src/test_outlier_filter.c b/test/modules/src/test_outlier_filter.c
index 727e2dc1a7..cb93c1ff25 100644
--- a/test/modules/src/test_outlier_filter.c
+++ b/test/modules/src/test_outlier_filter.c
@@ -74,18 +74,6 @@ void testThatSamplesAreRejectedWhenTdoaIsGreaterButNegativeThanDistanceBetweenAn
 #define LH_GOOD_ANGLE 0.0001
 #define LH_TIME_STEP (1000 / 120)
 
-// TOOD krri remove
-static void print(OutlierFilterLhState_t* this, uint32_t time) {
-  printf("win:%i openingTime:%i time:%i ", this->openingWindow, this->openingTime, time);
-  bool isFilterClosed = (time < this->openingTime);
-  if (isFilterClosed) {
-    printf("Is closed\n");
-  } else {
-    printf("Is open\n");
-  }
-}
-
-
 void testThatLhFilterLetsGoodSampleThroughWhenOpen() {
   // Fixture
   OutlierFilterLhState_t this;