Invariant Extended Kalman Filter for Localization in Underwater Caves

Introduction: This is a final research project for NA 568/EECS 568/ROB 530 MOBILE ROBOTICS: METHODS & ALGORITHMS WINTER 2021 at the University of Michigan. The goal of this project is to use Invariant Kalman filtering on data gathered from underwater cave systems for autonomous localization. This program uses the accelerator and gyroscope data for prediction, DVL, depth sensor and magnetometer for correction, and the 6 traffic cones for ground truth.

You can see our final presentation video of our program here

Dataset

The program relied on:

UNDERWATER CAVES SONAR AND VISION DATA SET for data gathering

about file

riekf - Initialization, prediction, correction for riekf algorithm
import_data - Import data from the caves dataset(CSV file)
problem_skeleton - Main function to run our code
plot_ekf_results - Plot cones position
localization_metrics - Contain two metrics to compare our work with visual odometry and SLAM

Getting started

These instructions will get you a copy of the project up and running on your local machine for development purposes.

Prerequisites

Python3, numpy, scipy, os, csv and some knowledge about command line tool (e.g. terminal)

Running the code

In order to configure this project, please follow these steps:

Clone the repository onto your local system.

$ git clone https://github.com/sansaldo/IEKF_AUV_Cave_Navigation.git

The "problem_skeleton.py" file will initiate the program.

$ python3 problem_skeleton.py

💡 Note: in your terminal, please change the directory to the IEKF_AUV_Cave_Navigation directory before you run the program

Result presentation

Below are the 3D and 2D plots of our approach (in blue) versus visual odometry and SLAM algorithm approach after applied the DVL, Depth sensor and magnetometer for correction step. The triangles indicate those predicted positions of each cone (the colors indicate the cone number from 1 through 6)

You can see that our approach largely matches the other two in shape.

😃 If applicable, we will apply ROS package in our future work, which enables online usage of data. Data processing is available on import_data_ros.py

Authors

Samuel Ansaldo - sansaldo
AJ Bull - BullAJ
Alyssa Scheske - ascheske
Shane Storks - shanestorks
Xinyu Ma - xinyuma

Acknowledgments

Professor Maani Ghaffari and the instruction team of EECS 568 in winter 2021, whose code examples helped form the basis of this project.

onurbagoren/IEKF_AUV_Cave_Navigation