EKF in MS Robotics Studio 2010

Introduction

Problem Statement

The problem being addressed is that of autonomously localizing a robot in an unknown environment in which its location is also unknown. The robots job is to use build a map of the environment without a priori knowledge of that map and at the same time keep track of their location. Both tasks, localization and mapping, are hard problems themselves. Since both of these tasks depend on each other, there needs to be a way to go back and forth between solving for them and estimating at time intervals. This problem is a central component in creating fully autonomous robots.

SLAM

A popular method of solving this problem is with SLAM (Simultaneous Mapping and Localization). SLAM combines both processes with a loop. One of the key strengths of a SLAM algorithm is that it is guaranteed to converge on a solution, though the time until convergence is unknown. Two common implementations of SLAM are FastSLAM and EKF-SLAM. Both are probabilistic methods that involve the use of a filter. FastSLAM uses a particle filter and EKF-SLAM uses a Kalman filter, specifically an Extended Kalman filter. While SLAM has been “solved” theoretically, its current implementations are too slow for all practical uses. This is the major problem in this field.

Extended Kalman Filter

An EKF (Extended Kalman Filter) is a Kalman Filter that can run with non-linear equations in addition to linear equations. This is where the "Extended" comes in to the name. SLAM is considered a non-linear problem, and therefore an EKF is needed. The EKF does not actually deal with the non-linear equations, it instead "linearizes" them through the use of Jacobians.
Like a regeular Kalman Filter, EKF has two main steps: Predict and Update. During the Predict stage, the next state vector x is predicted based on current state vector, control input and F jacobian. The estimate copvariance is also predicted. During the Update stage, the Kalman gain is calculated, which is used to update the state vector predicted in the previous step and the covariance is updated base on the H jacobian. The F jacobian is the partial derivatives of the function used to calculate the next state from the current state. The H jacobian is the partial derivatives of the function used to predict the measurement based on the predicted state [1]. For specific equations see here.

EKF-SLAM

EKF-SLAM is an Extended Kalman Filter implemented specifically for SLAM. The state vector x consists of the state of the robot (x,y,theta) as well as the (x,y) location of all landmarks. The size of this vector is 3+2*n where n i the number of landmarks. The vector would look something like this [x,y,theta,L1x,L1y,L2x,L2y,...,Lnx,Lny]^T. The control input u is input that x needs to get to the new x estimate, therefore it has the same size of x.

Environment and Architecture

All code was developed in Microsoft Visual Studio 2008 on a Lenovo laptop with Windows Vista Business.

MRDS Service

To run the Simulation, I used MRDS (Microsoft Robotics Developers Studio) [2], which is built based on the .NET framework and utilizes the CCR (Concurrency and Coordination Runtime) [3] and DSS (Decentralized Software Services) [4] to help the user with concurrency and make developing services that interact with each other, much easier. Anything that runs in MRDS uses a service. Simulation Environment, Differential Drivers, and Laser Sensors are all examples of services. I created a service called SimulatedSLAM that interacts with the SimulatedGenericDiffereantialDrive and SimulatedEngine services to run a simulation.

EKF-SLAM Algorithm

The main Algorithm was written in C# and based off of the KFilter C++ Extended Kalman Filter Library [5]. It consists of three main parts: the IKalmanFilter interface which defines the essential methods to a Kalman Filter, the AbstractEKF which is very similar to the ExtendedKalmanFilter class in KFilter, and the EKFSLAM class which implements the abstract methods in the AbstractEKF class to make the algorithm specific to the SLAM problem.

Assumptions

In order to make the simulation easier and get to the core issues of implementing an EKF-SLAM algorithm, I made several assumptions. They are listed below.

• The random moving my robot does has two seperate phases. Move Forward and Turn. Every second, I either move forward for a random distance or turn a random number of degrees. During the forward phase (this is always the first phase to go), I move forward, update the location based on odometry, and perform the Time Update Step of the EKF. During the degrees phase, I rotate, update the direction the robot is facing, and perform the Observation Update Step by observing the landmarks in the environment. In practice, the robot would most likely continuously move, but for the sake of keeping the simulation easy, I chose the move in steps.
• Instead of adding landmarks each iteration, I kept the number of landmarks static throughout the run.
• I also did not do any data association. I felt this would really complicate the problem and push me away from my focus of the Kalman Filter.

