Ukf

docs/functional-areas/sensor-to-control/state_estimators.md

@@ -9,6 +9,7 @@ A state estimator turns sensor signals into an estimate of the state that the cr
 
 * [Complementary filter](#complementary-filter)
 * [Extended Kalman filter](#extended-kalman-filter)
+- [Unscented  Kalman filter](#unscented-kalman-filter) (Experimental)
 * [References](#references)
 
 ## Complementary filter
@@ -19,8 +20,6 @@ The complementary filter is consider a very lightweight and efficient filter whi
 
 To checkout the implementation details, please checkout the firmware in `estimator_complementary.c` and `sensfusion6.c`. The complementary filter is set as the default state estimator on the Crazyflie firmware, unless a deck is mounted that requires the Extended Kalman filter.
 
-
-
 ## Extended Kalman filter
 
 ![extended kalman filter](/docs/images/extended_kalman_filter.png){:width="500"}
@@ -52,10 +51,33 @@ This illustration explains how the height from the VL53L1x sensor and flow from
 
 ![flowdeck velocity](/docs/images/flowdeck_velocity.png){:width="500"}
 
+## Unscented Kalman Filter
+**NOTE**
+*This is still in the experimental phase, so it would need to be enabled in [kbuild](/docs/development/kbuild.md) in 'Controllers and Estimators' and Enable error-state UKF estimator'*
+
+The error-state unscented Kalman Filter (error-state UKF) is another navigational algorithm for the Crazyflie whose objective is improving navigation quality when relying on the Loco-Positioning system. It combines a strapdown navigation algorithm [4] performing a time integration of the accelerometer and gyrosope measurements with an unscented Kalman-Filter estimating the error-state between the strapdown navigation solution and the true position by means of the deck measurements. Just like the EKF described above the UKF is a recursive filter. However, compared to an EKF approach, the UKF does not rely on linearizing the nonlinear measurement equations but instead employs deterministically chosen sigma-points to approximate the mean and the covariance of the error-state. This enhances the approximation accuracy, which turns out to be particularly advantageous when using the Loco-Positioning-System for absolute positioning. The algorithm is described in more detail in reference [5]. Further details on deriving error-state predictions models as well as a more detailed derivation of the UKF approach can be found in [6] and [7].
+
+In its current implementation, the algorithm is able to fuse the following measurements:
+
+* Loco-Positioning Time difference of arrival (TdoA)
+* Flow-Deck v2
+
+
+The time-of-flight measurements of the flow-deck are also subject to a simple outlier rejection scheme, allowing the Crazyflie to pass over ground obstacles without causing height jumps. The default parameterization consideres a Crazyflie 2.1 quadcpopter fusing Loco-Positioning and/or Flow-Deck measurements. When solely fusing Flow-Deck measurments, the quality gate for time-of-flight measurments (Parameter  "ukf.qualityGateTof") has to be increased.
+
+
 
 ## References
 [1] Mueller, Mark W., Michael Hamer, and Raffaello D'Andrea. "Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation." 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015.
 
 [2] Mueller, Mark W., Markus Hehn, and Raffaello D’Andrea. "Covariance correction step for kalman filtering with an attitude." Journal of Guidance, Control, and Dynamics 40.9 (2017): 2301-2306.
 
 [3] M. Greiff, Modelling and Control of the Crazyflie Quadrotor for Aggressive and Autonomous Flight by Optical Flow Driven State Estimation, Master’s thesis, Lund University, 2017
+
+[4] D.H. Titterton, J.L. Weston, “Strapdown Inertial Navigation Technology - Second Edition”, Institution of Electrical Engineers, 2004
+
+[5] Kefferpütz, Klaus, McGuire, Kimberly. "Error-State Unscented Kalman-Filter for UAV Indoor Navigation", 25th International Conference on Information Fusion (FUSION), Linköping, Schweden, 2022
+
+[6] J. Sola, “Quaternion kinematics for the error-state Kalman filter”, arXiv:1711.02508, November 2017
+
+[7] S.J. Julier, J.K. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear Systems“, Signal Processing, Sensor Fusion, and Target Recognition VI, Aerosense 97, Orlando, USA, 1997

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/deck/drivers/src/Kconfig

@@ -290,6 +290,15 @@ config DECK_LOCO
        LOCO deck alternative IRQ and RESET pins(IO_2, IO_3) instead of
        default (RX1, TX1), leaving UART1 free for use.
 
+  config LOCODECK_ALT_PIN_RESET
+  bool "Set pin GPIO_IO4 for reset on the LOCO deck"
+  default name
+  depends on DECK_LOCODECK_USE_ALT_PINS
+  help
+      Only works when LOCODECK_USE_ALT_PINS is set. For instance useful with
+      Loco + Lighhouse + Flow decks where IO_3 collides with the Flow deck.
+      Requires modified Loco deck with connection to GPIO_IO4.
+
   config DECK_LOCO_NR_OF_ANCHORS
   int "The number of anchors in use"
   default 8

src/deck/drivers/src/locodeck.c

@@ -65,10 +65,10 @@
 #ifdef CONFIG_DECK_LOCODECK_USE_ALT_PINS
   #define GPIO_PIN_IRQ 	  DECK_GPIO_IO2
 
-  #ifndef LOCODECK_ALT_PIN_RESET
+  #ifndef CONFIG_LOCODECK_ALT_PIN_RESET
   #define GPIO_PIN_RESET 	DECK_GPIO_IO3
   #else
-  #define GPIO_PIN_RESET 	LOCODECK_ALT_PIN_RESET
+  #define GPIO_PIN_RESET 	DECK_GPIO_IO4
   #endif
 
   #define EXTI_PortSource EXTI_PortSourceGPIOB
@@ -570,7 +570,11 @@ static const DeckDriver dwm1000_deck = {
   .name = "bcDWM1000",
 
 #ifdef CONFIG_DECK_LOCODECK_USE_ALT_PINS
+  #ifndef CONFIG_LOCODECK_ALT_PIN_RESET
   .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_2 | DECK_USING_IO_3,
+  #else
+  .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_2 | DECK_USING_IO_4,
+  #endif
 #else
    // (PC10/PC11 is UART1 TX/RX)
   .usedGpio = DECK_USING_IO_1 | DECK_USING_PC10 | DECK_USING_PC11,

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/interface/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/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) += 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

src/modules/src/estimator.c

@@ -9,6 +9,7 @@
 #include "estimator.h"
 #include "estimator_complementary.h"
 #include "estimator_kalman.h"
+#include "estimator_ukf.h"
 #include "log.h"
 #include "statsCnt.h"
 #include "eventtrigger.h"
@@ -78,6 +79,15 @@ static EstimatorFcns estimatorFunctions[] = {
         .name = "Kalman",
     },
 #endif
+#ifdef CONFIG_ESTIMATOR_UKF_ENABLE
+    {
+	    .init = errorEstimatorUkfInit,
+	    .deinit = NOT_IMPLEMENTED,
+	    .test = errorEstimatorUkfTest,
+	    .update = errorEstimatorUkf,
+	    .name = "Error State UKF",
+	},
+#endif
 #ifdef CONFIG_ESTIMATOR_OOT
     {
         .init = estimatorOutOfTreeInit,
@@ -106,7 +116,9 @@ void stateEstimatorSwitchTo(StateEstimatorType estimator) {
   }
 
   #if defined(CONFIG_ESTIMATOR_KALMAN)
-    #define ESTIMATOR kalmanEstimator
+    #define ESTIMATOR KalmanEstimator
+  #elif defined(CONFIG_UKF_KALMAN)
+    #define ESTIMATOR ukfEstimator
   #elif defined(CONFIG_ESTIMATOR_COMPLEMENTARY)
     #define ESTIMATOR complementaryEstimator
   #else