Improved TDoA outlier filter

src/modules/interface/kalman_core/mm_tdoa.h

@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2021 Bitcraze AB
+ * 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
@@ -28,4 +28,4 @@
 #include "kalman_core.h"
 
 // Measurements of a UWB Tx/Rx
-void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa);
+void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa, const uint32_t nowMs);

src/modules/interface/outlierFilter.h

@@ -5,7 +5,7 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * Copyright (C) 2011-2019 Bitcraze AB
+ * Copyright (C) 2011-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
@@ -29,6 +29,7 @@
 
 bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
 bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
+bool outlierFilterValidateTdoaIntegrator(const tdoaMeasurement_t* tdoa, const float error, const uint32_t nowMs);
 
 typedef struct {
     uint32_t openingTimeMs;

src/modules/src/estimator_kalman.c

@@ -299,7 +299,7 @@ static void updateQueuedMeasurements(const uint32_t nowMs, const bool quadIsFlyi
           kalmanCoreRobustUpdateWithTDOA(&coreData, &m.data.tdoa);
         }else{
           // standard KF update
-          kalmanCoreUpdateWithTDOA(&coreData, &m.data.tdoa);
+          kalmanCoreUpdateWithTDOA(&coreData, &m.data.tdoa, nowMs);
         }
         break;
       case MeasurementTypePosition:

src/modules/src/kalman_core/mm_tdoa.c

@@ -30,7 +30,12 @@
 // 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)
+// Enable this to use the old step outlier filter
+// This filter is deprecated and will be removed after October 2023
+// #define USE_STEP_OUTLIER_FILTER 1
+
+
+void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa, const uint32_t nowMs)
 {
   if (tdoaCount >= 100)
   {
@@ -71,6 +76,7 @@ void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa)
       h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
       h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
 
+    #if USE_STEP_OUTLIER_FILTER
       vector_t jacobian = {
         .x = h[KC_STATE_X],
         .y = h[KC_STATE_Y],
@@ -84,6 +90,10 @@ void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa)
       };
 
       bool sampleIsGood = outlierFilterValidateTdoaSteps(tdoa, error, &jacobian, &estimatedPosition);
+      #else
+      bool sampleIsGood = outlierFilterValidateTdoaIntegrator(tdoa, error, nowMs);
+      #endif
+
       if (sampleIsGood) {
         kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
       }

src/modules/src/outlierFilter.c

@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2011-2018 Bitcraze AB
+ * Copyright (C) 2011-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
@@ -30,6 +30,12 @@
 #include "log.h"
 #include "debug.h"
 
+
+static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(const tdoaMeasurement_t* tdoa);
+
+
+// The step TDoA outlier filter is deprecated and will be removed
+
 #define BUCKET_ACCEPTANCE_LEVEL 3
 #define MAX_BUCKET_FILL 10
 #define FILTER_CLOSE_DELAY_COUNT 30
@@ -55,19 +61,12 @@ filterLevel_t filterLevels[FILTER_LEVELS] = {
 };
 
 
-static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(const tdoaMeasurement_t* tdoa);
 static float distanceSq(const point_t* a, const point_t* b);
 static float sq(float a) {return a * a;}
 static void addToBucket(filterLevel_t* filter);
 static void removeFromBucket(filterLevel_t* filter);
 static int updateBuckets(float errorDistance);
 
-
-
-bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa) {
-  return isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa);
-}
-
 bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos) {
   bool sampleIsGood = false;
 
@@ -102,6 +101,84 @@ bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float e
   return sampleIsGood;
 }
 