Algorithm

Code

• MRDS Service
  • SimulatedSLAM
    • This is the main class for the Service and is essentially the driver for my program
    • Performs Odometry and uses Normal Distribution to create noice in the odometry data
    • Performs Landmark indentifying and uses Normal Distribution to create noise in the data
  • SimulatedSLAMState
  • SimulatedSLAMTypes
• Core Algorithm
  • IKalmanFilter, Interface
    • Initialize(Vector x, Matrix p)
    • TimeUpdateStep(Vector u)
    • ObservationUpdateStep(Vector x)
  • AbstractEKF, Abstract Class, Implements IKalmanFilter
    • Implements functionality that is common to all Extended Kalman Filters
    • Leaves the user of the class to implement methods that create/update the A, H, Q, R, V, W matrices, and the x and z vectors, for each iteration
  • EKFSLAM, Class, Extends AbstractEKF
    • Provides Specific functionality for the SLAM algorithm
• Linear Algebra Objects
  • Matrix, Class
    • m,n dimension
    • GetInverse()
    • GetTranspose()
    • Overrloaded operators include Access([]), Addition(+) and Multiplication(*)
  • Vector, Class, Extends Matrix
    • A vector is a Matrix with dimension (n,1)
• Statistical Distributions
  • IDistribution, Interface
    • GetRandomNumber()
  • NormalDistribution, Class, Implements IDistribution
    • GetRandomNumber() returns a random Gaussian number
  • UniformDistribution, Class, Implements IDistribution
    • GetRandomNumber() reutrns a random Uniformly-distributed number

To download code, see here.

Experiments

Media

To view media (videos and screen shots) see here.

References

The references below are to papers. All web references are cited and have a link where the citation is.

• A. Martinellie and R. Siegwart. "Estimating the Odometry Error of a Mobile Robot during Navigation." Autonomous Systems Lab Swiss Federal Insitute of Technology Lausanne 2003.
• G. Welch and G. Bishop. "An Introduction to the Kalman Filter." UNC Chapel Hill, TR 95-041 July 2006.
• H. Durrant-Wyte and T. Bailey. "Simultaneous Localization and Mapping: Part 1." IEEE Robotics & Automation Magazine June 2006 pp. 99-108
• S. Riisgaard and M. Rufus Blas. "SLAM for Dummies." MIT 2004. [6]
• T. Bailey and H. Durrant-Wyte. "Simultaneous Localization and Mapping (SLAM): Part 2." IEEE Robotics & Automation Magazine September 2006 pp. 108-117

Technical Definitions and Acronyms

• MRDS - Microsoft Robotics Developers Studio
• CCR - Concurrency and Coordinatio Runtime
• DSS - Decentralized Software Services
• SLAM - Simultaneous Localization and Mapping
• EKF - Extended Kalman Filter

Hints for Starting and Using MRDS

One of the main hurdles I encountered when doing this project, was the learning curve of MRDS. Here are some hints for anyone interested in programming in the MRDS environment.

• Just because you are familiar with the .NET environment, does not mean MRDS will come easy to you. I had been programming .NET Web Application for the past 6 months when I started the project and I thought it would be very similiar. While a lot of the code and basic libraries that are the same, there are many practices, such as concurrency, that is not used in ASP.NET. With this said, give yourself plenty of time to learn the basics of the MRDS architecture (CCR, DSS, Servies, Simulation Environment, etc.)
• I did not find many great tutorials out there for MRDS. Most of the tutorials on the Microsoft MSDN Library where not helpful when it came to driving a robot through a simulated environment. The tutorials covered Services in a general sense and covered very basic Simulation practices. I was not able to find a tutorial on there site that gave code for driving a vehichle. I luckily found an example at ProMRDS that gave some very helpful code. You can also look at my code, which has a fairly straightforward implementation of randomly moving the vehicle based on time ticks and keeping track of the location based on odometry.