Merge pull request #1237 from bitcraze/krichardsson/issue-1158

Improved TDoA outlier filter
bitcraze · Mar 1, 2023 · 6490515 · 6490515
2 parents f6eb3ef + 55aab65
commit 6490515
Show file tree

Hide file tree

Showing 29 changed files with 488 additions and 362 deletions.
diff --git a/src/deck/drivers/src/flowdeck_v1v2.c b/src/deck/drivers/src/flowdeck_v1v2.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2021, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2016, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2016, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/deck/drivers/src/multiranger.c b/src/deck/drivers/src/multiranger.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2021, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/deck/drivers/src/oa.c b/src/deck/drivers/src/oa.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2021, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/drivers/src/pmw3901.c b/src/drivers/src/pmw3901.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2017, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/modules/interface/app.h b/src/modules/interface/app.h
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2019, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
@@ -27,10 +25,10 @@
 
 /**
  * App Inintialization
- * 
+ *
  * Called during firmware initialization. A default weak function is implemented
  * that creates a task and calls appMain() when the system is started.
- * 
+ *
  * Implementation can be overwritten.
  */
 void appInit();
@@ -39,4 +37,4 @@ void appInit();
  * app main function, called when the Crazyflie has started from within a task created
  * by appInit().
  */
-void appMain();
+void appMain();
diff --git a/src/modules/interface/app_channel.h b/src/modules/interface/app_channel.h
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2020, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/modules/interface/kalman_core/mm_sweep_angles.h b/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);
diff --git a/src/modules/interface/kalman_core/mm_tdoa.h b/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);
diff --git a/src/modules/interface/kalman_core/mm_tdoa_robust.h b/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);
diff --git a/src/modules/interface/outlierFilter.h → ...ace/kalman_core/outlierFilterLighthouse.h b/src/modules/interface/outlierFilter.h → ...ace/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);
diff --git a/src/modules/interface/kalman_core/outlierFilterTdoa.h b/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);
diff --git a/src/modules/interface/kalman_core/outlierFilterTdoaSteps.h b/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);
diff --git a/src/modules/src/Kbuild b/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

diff --git a/src/modules/src/Kconfig b/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

diff --git a/src/modules/src/app_channel.c b/src/modules/src/app_channel.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2020, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/modules/src/app_handler.c b/src/modules/src/app_handler.c
@@ -5,8 +5,6 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * LPS node firmware.
- *
  * Copyright 2019, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify

diff --git a/src/modules/src/estimator_kalman.c b/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);