Introduction Link to heading

One of the most fundamental problems in robotics is the simultaneous localization and mapping (SLAM) problem.

It is more difficult than localization in that the map is unknown and has to be estimated along the way. It is more difficult than mapping with known poses, since the poses are unknown and have to be estimated along the way.

– S. Thrun

In the following post I would like to discuss the EKF SLAM and highlight the important aspects of its implementation and convergence. Particularly, I would like to demonstrate how the gyroscope bias causes the estimator divergence and how this can be fixed by filter state expansion. To illustrate the concepts I provide the code for visualization and EKF SLAM process.

EKF SLAM allows to solve online SLAM problem: estimation of the posterior over the momentary pose along with the map:

$$p(x_t, m | z_{1:t}, u_{1:t}).$$

As usual, all the code is available here. The exemplary animation built with the code and random robot controller:

Problem formulation Link to heading

Let’s pretend that we have a robot which state is represented by its position $(x, y)$ and orientation $\theta$. These values fully determine robot position and orientation in 2D and therefore named robot pose. The controller of the robot reports its forward speed and angular velocity. In addition to that, it can measure the distances and bearings to the landmarks. The robot can distinguish the measurements to one landmark from another, however, it does not know the positions $(m_x, m_y$) of the landmarks. Our task is to find the pose of the robot and the positions of the landmarks w.r.t. local coordinate frame.

Fortunately, this task can be resolved using Kalman filter under the following assumptions:

  • The robot motion and perception noises are Gaissian distributed.
  • The knowledge of robot dynamic is known, and the relations between measurements $z$, pose $x_t$ and map $m$ are also known and all of them can be expressed in terms of each other. The last statement is very important – it makes the system observable and allows to tie all states together in covariance matrix.

As usual, the Kalman filtering process consints of two repetitive steps:

  1. Prediction.

    The filter mean and covariance are being propogated according to the robot motion model:

    $$\bar{\mu} = f(\mu_{t-1}, u_t)$$ $$\bar{\Sigma} = F_t \Sigma_{t-1} F_t^T + G Q G^T,$$

    where $f$ is the the state transition function, $F$ – is the jacobian of the state transition function $f$ w.r.t. filter state, that, in our case consists of robot pose $(x, y, \theta, \omega_b)$ and $N$ landmark positions $(m_x^j, m_y^j)$, $G$ – is the jacobian of the state transition function w.r.t control $u$. For our robot, $u$ is equal to $(v, \omega)$, where $v$ is the speed, reported by robot odometry and $\omega$ is its angular velocity, reported by robot gyroscope. $Q$ is the diagonal matrix of the process noise,typically it is equal to $diag ( \[\sigma_{v}^2, \sigma_{g}^2\])$.

    The errors in velocity are assumed to be distributed according to $N(0, \sigma_v)$. The errors in angular velocity are distributed according to $N(\mu_g, \sigma_g)$ meaning that the gyroscope measurements are biased. This gyroscope bias leads to continously growing error in orientation and, as consequence, in position. To encount for this constant bias I propose to add new variable into the filter state, this variable represents gyroscope bias $\omega_b$. Now, state transition functions have the following forms:

    $$ f(\mu_{t-1}, u_t) = \begin{bmatrix} x_{t-1} + \Delta{t} v_t \cos{\left(\theta_{t-1} \right)} \newline y_{t-1} + \Delta{t} v_t \sin{\left(\theta_{t-1} \right)} \newline \theta_{t-1} + \Delta{t} (\omega_t - \omega_b)\newline \omega_b \newline m_x^1 \newline m_y^1 \newline \vdots \newline m_x^N \newline m_y^N \end{bmatrix} $$

  2. Correction.

    When new distance and bearing measurements are available we have to calculate the “expected” measurement given the current state and compare this measurements with the obtained one. For distance and bearing measurement to landmark $j$ this expected measurement can be calculated as:

    $$h(\bar{\mathbf{x}}_k) = \hat{z}_t^j = \begin{bmatrix} \sqrt{(m_x^j - \bar{x})^2 + (m_y^j - \bar{y})^2} \newline atan2(m_y^j - \bar{y}, m_x^j - \bar{x}) - \bar{\theta} \end{bmatrix} $$

    Then the observation matrix $H$ should be evaluated as Jacobian of $h(\bar{\mathbf{x}}_k)$ w.r.t. state and standard correction, or update equations for EKF can be employed.

