#375 Improved outlier filter for TDoA

The call to the outlier filter has been moved to the kalman filter.
The outlier filter is based on the estimated distance from the current estimated position to the candidate sample.
A number of counters are used to keep track of how many samples that are within certain error levels.
The counters in turn are used to determine which error level to accept and if a sample should be passed on to the kalman filter or not.

src/deck/drivers/src/lpsTdoa2Tag.c

@@ -38,7 +38,6 @@
 
 #include "estimator.h"
 #include "estimator_kalman.h"
-#include "outlierFilter.h"
 
 #include "physicalConstants.h"
 
@@ -152,12 +151,10 @@ static void enqueueTDOA(uint8_t anchorA, uint8_t anchorB, double distanceDiff) {
     .anchorPosition[1] = options->anchorPosition[anchorB]
   };
 
-  if (outlierFilterValidateTdoa(&tdoa)) {
-    if (options->combinedAnchorPositionOk ||
-        (options->anchorPosition[anchorA].timestamp && options->anchorPosition[anchorB].timestamp)) {
-      stats.packetsToEstimator++;
-      estimatorKalmanEnqueueTDOA(&tdoa);
-    }
+  if (options->combinedAnchorPositionOk ||
+      (options->anchorPosition[anchorA].timestamp && options->anchorPosition[anchorB].timestamp)) {
+    stats.packetsToEstimator++;
+    estimatorKalmanEnqueueTDOA(&tdoa);
   }
 }
 

src/modules/interface/estimator_kalman.h

@@ -80,4 +80,4 @@ void estimatorKalmanSetShift(float deltax, float deltay);
 
 void estimatorKalmanGetEstimatedPos(point_t* pos);
 
-#endif // __ESTIMATOR_KALMAN_H__
+#endif // __ESTIMATOR_KALMAN_H__

src/utils/interface/outlierFilter.h → src/modules/interface/outlierFilter.h

@@ -29,7 +29,9 @@
 
 #include "stabilizer_types.h"
 
-bool outlierFilterValidateTdoa(tdoaMeasurement_t* tdoa);
+bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
+bool outlierFilterVaildateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
+
 void outlierFilterReset();
 
 #endif // __OUTLIER_FILTER_H__

src/modules/interface/stabilizer_types.h

@@ -52,6 +52,7 @@ struct vec3_s {
   float z;
 };
 
+typedef struct vec3_s vector_t;
 typedef struct vec3_s point_t;
 typedef struct vec3_s velocity_t;
 typedef struct vec3_s acc_t;

src/modules/src/estimator_kalman.c

@@ -56,6 +56,8 @@
  */
 
 #include "estimator_kalman.h"
+#include "outlierFilter.h"
+
 
 #include "stm32f4xx.h"
 
@@ -1019,39 +1021,28 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
     float h[STATE_DIM] = {0};
     arm_matrix_instance_f32 H = {1, STATE_DIM, h};
 
-    // We want to do
-    // h[STATE_X] = (dx1 / d1 - dx0 / d0);
-    // h[STATE_Y] = (dy1 / d1 - dy0 / d0);
-    // h[STATE_Z] = (dz1 / d1 - dz0 / d0);
-    // but have to handle divide by zero
-
-    if (d1 != 0.0f)
-    {
-      h[STATE_X] = dx1 / d1;
-      h[STATE_Y] = dy1 / d1;
-      h[STATE_Z] = dz1 / d1;
-    }
-    else
-    {
-      h[STATE_X] = 1.0f;
-      h[STATE_Y] = 0.0f;
-      h[STATE_Z] = 0.0f;
-    }
-
-    if (d0 != 0.0f)
-    {
-      h[STATE_X] = h[STATE_X] - dx0 / d0;
-      h[STATE_Y] = h[STATE_Y] - dy0 / d0;
-      h[STATE_Z] = h[STATE_Z] - dz0 / d0;
-    }
-    else
-    {
-      h[STATE_X] = h[STATE_X] - 0.0f;
-      h[STATE_Y] = h[STATE_Y] - 1.0f;
-      h[STATE_Z] = h[STATE_Z] - 0.0f;
+    if ((d0 != 0.0f) && (d1 != 0.0f)) {
+      h[STATE_X] = (dx1 / d1 - dx0 / d0);
+      h[STATE_Y] = (dy1 / d1 - dy0 / d0);
+      h[STATE_Z] = (dz1 / d1 - dz0 / d0);
+
+      vector_t jacobian = {
+        .x = h[STATE_X],
+        .y = h[STATE_Y],
+        .z = h[STATE_Z],
+      };
+
+      point_t estimatedPosition = {
+        .x = S[STATE_X],
+        .y = S[STATE_Y],
+        .z = S[STATE_Z],
+      };
+
+      bool sampleIsGood = outlierFilterVaildateTdoaSteps(tdoa, error, &jacobian, &estimatedPosition);
+      if (sampleIsGood) {
+        stateEstimatorScalarUpdate(&H, error, tdoa->stdDev);
+      }
     }
-
-    stateEstimatorScalarUpdate(&H, error, tdoa->stdDev);
   }
 
   tdoaCount++;

src/modules/src/outlierFilter.c

