Improved TDoA outlier filter

src/modules/interface/kalman_core/mm_sweep_angles.h

@@ -26,7 +26,7 @@
 #pragma once
 
 #include "kalman_core.h"
-#include "outlierFilter.h"
+#include "outlierFilterLighthouse.h"
 
 // Measurement of sweep angles from a Lighthouse base station
 void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t nowMs, OutlierFilterLhState_t* sweepOutlierFilterState);

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
@@ -26,6 +26,7 @@
 #pragma once
 
 #include "kalman_core.h"
+#include "outlierFilterTdoa.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, OutlierFilterTdoaState_t* outlierFilterState);

src/modules/interface/kalman_core/mm_tdoa_robust.h

@@ -1,4 +1,4 @@
-/** 
+/**
  * This robust M-estimation-based Kalman filter was originally implemented in
  * work by the Dynamic Systems Lab (DSL) at the University of Toronto
  * Institute for Aerospace Studies (UTIAS) and the Vector Institute for
@@ -18,6 +18,7 @@
 #pragma once
 
 #include "kalman_core.h"
+#include "outlierFilterTdoa.h"
 
 // M-estimation based robust Kalman filter update for UWB TDOA measurements
-void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa);
+void kalmanCoreRobustUpdateWithTdoa(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa, OutlierFilterTdoaState_t* outlierFilterState);

src/modules/interface/outlierFilter.h → src/modules/interface/kalman_core/outlierFilterLighthouse.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
@@ -19,23 +19,17 @@
  * 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.h: Outlier rejection filter for the kalman filter
+ * Outlier rejection filter for the kalman filter
  */
 
-#ifndef __OUTLIER_FILTER_H__
-#define __OUTLIER_FILTER_H__
+#pragma once
 
 #include "stabilizer_types.h"
 
-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 openingTimeMs;
     int32_t openingWindowMs;
 } OutlierFilterLhState_t;
-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__
+bool outlierFilterLighthouseValidateSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t nowMs);
+void outlierFilterLighthouseReset(OutlierFilterLhState_t* this, const uint32_t nowMs);

src/modules/interface/kalman_core/outlierFilterTdoa.h

@@ -0,0 +1,36 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * 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
+ * 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/>.
+ *
+ * Outlier rejection filter for the kalman filter
+ */
+
+#pragma once
+
+#include "stabilizer_types.h"
+
+typedef struct {
+    float integrator;
+    uint32_t latestUpdateMs;
+    bool isFilterOpen;
+} OutlierFilterTdoaState_t;
+
+void outlierFilterTdoaReset(OutlierFilterTdoaState_t* this);
+bool outlierFilterTdoaValidateIntegrator(OutlierFilterTdoaState_t* this, const tdoaMeasurement_t* tdoa, const float error, const uint32_t nowMs);

src/modules/interface/kalman_core/outlierFilterTdoaSteps.h

@@ -0,0 +1,33 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * 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
+ * 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/>.
+ *
+ * Outlier rejection filter for the kalman filter
+ */
+
+#pragma once
+
+#include "stabilizer_types.h"
+
+// This functionality is deprecated and will be removed after September 2023. Use outlierFilterTdoaValidateIntegrator() instead.
+// This code is kept here for a while to make it possible to switch back to the "old" outlier filter if the new one is
+// not performing well.
+
+bool outlierFilterTdoaValidateSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);

src/modules/src/Kbuild

@@ -32,7 +32,6 @@ obj-y += axis3fSubSampler.o
 obj-y += log.o
 obj-y += mem.o
 obj-y += msp.o
-obj-y += outlierFilter.o
 obj-y += param_logic.o
 obj-y += param_task.o
 obj-y += peer_localization.o

src/modules/src/Kconfig

@@ -53,6 +53,13 @@ config ESTIMATOR_KALMAN_ENABLE
     help
         Enable the Kalman (EKF) estimator.
 
+config ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK
+    bool "Use 'old' TDoA outlier filter."
+    default n
+    depends on ESTIMATOR_KALMAN_ENABLE
+    help
+        Use the 'old' TDoA outlier filter instead of the default one. Deprecated, will be removed after September 2023.
+
 config ESTIMATOR_UKF_ENABLE
     bool "Enable error-state UKF estimator"
     default n

src/modules/src/estimator_kalman.c

@@ -146,8 +146,10 @@ static Axis3fSubSampler_t gyroSubSampler;
 static Axis3f accLatest;
 static Axis3f gyroLatest;
 
+static OutlierFilterTdoaState_t outlierFilterTdoaState;
 static OutlierFilterLhState_t sweepOutlierFilterState;
 
+
 // Indicates that the internal state is corrupt and should be reset
 bool resetEstimation = false;
 
