Intro to the Extended Kalman Filter

Posted on by

I started a PhD in computer science at CU Boulder four months ago. This is mainly because three years ago I made a wager. Since then I’ve become completely fascinated with robotics and have decided to pursue robotics research as a career.

Colin with his new sonar

Colin, my robot

I’m currently working at the ARPG at CU Boulder. As part of that work I wrote an implementation of SLAM using the Extended Kalman Filter (EKF). The EKF is the oldest probabilistic SLAM method. It’s a great introduction to SLAM techniques. However, it’s not very good by modern standards; it’s computationally expensive and it requires hand tuning several parameters to achieve passably accurate operation. But it is a good step in learning SLAM techniques, and I will likely implement an EKF on Colin at some point.

Tutorial Structure

Over the next several posts I’ll take you through my implementation of EKF SLAM in several steps. At the first step the robot will have a lot of prior knowledge about its environment and sensor readings. We’ll progressively take away pieces of the robot’s prior knowledge, making the task harder and harder as we go. In our last step the robot will have no prior knowledge. Our solution to this last step will be a fully-fledged SLAM program.

See the following for links to other tutorials in this series:

Dataset

I will use the UTIAS robot localization dataset for my tutorials. I’ll go into more detail on how to use and interpret the UTIAS data in my next post. The important thing to understand is that the UTIAS data abstracts away large parts of the SLAM problem such as controls and computer vision. This means we can just focus on programming and tuning our Extended Kalman Filters when using the UTIAS data. However, it also means we can’t simply upload our EKF programs to Colin. We would also need to write supporting programs that could supply data to the EKF in a form it can use.

Reference Material

Most of the math I’ll present in the next several posts came from Probabilistic Robotics, by Thrun, Fox, and Burgard. It’s a great book for anyone who wants to learn the theory behind fundamental robotics problems. It’s also not difficult to find a free pdf copy of an earlier edition if you don’t want to pay $85 for the current edition. But be warned that there are some math errors in the free pdf edition, so you may need to re-derive some things if you use it.

Probabilistic Robotics has a great section on Extended Kalman Filters and I’d recommend reading it thoroughly. However, the motion model it presents can’t be used with the UTIAS dataset so I’ll be using a different one. I’ll highlight where I deviate from the method presented in Probabilistic Robotics so as not to confuse those of you who are using it as a reference.

Programming Note

Lastly, the code I’m presenting is written in MATLAB. For those who don’t have access to MATLAB I would suggest using Python and NumPy. NumPy has all of the required functions to complete these tutorials. The only thing to watch out for is that Python uses zero-based indexing while MATLAB uses one-based indexing. If you don’t keep this in mind it could cause you some serious headaches.