Add error-state unscented Kalman filter

src/config/config.h

@@ -92,6 +92,7 @@
 #define UART1_TEST_TASK_PRI     1
 #define UART2_TEST_TASK_PRI     1
 #define KALMAN_TASK_PRI         2
+#define ERROR_UKF_TASK_PRI      2
 #define LEDSEQCMD_TASK_PRI      1
 #define FLAPPERDECK_TASK_PRI    2
 #define SYSLINK_TASK_PRI        3
@@ -145,6 +146,7 @@
 #define UART1_TEST_TASK_NAME    "UART1TEST"
 #define UART2_TEST_TASK_NAME    "UART2TEST"
 #define KALMAN_TASK_NAME        "KALMAN"
+#define ERROR_UKF_TASK_NAME     "ERROR_UKF"
 #define ACTIVE_MARKER_TASK_NAME "ACTIVEMARKER-DECK"
 #define AI_DECK_GAP_TASK_NAME   "AI-DECK-GAP"
 #define AIDECK_ESP_TX_TASK_NAME "AI-DECK ESP TX"
@@ -203,6 +205,7 @@
 #define OA_DECK_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
 #define KALMAN_TASK_STACKSIZE         (3 * configMINIMAL_STACK_SIZE)
 #define FLAPPERDECK_TASK_STACKSIZE    (2 * configMINIMAL_STACK_SIZE)
+#define ERROR_UKF_TASK_STACKSIZE      (4 * configMINIMAL_STACK_SIZE)
 
 //The radio channel. From 0 to 125
 #define RADIO_CHANNEL 80

src/modules/interface/error_estimator_ukf.h

@@ -0,0 +1,73 @@
+/**
+ *                                             __
+ *    __  __ ____      ____  __  __ __________/ /_  __   _____________
+ *   / /_/ / ___/____ / __ `/ / / /´__  / ___/ __ \/ /  / / ___/ __  /
+ *  / __  (__  )/___// /_/ / /_/ / /_/ (__  ) /_/ / /__/ / /  / /_/ /
+ * /_/ /_/____/      \__,_/_____/\__  /____/\____/______/_/   \__  /
+ *                              /____/                       /____/
+ * Crazyflie Project
+ *
+ * Copyright (C) 2019-2022 University of Applied Sciences Augsburg
+ *
+ * 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/>.
+ * 
+ * ============================================================================
+ * Error-State Unscented Kalman Filter
+ * ============================================================================
+ * 
+ * The error-state unscented Kalman filter implemented in this file is based
+ * on the paper:
+ * 
+ * BIBTEX ENTRIES:
+      @INPROCEEDINGS{9841385,
+      author={Kefferpütz, Klaus and McGuire, Kimberly},
+      booktitle={2022 25th International Conference on Information Fusion (FUSION)}, 
+      title={Error-State Unscented Kalman-Filter for UAV Indoor Navigation}, 
+      year={2022},
+      volume={},
+      number={},
+      pages={01-08},
+      doi={10.23919/FUSION49751.2022.9841385}}
+ * 
+ * ============================================================================
+ * 
+ * Authored by Klaus Kefferpütz, December 2022
+ *
+ * ============================================================================
+ *
+ * MAJOR CHANGELOG:
+ * 2021.09.03 Initial Commit
+ *
+ */
+
+#ifndef __ERROR_ESTIMATOR_UKF_H__
+#define __ERROR_ESTIMATOR_UKF_H__
+
+#include <stdint.h>
+#include "stabilizer_types.h"
+
+void errorEstimatorUkfInit(void);
+bool errorEstimatorUkfTest(void);
+void errorEstimatorUkf(state_t *state, const uint32_t tick);
+
+void errorEstimatorUkfTaskInit();
+bool errorEstimatorUkfTaskTest();
+
+void errorEstimatorUkfGetEstimatedPos(point_t* pos);
+
+/**
+ * Copies 9 floats representing the current state rotation matrix
+ */
+void errorEstimatorUkfGetEstimatedRot(float * rotationMatrix);
+
+#endif // __ERROR_ESTIMATOR_UKF_H__

src/modules/interface/estimator.h

@@ -34,6 +34,9 @@ typedef enum {
 #ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE
   kalmanEstimator,
 #endif
+#ifdef CONFIG_ESTIMATOR_UKF_ENABLE
+  ukfEstimator,
+#endif
 #ifdef CONFIG_ESTIMATOR_OOT
   OutOfTreeEstimator,
 #endif

src/modules/src/Kbuild

@@ -22,6 +22,7 @@ obj-y += crtpservice.o
 obj-y += esp_deck_flasher.o
 obj-y += estimator_complementary.o
 obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += estimator_kalman.o
+obj-$(CONFIG_ESTIMATOR_UKF_ENABLE) += error_estimator_ukf.o
 obj-y += estimator.o
 obj-y += eventtrigger.o
 obj-y += extrx.o

src/modules/src/Kconfig

@@ -49,6 +49,12 @@ config ESTIMATOR_KALMAN_ENABLE
     help
         Enable the Kalman (EKF) estimator.
 
+config ESTIMATOR_UKF_ENABLE
+    bool "Enable error-state UKF estimator"
+    default n
+    help
+        Enable the (error-state unscented) Kalman filter (UKF) estimator
+
 choice
     prompt "Default estimator"
     default CONFIG_ESTIMATOR_ANY
@@ -64,6 +70,12 @@ config ESTIMATOR_KALMAN
     help
         Use the (extended) Kalman filter (EKF) estimator as default
 
+config ESTIMATOR_UKF
+    bool "Error-state UKF estimator"
+    depends on ESTIMATOR_UKF_ENABLE
+    help
+        Use the (error-state unscented) Kalman filter (UKF) estimator as default
+
 config ESTIMATOR_COMPLEMENTARY
     bool "Complementary estimator"
     help