+// Simple TDoA outlier filter
+bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa) {
+  return isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa);
+}
+
+
+// Integrator TDoA outlier filter -------------------------------------
+// This filter uses an "integrator" to track the ratio of good/samples. Samples with an errors (measurement - predicted)
+// larger than the acceptance level will be discarded. The acceptance level is based on the integrator, with a
+// hysteresis to avoid rapid changes. When the filter is open, all samples are let through, usually at startup to let
+// the kalman filter converge. When the filter is closed, only samples with an error < the acceptance level, this should
+// be most of the time.
+// The acceptance level is based on the standard deviation used for the tdoa samples, which should be based on the
+// noise level in the system.
+
+
+// The maximum size of the integrator. This size determines the time [in ms] needed to open/close the filter.
+static const float INTEGRATOR_SIZE = 300.0f;
+
+// The level when the filter open up to let all samples through
+static const float INTEGRATOR_FORCE_OPEN_LEVEL = INTEGRATOR_SIZE * 0.1f;
+
+// The level when the filter closes again
+static const float INTEGRATOR_RESUME_ACTION_LEVEL = INTEGRATOR_SIZE * 0.9f;
+
+static float integrator;
+static uint32_t latestUpdateMs;
+static bool isFilterOpen = true;
+
+
+bool outlierFilterValidateTdoaIntegrator(const tdoaMeasurement_t* tdoa, const float error, const uint32_t nowMs) {
+  // The accepted error when the filter is closed
+  const float acceptedDistance = tdoa->stdDev * 2.5f;
+
+  // The level used to determine if a sample is added or removed from the integrator
+  const float integratorTriggerDistance = tdoa->stdDev * 2.0f;
+
+
+  bool sampleIsGood = false;
+
+  // Discard samples that are physically impossible, most likely measurement error
+  if (isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa)) {
+    uint32_t dtMs = nowMs - latestUpdateMs;
+    // Limit dt to minimize the impact on the integrator if we have not received samples for a long time (or at start up)
+    dtMs = fminf(dtMs, INTEGRATOR_SIZE / 10.0f);
+
+    if (fabsf(error) < integratorTriggerDistance) {
+      integrator += dtMs;
+      integrator = fminf(integrator, INTEGRATOR_SIZE);
+    } else {
+      integrator -= dtMs;
+      integrator = fmaxf(integrator, 0.0f);
+    }
+
+    if (isFilterOpen) {
+      // The filter is open, let all samples through
+      sampleIsGood = true;
+
+      if (integrator > INTEGRATOR_RESUME_ACTION_LEVEL) {
+        // We have recovered and converged, close the filter again
+        isFilterOpen = false;
+      }
+    } else {
+      // The filter is closed, let samples with a small error through
+      sampleIsGood = (fabsf(error) < acceptedDistance);
+
+      if (integrator < INTEGRATOR_FORCE_OPEN_LEVEL) {
+        // We have got lots of outliers lately, the kalman filter may have diverged. Open up to try to recover
+        isFilterOpen = true;
+      }
+    }
+
+    latestUpdateMs = nowMs;
+  }
+
+  return sampleIsGood;
+}
+
 
 #define LH_MS_PER_FRAME (1000 / 120)
 static const int32_t lhMinWindowTimeMs = -2 * LH_MS_PER_FRAME;

test/modules/src/kalman_core/test_mm_tdoa.c

@@ -51,10 +51,10 @@ void testThatScalarUpdateIsCalledInSimpleCase() {
   };
 
   setKalmanCoreScalarUpdateExpectationsSingleCall(&this, expectedHm, expectedError, expectedStdMeasNoise);
-  outlierFilterValidateTdoaSteps_IgnoreAndReturn(true);
+  outlierFilterValidateTdoaIntegrator_IgnoreAndReturn(true);
 
   // Test
-  kalmanCoreUpdateWithTDOA(&this, &measurement);
+  kalmanCoreUpdateWithTDOA(&this, &measurement, 0);
 
   // Assert
   assertScalarUpdateWasCalled();
@@ -77,7 +77,7 @@ void testThatSampleWhereDroneIsInSamePositionAsAnchorIsIgnored() {
   };
 
   // Test
-  kalmanCoreUpdateWithTDOA(&this, &measurement);
+  kalmanCoreUpdateWithTDOA(&this, &measurement, 0);
 
   // Assert
   assertScalarUpdateWasNotCalled();
@@ -99,10 +99,10 @@ void testThatScalarUpdateIsNotCalledWhenTheOutlierFilterIsBlocking() {
     .stdDev = 0.123,
   };
 
-  outlierFilterValidateTdoaSteps_IgnoreAndReturn(false);
+  outlierFilterValidateTdoaIntegrator_IgnoreAndReturn(false);
 
   // Test
-  kalmanCoreUpdateWithTDOA(&this, &measurement);
+  kalmanCoreUpdateWithTDOA(&this, &measurement, 0);
 
   // Assert
   assertScalarUpdateWasNotCalled();