@@ -0,0 +1,160 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2018 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/>.
+ *
+ * outlierFilter.c: Outlier rejection filter for the LPS system
+ */
+
+#include <math.h>
+#include "outlierFilter.h"
+#include "stabilizer_types.h"
+#include "log.h"
+
+#define BUCKET_ACCEPTANCE_LEVEL 2
+#define MAX_BUCKET_FILL 10
+#define FILTER_CLOSE_DELAY_COUNT 30
+
+static float acceptanceLevel = 0.0;
+static float errorDistance;
+static int filterCloseDelayCounter = 0;
+static int previousFilterIndex = 0;
+
+typedef struct {
+  float acceptanceLevel;
+  int bucket;
+} filterLevel_t;
+
+#define FILTER_LEVELS 5
+#define FILTER_NONE FILTER_LEVELS
+filterLevel_t filterLevels[FILTER_LEVELS] = {
+  {.acceptanceLevel = 0.4},
+  {.acceptanceLevel = 0.8},
+  {.acceptanceLevel = 1.2},
+  {.acceptanceLevel = 1.6},
+  {.acceptanceLevel = 2.0},
+};
+
+
+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 outlierFilterVaildateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos) {
+  bool sampleIsGood = false;
+
+  if (isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa)) {
+    float errorBaseDistance = sqrtf(powf(jacobian->x, 2) + powf(jacobian->y, 2) + powf(jacobian->z, 2));
+    errorDistance = fabsf(error / errorBaseDistance);
+
+    int filterIndex = updateBuckets(errorDistance);
+
+    if (filterIndex > previousFilterIndex) {
+      filterCloseDelayCounter = FILTER_CLOSE_DELAY_COUNT;
+    } else if (filterIndex < previousFilterIndex) {
+      if (filterCloseDelayCounter > 0) {
+        filterCloseDelayCounter--;
+        filterIndex = previousFilterIndex;
+      }
+    }
+    previousFilterIndex = filterIndex;
+
+    if (filterIndex == FILTER_NONE) {
+      // Lost tracking, open up to let the kalman filter converge
+      acceptanceLevel = 100.0;
+      sampleIsGood = true;
+    } else {
+      acceptanceLevel = filterLevels[filterIndex].acceptanceLevel;
+      if (errorDistance < acceptanceLevel) {
+        sampleIsGood = true;
+      }
+    }
+  }
+
+  return sampleIsGood;
+}
+
+void outlierFilterReset() {
+  // Nothing here
+}
+
+static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(const tdoaMeasurement_t* tdoa) {
+  float anchorDistanceSq = distanceSq(&tdoa->anchorPosition[0], &tdoa->anchorPosition[1]);
+  float distanceDiffSq = sq(tdoa->distanceDiff);
+  return (distanceDiffSq < anchorDistanceSq);
+}
+
+static float distanceSq(const point_t* a, const point_t* b) {
+  return sq(a->x - b->x) + sq(a->y - b->y) + sq(a->z - b->z);
+}
+
+
+static void addToBucket(filterLevel_t* filter) {
+  if (filter->bucket < MAX_BUCKET_FILL) {
+    filter->bucket++;
+  }
+}
+
+static void removeFromBucket(filterLevel_t* filter) {
+  if (filter->bucket > 0) {
+    filter->bucket--;
+  }
+}
+
+static int updateBuckets(float errorDistance) {
+  int filterIndex = FILTER_NONE;
+
+  for (int i = FILTER_LEVELS - 1; i >= 0; i--) {
+    filterLevel_t* filter = &filterLevels[i];
+
+    if (errorDistance < filter->acceptanceLevel) {
+      removeFromBucket(filter);
+    } else {
+      addToBucket(filter);
+    }
+
+    if (filter->bucket < BUCKET_ACCEPTANCE_LEVEL) {
+      filterIndex = i;
+    }
+  }
+
+  return filterIndex;
+}
+
+LOG_GROUP_START(outlierf)
+  LOG_ADD(LOG_INT32, bucket0, &filterLevels[0].bucket)
+  LOG_ADD(LOG_INT32, bucket1, &filterLevels[1].bucket)
+  LOG_ADD(LOG_INT32, bucket2, &filterLevels[2].bucket)
+  LOG_ADD(LOG_INT32, bucket3, &filterLevels[3].bucket)
+  LOG_ADD(LOG_INT32, bucket4, &filterLevels[4].bucket)
+  LOG_ADD(LOG_FLOAT, accLev, &acceptanceLevel)
+  LOG_ADD(LOG_FLOAT, errD, &errorDistance)
+
+LOG_GROUP_STOP(outlierf)

src/utils/src/tdoa/tdoaEngine.c

@@ -50,7 +50,6 @@ The implementation must handle
 
 #include "tdoaEngine.h"
 #include "tdoaStats.h"
-#include "outlierFilter.h"
 #include "clockCorrectionEngine.h"
 #include "physicalConstants.h"
 
@@ -77,7 +76,6 @@ static void enqueueTDOA(const tdoaAnchorContext_t* anchorACtx, const tdoaAnchorC
   };
 
   if (tdoaStorageGetAnchorPosition(anchorACtx, &tdoa.anchorPosition[0]) && tdoaStorageGetAnchorPosition(anchorBCtx, &tdoa.anchorPosition[1])) {
-    if (outlierFilterValidateTdoa(&tdoa)) {
       stats->packetsToEstimator++;
       engineState->sendTdoaToEstimator(&tdoa);
 
@@ -89,7 +87,6 @@ static void enqueueTDOA(const tdoaAnchorContext_t* anchorACtx, const tdoaAnchorC
       if (idB == stats->anchorId && idA == stats->remoteAnchorId) {
         stats->tdoa = -distanceDiff;
       }
-    }
   }
 }