Goal is to obtain accurate, drift-less estimations of 3D position, velocity and rotation of an autonomous drone, similarly designed as the CrayzFlie drone in the following image:

Drone contains IMU, optical flow (OF) and time-of-flight (ToF) sensors. Due to the nature of IMU noise and bias, trajectories drift without fusion with complementary sensors.

Solution should be real-time and usable on time-constrained Arduino and ESP32 MCUs

• c++17 compiler
• Eigen (header-only library included in external/ for ease of compilation)

In order to reduce the drift, OF and ToF measuements are fused with the Error-state Kalman Filter. This is a variation of EKF, which propagates nominal states and error-states, which are then updated when OF and ToF measurements arrive. Error-state formulation decreases linearization errors in Jacobian calculation, and allows seamless representation of rotation propagation and update as errors in the $so(3)$ tangent space. Filter implementation is based on the following papares, where you can read more:

• Covariance Correction Step for Kalman Filtering with an Attitude
• Quaternion kinematics for the error-state Kalman filter

ESKF is implemented in ESKF.cpp. Simple estimator reads IMU, OF and ToF measurements from CSV data for testing. To use the repository as a library, simply change CMakeLists to compile a shared or a static library.

• Measurements of simple testing trajectories are provided in serial_data.csv.

Compile with:

mkdir build && cd build
cmake ..
make -j8

Simply provide paths to csv file containing measurements and output file for storing results, e.g.:

./estimator measurements/serial_data.csv results.txt

This will create a results.txt file with positions, velocities and rotations (as quaternions) at each timestamp.

Filter completely eliminates drift due to IMU noise and bias. Left -- IMU dead reckoning. Right -- ESKF.

Estimated positions:

Repository is compatible with Arduino and ESP32-based MCUs. Only requirement is the Eigen library, which can be easily deployed as in:

• ESP32 Eigen
• Arduino Eigen

• Adding cross-correlation elements to process noise
• Adding rotation variance to process noise
• Adding bias error-states