@@ -296,10 +298,10 @@ static void updateQueuedMeasurements(const uint32_t nowMs, const bool quadIsFlyi
       case MeasurementTypeTDOA:
         if(robustTdoa){
           // robust KF update with TDOA measurements
-          kalmanCoreRobustUpdateWithTDOA(&coreData, &m.data.tdoa);
+          kalmanCoreRobustUpdateWithTdoa(&coreData, &m.data.tdoa, &outlierFilterTdoaState);
         }else{
           // standard KF update
-          kalmanCoreUpdateWithTDOA(&coreData, &m.data.tdoa);
+          kalmanCoreUpdateWithTdoa(&coreData, &m.data.tdoa, nowMs, &outlierFilterTdoaState);
         }
         break;
       case MeasurementTypePosition:
@@ -357,7 +359,8 @@ void estimatorKalmanInit(void)
   axis3fSubSamplerInit(&accSubSampler, GRAVITY_MAGNITUDE);
   axis3fSubSamplerInit(&gyroSubSampler, DEG_TO_RAD);
 
-  outlierFilterReset(&sweepOutlierFilterState, 0);
+  outlierFilterTdoaReset(&outlierFilterTdoaState);
+  outlierFilterLighthouseReset(&sweepOutlierFilterState, 0);
 
   uint32_t nowMs = T2M(xTaskGetTickCount());
   kalmanCoreInit(&coreData, &coreParams, nowMs);

src/modules/src/estimator_ukf.c

@@ -67,7 +67,8 @@
 #include "log.h"
 #include "param.h"
 #include "physicalConstants.h"
-#include "outlierFilter.h"
+#include "outlierFilterTdoa.h"
+#include "outlierFilterLighthouse.h"
 #include "usec_time.h"
 
 #include "statsCnt.h"
@@ -183,6 +184,7 @@ static float innoCheckFlow_y = 0.0f;
 static float distanceTWR = 0.0f;
 
 static float qualGateSweep = 1000.63f;
+static OutlierFilterTdoaState_t outlierFilterTdoaState;
 static OutlierFilterLhState_t sweepOutlierFilterState;
 
 //static bool activateFlowDeck = true;
@@ -202,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;
@@ -350,7 +351,6 @@ static void errorUkfTask(void *parameters)
       accNed[1] = 0.0f;
       accNed[2] = 0.0f;
 
-      tdoaCount = 0;
       nanCounterFilter = 0;
 
       navigationInit();
@@ -496,7 +496,8 @@ void errorEstimatorUkfInit(void)
   baroAccumulatorCount = 0;
   xSemaphoreGive(dataMutex);
 
-  outlierFilterReset(&sweepOutlierFilterState, 0);
+  outlierFilterTdoaReset(&outlierFilterTdoaState);
+  outlierFilterLighthouseReset(&sweepOutlierFilterState, 0);
   //sweepOutlierFilterState.openingWindow=2*sweepOutlierFilterState.openingWindow;
   navigationInit();
 
@@ -788,6 +789,8 @@ static bool updateQueuedMeasurements(const uint32_t tick, Axis3f *gyroAverage)
   bool doneUpdate = false;
   float zeroState[DIM_FILTER] = {0};
 
+  const uint32_t nowMs = T2M(tick);
+
   // Pull the latest sensors values of interest; discard the rest
   measurement_t m;
   while (estimatorDequeue(&m))
@@ -821,81 +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;
-
-            float dx1 = stateNav[0] - m.data.tdoa.anchorPositions[1].x;
-            float dy1 = stateNav[1] - m.data.tdoa.anchorPositions[1].y;
-            float dz1 = stateNav[2] - m.data.tdoa.anchorPositions[1].z;
-
-            float dx0 = stateNav[0] - m.data.tdoa.anchorPositions[0].x;
-            float dy0 = stateNav[1] - m.data.tdoa.anchorPositions[0].y;
-            float dz0 = stateNav[2] - m.data.tdoa.anchorPositions[0].z;
-
-            float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
-            float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
-            vector_t jacobian = {
-                .x = (dx1 / d1 - dx0 / d0),
-                .y = (dy1 / d1 - dy0 / d0),
-                .z = (dz1 / d1 - dz0 / d0),
-            };
-
-            point_t estimatedPosition = {
-                .x = stateNav[0],
-                .y = stateNav[1],
-                .z = stateNav[2],
-            };
+          // 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 (outlierFilterValidateTdoaSteps(&m.data.tdoa, innovation, &jacobian, &estimatedPosition))
-            {
-              //	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:
@@ -1173,7 +1150,7 @@ static bool updateQueuedMeasurements(const uint32_t tick, Axis3f *gyroAverage)
             innovation = m.data.sweepAngle.measuredSweepAngle - observation;
 
             innoCheck = innovation * innovation / Pyy;
-            if (outlierFilterValidateLighthouseSweep(&sweepOutlierFilterState, r, innovation, tick))
+            if (outlierFilterLighthouseValidateSweep(&sweepOutlierFilterState, r, innovation, tick))
             {
               //if(innoCheck<qualGateSweep){
               ukfUpdate(&Pxy[0], &Pyy, innovation);

src/modules/src/kalman_core/Kbuild

@@ -10,3 +10,6 @@ obj-y += mm_tdoa.o
 obj-y += mm_tdoa_robust.o
 obj-y += mm_tof.o
 obj-y += mm_yaw_error.o
+obj-y += outlierFilterTdoa.o
+obj-$(CONFIG_ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK) += outlierFilterTdoaSteps.o
+obj-y += outlierFilterLighthouse.o