Now, as robot moves the uncertainty of our pose grows until we get some measurements. The measurements, however, improve not only the robot pose estimate, but also the uncertainty of landmarks previously seen by the robot. This dependence between poses and landmark positions is reflected in off-diagonal elements of the covariance matrix $\Sigma$.

Implementation Link to heading

To see EKF SLAM in action I wrote its implementation in Python. The implementation consists of the following files:

  • robot.py – the robot simulator. The robot is able to move along the map and measure the distances and bearings to the landmarks which are within its sensing range (30 meters).
  • ekf_slam.py – the implementation of EKF SLAM algorithm, as presented in [1] with one extra state: gyroscope bias.
  • animation.py – the animation code to visualize the robot motion, its measurements and EKF SLAM state estimates.

Illustration Link to heading

Let`s look at the EKF SLAM performance when robot sensors have the following error characteristics:

  • Velocity measurement noise $\sigma_v = 0.1 \frac{m}{s^2}$, meaning that 99.7% of velocity measurements, or controls, lie within $v_{true} \pm 0.3 m$ range.
  • Gyroscope measurement noise $\sigma_g = 0.5 \frac{deg}{s}$. The gyroscope bias is constant during the robot operations and randomly generated on a robot startup.
  • Distance measurement noise $\sigma_{d} = 1.0 m$.
  • Bearing measurement noise $\sigma_{b} = 5 deg$.

The measurement accuracy is quite high: we know, that almost all distance measurements are accurate up to 3 meters, and bearing measurements up to 15 degrees (see 3 sigma rule).

At first, let`s look at EKF SLAM performance when gyroscope bias is not included in its state and therefore not estimated. In the following animation the robot moves along the rectangular path while measuring the distances and bearings. The true landmark positions are marked with black crosses, estimated positions - with magenta markers, the landmarks position uncertainty is shown by confidence ellipses, which diameters are proportional to its uncertainty. The measurements are depicted with dashed lines and the points on the map indicate the recent measurements to each landmark. The uncertainty in robot position is marked with magenta confidence ellipse around robot position estimate.

When the robot observes the landmarks its own pose is being corrected. The interesting thing happens around 1100 frame: the robot detects the landmark that he saw in the beginning of mapping process. This knowledge, thanks to the correlation from the covariance matrix, allows to immidiately correct not only the pose of the robot, but also the position estimates of all other landmarks. Take a look at the covariance matrix on the right part of the plot: after some time all the states in the covariance matrix become correlated, meaning that all the states can be observed.

Now, assume, that the robot equipped with much worse sensors, which noises levels 3 times higher than before, particularly $\sigma_{d} = 3.0 m$ and $\sigma_{b} = 15 deg$. The results are much worse: the robot is simply unable to correctly estimate its own position and the positions of the landmarks, because the uncertainty both of them is too high (Note: of course, we are able to “cheat” there and set the measurement noise of measurements lower than it tend to be in real life, but as long as it is only an example, I leave everything as iss).

Now, let`s add the gyroscope bias into the filter state and allow the filter to estimate it, applying the equations, mentioned earlier. The results are even better than in the first case, when robot measurements had low noises. The filter was able to estimate gyroscope bias and, consequently, exclude this systematic error. The robot pose now drifts much slower than in two first cases, identifying the importance of correct system modelling and taking into account significant error sources. This result is especially important for SLAM with unknown landmark correspondences: the measurements clusters can be separated only if robot path drift is slow compared to the error growth rate.

To run the animations and the EKF SLAM process locally simply execute run.py script.

Afterword Link to heading

EKF SLAM is one of the earliest algorithm that allowed to solve SLAM problem quite efficiently. The simplicity of its implementation and intuitive transparency allows to grasp in the main concepts of SLAM. The limitations of EKF SLAM is well known and discussed in the literature. Therefore, in the next articles I plan to cover other SLAM algorithms that are intended for online and offline data processing.