An Ensemble Kalman Filter for Feature-Based SLAM with Unknown Associations Fabian Sigges, Christoph Rauterberg and Marcus Baum Uwe D. Hanebeck Institute of Computer Science Intelligent Sensor-Actuator-Systems Laboratory (ISAS) University of Goettingen, Germany Institute for Anthropomatics and Robotics Email: ffabian.sigges, [email protected] Karlsruhe Institute of Technology (KIT), Germany [email protected] Email: [email protected] Abstract—In this paper, we present a new approach for solving step of the filter, the posterior is then obtained using the the SLAM problem using the Ensemble Kalman Filter (EnKF). ensemble mean and an estimate of the sample covariance from In contrast to other Kalman filter based approaches, the EnKF the ensemble members. uses a small set of ensemble members to represent the state, thus circumventing the computation of the large covariance matrix Although being established in the geoscientific community, traditionally used with Kalman filters, making this approach a the EnKF has only recently received attention from the track- viable application in high-dimensional state spaces. Our approach ing community [3], [4]. In the case of SLAM, the EnKF is adapts techniques from the geoscientific community such as mostly just applied to specific parts of the whole problem. localization to the SLAM problem domain as well as using For example, in [5] the EnKF is used to generate a suitable the Optimal Subpattern Assignment (OSPA) metric for data association. We then compare the results of our algorithm with an proposal distribution for FastSLAM or in [6] the EnKF realizes extended Kalman filter (EKF) and FastSLAM, showing that our the merging operation on two approximate transformations. approach yields a more robust, accurate, and computationally In the following, we derive an algorithm to solve the less demanding solution than the EKF and similar results to SLAM problem based on the ensemble Kalman filter. We FastSLAM. implement and discuss several techniques for improving the performance of the EnKF that are common in the geoscientific I. INTRODUCTION community. Furthermore, we show our use of the Optimal Simultaneous localization and mapping (SLAM) describes Subpattern Assignment (OSPA) metric [7] to achieve reliable the problem of creating a map of an unknown environment data association. We then present our results in comparison to while simultaneously tracking a robot’s position within the an EKF and FastSLAM. map. Tasks such as navigation in autonomous driving need In the remainder of this paper, we will first introduce the a detailed map of the surroundings to provide an accurate EnKF idea as well as derive the nature and challenges of localization. However, detailed knowledge about the current the SLAM problem in Section 2. We will then discuss our position is needed to build a map with newly discovered algorithm in detail in Section 3. The following section shows features, which makes the nature of the SLAM problem a the results from our simulations. Section 5 will give our “chicken-and-egg-problem”. conclusions and an outlook on future work. Combining the state of the robot and the map into a single state makes this application suitable for state estima- II. THE ENSEMBLE KALMAN FILTER tion solutions. Traditional approaches use techniques such as the extended Kalman filter (EKF) [1] or particle filters [2]. The ensemble Kalman filter can be seen as a Monte Carlo However, as the discovered map grows, so does the number implementation of the traditional Kalman filter or even as of observed landmarks and therefore the dimension of the state a Kalman filter version of a particle filter: As the Kalman vector. As approaches such as the EKF need to keep track of filter represents the state estimation at time step k by its an n × n covariance matrix, they fall short to the curse of first moments, the mean x^k and covariance matrix Σk, the dimensionality and memory constraints. Kalman filter is not applicable to high-dimensional states as The ensemble Kalman filter (EnKF) however, developed the computation and storage of a n × n covariance matrix in the geosciences, has been mostly unused in the signal becomes computationally very hard. processing community: Typical applications, such as weather We begin with a closer look at the Kalman filter for linear prediction, are extremely high-dimensional state estimation systems. problems, which produce states with orders of several millions The Kalman filter algorithm consists of two steps: and are therefore not tangible using traditional state estimation. 1) In the prediction-step, we use the process model equa- Instead of representing the state using mean and covariance tion to predict the next state matrix, the EnKF uses ensemble members to represent the state - not unlike the particles of a particle filter. In the update xek+1 = Fxk + wk with wk ∼ Nn(0; Q) ; (1) where xk denotes the previous state, xek+1 the prediction, where F the state transition model and wk the state noise. We z(i) = h(x(i) ) ; and (14) assume wk to be normally distributed with zero mean k+1 ek+1 N and covariance matrix Q. 1 X (i) z = h(x ) : (15) 2) In the update step, we use information obtained in form N ek+1 i=1 of measurements to correct the prediction xek+1 Time subscripts have been omitted for simplicity. This means, zk = Hxk + vk with vk ∼ Nm(0; R) ; (2) we can now compute the Kalman gain as in [8] x^k+1 = xek+1 + Kk+1(zk+1 − Hxek+1) ; (3) K = ΣHT (HΣHT + R)−1 where zk denotes the m-dimensional measurement, H −1 the measurement model, vk the measurement noise, and = (XXT )HT H(XXT )H + R K the Kalman gain. We assume v to be normally k+1 k −1 (16) distributed with zero mean and covariance matrix R. The = X(HX)T HX(HX)T + R Kalman gain is computed via −1 T T T T −1 = XZ ZZ + R : Kk = ΣkH (HΣkH + R) : (4) The general idea of the EnKF is to propagate a set of N The distinct advantage here is that we do not need to (1) (N) n ensemble members of the ensemble X = fxk ; : : : ; xk g compute a matrix of size n × n: X is of size n × N instead of the state x^k and the covariance Σk, such that the and, as we use range-bearing-measurements, Z is of size mean of the ensemble members xk approximates the true 2 × M, with M the number of measurements. As N n, state x^k and the sample covariance Σk approximates the true this reduces the complexity of the Kalman gain computation covariance matrix Σk immensely and sets the EnKF with O(NMn) between the 2 N EKF with O(n M) and FastSLAM with O(N log n), where 1 X (i) x = x ≈ x^ ; (5) the advantage of FastSLAM stems from the special tree k N k k i=1 structure for the landmarks. However, EKF and EnKF pro- N cess all measurements at once, while FastSLAM updates the 1 X (i) (i) T Σk = (x − xk)(x − xk) ≈ Σk : (6) measurements sequentially. Therefore, for M measurements N − 1 k k i=1 FastSLAM requires O(NM log n), which is still a bit faster Therefore, in the prediction-step of the Kalman filter we than the EnKF approach. (1) (N) can simply propagate each ensemble member xk ; : : : ; xk through the process model and in the update step we update III. ENKF-SLAM each ensemble member with a simulated measurement A. Problem Formulation (i) (i) i xek+1 = Fxk + wk 8i 2 f1;:::;Ng ; (7) SLAM describes the problem of simultaneously creating a (i) i map of the environment while localizing the robot in this map. z = zk+1 + v ; (8) ek+1 k+1 To be able to achieve this at the same time, the state that we (i) (i) (i) x^k+1 = xek+1 + Kk+1(zek+1 − Hxek+1) ; (9) are estimating with the EnKF must represent the state of the where an approximation of the Kalman gain is computed using robot as well as the entire map the sample covariance Σk R T T −1 xk = : Kk+1 = ΣkH (HΣkH + R) : (10) M When dealing with non-linear relationships, we use a non- Let the state of the robot R be represented by its position in the linear state transition function f to predict the state of each rx 2 Euclidean plane 2 R and its heading rh 2 [0; 2π]. Let ensemble member and we use a non-linear measurement func- ry tion h to transform each ensemble member into measurement the map M consist of the positions of the observed landmarks L space L = (j);x 2 2; j = 1;:::; jlandmarksj. (j) L R (i) (i) i (j);y xek+1 = f(xk ) + wk 8i 2 f1;:::;Ng ; (11) The idea to tackle the SLAM problem then is to append (i) (i) (i) each newly observed landmark to the current state estimate, x^ = x + Kk+1(zk+1 − h(x )) : (12) k+1 ek+1 e ek+1 producing a large state vector. However, as we want to circumvent the computation of a n×n When estimating this state using an EKF, in every update covariance matrix or even its approximation in (10), we instead step we can use the measurement zk to correct the prediction use the sample deviation X and the measurement deviation Z of the robot state R and of the according landmark position 1 h i X = x(1) − x; : : : ; x(N) − x ; (13a) L(j). N − 1 ek+1 ek+1 However, any solution using the EKF quickly becomes 1 h i Z = z(1) − z; : : : ; z(N) − z ; (13b) intractable due to the curse of dimensionality, which is why N − 1 k+1 k+1 state-of-the-art approaches such as FastSLAM use multiple • The control input for the state transition function f consists of the velocity v and the turning angle α relative Measurement to the robots heading.